Cleaning code + add missing const references

apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp

@@ -10,7 +10,7 @@
 
 #include <export_laz.h>
 
-bool load_pc(PointCloud& pc, std::string input_file_name)
+bool load_pc(PointCloud& pc, const std::string& input_file_name)
 {
     laszip_POINTER laszip_reader;
     if (laszip_create(&laszip_reader))

apps/lidar_odometry_step_1/lidar_odometry.cpp

@@ -453,13 +453,8 @@ void calculate_trajectory(
             counter++;
             if (counter % 100 == 0)
             {
-                printf(
-                    "Roll %0.1f, Pitch %0.1f, Yaw %0.1f [%d of %d]\n",
-                    euler.angle.roll,
-                    euler.angle.pitch,
-                    euler.angle.yaw,
-                    counter++,
-                    (int)imu_data.size());
+                std::cout << "Roll " << euler.angle.roll << ", Pitch " << euler.angle.pitch << ", Yaw " << euler.angle.yaw << " ["
+                          << counter++ << " of " << imu_data.size() << "]" << std::endl;
             }
         }
 
@@ -731,7 +726,7 @@ void save_result(std::vector<WorkerData>& worker_data, LidarOdometryParams& para
 
         if (i % 1000 == 0)
         {
-            printf("processing worker_data %d/%d \n", i + 1, (int)worker_data.size());
+            std::cout << "processing worker_data " << (i + 1) << "/" << static_cast<int>(worker_data.size()) << " \n";
         }
         auto tmp_data = original_points;
         point_sizes_per_chunk.push_back(tmp_data.size());
@@ -1030,7 +1025,7 @@ void save_trajectory_to_ascii(std::vector<WorkerData>& worker_data, std::string
     file.close();
 }
 
-void load_reference_point_clouds(std::vector<std::string> input_file_names, LidarOdometryParams& params)
+void load_reference_point_clouds(const std::vector<std::string>& input_file_names, LidarOdometryParams& params)
 {
     params.reference_buckets.clear();
     params.reference_points.clear();
@@ -1055,7 +1050,7 @@ std::string save_results_automatic(
     return outwd.string();
 }
 
-std::vector<WorkerData> run_lidar_odometry(std::string input_dir, LidarOdometryParams& params)
+std::vector<WorkerData> run_lidar_odometry(const std::string& input_dir, LidarOdometryParams& params)
 {
     Session session;
     std::vector<WorkerData> worker_data;

apps/lidar_odometry_step_1/lidar_odometry.h

@@ -39,13 +39,13 @@ bool compute_step_1(
     const std::atomic<bool>& pause);
 void run_consistency(std::vector<WorkerData>& worker_data, LidarOdometryParams& params);
 void filter_reference_buckets(LidarOdometryParams& params);
-void load_reference_point_clouds(std::vector<std::string> input_file_names, LidarOdometryParams& params);
+void load_reference_point_clouds(const std::vector<std::string>& input_file_names, LidarOdometryParams& params);
 void save_result(std::vector<WorkerData>& worker_data, LidarOdometryParams& params, fs::path outwd, double elapsed_seconds);
 void save_parameters_toml(const LidarOdometryParams& params, const fs::path& outwd, double elapsed_seconds);
 void save_processing_results_json(const LidarOdometryParams& params, const fs::path& outwd, double elapsed_seconds);
 void save_trajectory_to_ascii(std::vector<WorkerData>& worker_data, std::string output_file_name);
 std::string save_results_automatic(
     LidarOdometryParams& params, std::vector<WorkerData>& worker_data, std::string working_directory, double elapsed_seconds);
-std::vector<WorkerData> run_lidar_odometry(std::string input_dir, LidarOdometryParams& params);
+std::vector<WorkerData> run_lidar_odometry(const std::string& input_dir, LidarOdometryParams& params);
 // bool SaveParametersToTomlFile(const std::string &filepath, const LidarOdometryParams &params);
 // bool LoadParametersFromTomlFile(const std::string &filepath, LidarOdometryParams &params);

apps/lidar_odometry_step_1/lidar_odometry_gui.cpp

@@ -271,7 +271,8 @@ std::string formatCompletionTime(double remainingSeconds)
     return std::string(timeStr);
 }
 
-std::vector<std::vector<Point3Di>> get_batches_of_points(std::string laz_file, int point_count_threshold, std::vector<Point3Di> prev_points)
+#if 0
+std::vector<std::vector<Point3Di>> get_batches_of_points(std::string laz_file, int point_count_threshold, conststd::vector<Point3Di>& prev_points)
 {
     std::vector<std::vector<Point3Di>> res_points;
     std::vector<Point3Di> points = load_point_cloud(laz_file, false, 0, 10000, {});
@@ -296,6 +297,7 @@ std::vector<std::vector<Point3Di>> get_batches_of_points(std::string laz_file, i
     }
     return res_points;
 }
+#endif
 
 int get_index(set<int> s, int k)
 {
@@ -528,7 +530,7 @@ void alternative_approach()
                 counter++;
                 if (counter % 100 == 0)
                 {
-                    printf("Roll %0.1f, Pitch %0.1f, Yaw %0.1f [%d of %d]\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw, counter++, imu_data.size());
+                    std::cout << << "Roll " << euler.angle.roll<< ", Pitch " << euler.angle.pitch<< ", Yaw " << euler.angle.yaw<< " [" << counter++ << " of " << imu_data.size() << "]"<< std::endl;
                 }
             }
 

apps/lidar_odometry_step_1/lidar_odometry_utils.cpp

@@ -315,7 +315,7 @@ void update_rgd_spherical_coordinates(
     }
 }
 
-bool save_poses(const std::string file_name, std::vector<Eigen::Affine3d> m_poses, std::vector<std::string> filenames)
+bool save_poses(const std::string file_name, const std::vector<Eigen::Affine3d>& m_poses, const std::vector<std::string>& filenames)
 {
     std::ofstream outfile;
     outfile.open(file_name);
@@ -821,8 +821,8 @@ fs::path get_next_result_path(const std::string working_directory)
 bool loadLaz(
     const std::string& filename,
     std::vector<Point3Di>& points_out,
-    std::vector<int> index_poses_i,
-    std::vector<Eigen::Affine3d>& intermediate_trajectory,
+    const std::vector<int>& index_poses_i,
+    const std::vector<Eigen::Affine3d>& intermediate_trajectory,
     const Eigen::Affine3d& m_pose)
 {
     if (!std::filesystem::exists(filename))

apps/lidar_odometry_step_1/lidar_odometry_utils.h

@@ -199,7 +199,7 @@ std::vector<Point3Di> load_point_cloud(
     double filter_threshold_xy_outer,
     const std::unordered_map<int, Eigen::Affine3d>& calibrations);
 
-bool save_poses(const std::string file_name, std::vector<Eigen::Affine3d> m_poses, std::vector<std::string> filenames);
+bool save_poses(const std::string file_name, const std::vector<Eigen::Affine3d>& m_poses, const std::vector<std::string>& filenames);
 
 fs::path get_next_result_path(const std::string working_directory);
 
@@ -261,7 +261,7 @@ void optimize_icp(
     std::vector<Eigen::Affine3d>& intermediate_trajectory,
     std::vector<Eigen::Affine3d>& intermediate_trajectory_motion_model,
     NDT::GridParameters& rgd_params,
-    /*NDTBucketMapType &buckets*/ std::vector<Point3Di> points_global,
+    /*NDTBucketMapType &buckets*/ const std::vector<Point3Di>& points_global,
     bool useMultithread /*,
 bool add_pitch_roll_constraint, const std::vector<std::pair<double, double>> &imu_roll_pitch*/
 );
@@ -286,8 +286,8 @@ void compute_step_2_fast_forward_motion(std::vector<WorkerData>& worker_data, Li
 bool loadLaz(
     const std::string& filename,
     std::vector<Point3Di>& points_out,
-    std::vector<int> index_poses_i,
-    std::vector<Eigen::Affine3d>& intermediate_trajectory,
+    const std::vector<int>& index_poses_i,
+    const std::vector<Eigen::Affine3d>& intermediate_trajectory,
     const Eigen::Affine3d& inverse_pose);
 bool load_poses(const fs::path& poses_file, std::vector<Eigen::Affine3d>& out_poses);
 bool load_trajectory_csv(

apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp

@@ -29,7 +29,7 @@ namespace
         }
     };
 } // namespace
-std::vector<std::pair<int, int>> nns(std::vector<Point3Di> points_global, const std::vector<int>& indexes_for_nn)
+std::vector<std::pair<int, int>> nns(const std::vector<Point3Di>& points_global, const std::vector<int>& indexes_for_nn)
 {
     Eigen::Vector3d search_radious = { 0.1, 0.1, 0.1 };
 
@@ -127,7 +127,7 @@ void optimize_icp(
     std::vector<Eigen::Affine3d>& intermediate_trajectory,
     std::vector<Eigen::Affine3d>& intermediate_trajectory_motion_model,
     NDT::GridParameters& rgd_params,
-    /*NDTBucketMapType &buckets*/ std::vector<Point3Di> points_global,
+    /*NDTBucketMapType &buckets*/ const std::vector<Point3Di>& points_global,
     bool useMultithread /*,
 bool add_pitch_roll_constraint, const std::vector<std::pair<double, double>> &imu_roll_pitch*/
 )

apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp

@@ -1043,13 +1043,10 @@ void loadFiles(std::vector<std::string> input_file_names)
             const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
             counter++;
             if (counter % 100 == 0)
-                printf(
-                    "Roll %0.1f, Pitch %0.1f, Yaw %0.1f [%d of %zu]\n",
-                    euler.angle.roll,
-                    euler.angle.pitch,
-                    euler.angle.yaw,
-                    counter++,
-                    imu_data.size());
+            {
+                std::cout << "Roll " << euler.angle.roll << ", Pitch " << euler.angle.pitch << ", Yaw " << euler.angle.yaw << " ["
+                          << counter++ << " of " << imu_data.size() << "]" << std::endl;
+            }
 
             // log it for implot
             imu_data_plot.timestampLidar.push_back(timestamp_pair.first);

apps/manual_color/manual_color.cpp

@@ -160,7 +160,8 @@ int main(int argc, char* argv[])
     glutMainLoop();
 }
 
-void imagePicker(const std::string& name, ImTextureID tex1, std::vector<ImVec2>& point_picked, std::vector<ImVec2> point_pickedInPointcloud)
+void imagePicker(
+    const std::string& name, ImTextureID tex1, std::vector<ImVec2>& point_picked, const std::vector<ImVec2>& point_pickedInPointcloud)
 {
     ImGuiIO& io = ImGui::GetIO();
     static float zoom = 0.1f;

apps/quick_start_demo/quick_start_demo.cpp

@@ -945,7 +945,7 @@ int main(int argc, char *argv[])
             counter++;
             if (counter % 100 == 0)
             {
-                printf("Roll %0.1f, Pitch %0.1f, Yaw %0.1f [%d of %d]\n", euler.angle.roll, euler.angle.pitch, euler.angle.yaw, counter++, imu_data.size());
+                std::cout << "Roll " << euler.angle.roll<< ", Pitch " << euler.angle.pitch<< ", Yaw " << euler.angle.yaw<< " [" << counter++ << " of " << imu_data.size() << "]"<< std::endl;
             }
         }
 

core/include/export_laz.h

@@ -25,38 +25,14 @@ inline bool exportLaz(
     int number_of_points_with_timestamp_eq_0 = 0;
 
     constexpr float scale = 0.0001f; // one tenth of milimeter
-    // find max
-    Eigen::Vector3d _max(
-        std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest());
-    Eigen::Vector3d _min(std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
 
-    for (auto& p : pointcloud)
-    {
-        if (p.x() < _min.x())
-        {
-            _min.x() = p.x();
-        }
-        if (p.y() < _min.y())
-        {
-            _min.y() = p.y();
-        }
-        if (p.z() < _min.z())
-        {
-            _min.z() = p.z();
-        }
+    Eigen::Vector3d _min = Eigen::Vector3d::Constant(std::numeric_limits<double>::max());
+    Eigen::Vector3d _max = Eigen::Vector3d::Constant(std::numeric_limits<double>::lowest());
 
-        if (p.x() > _max.x())
-        {
-            _max.x() = p.x();
-        }
-        if (p.y() > _max.y())
-        {
-            _max.y() = p.y();
-        }
-        if (p.z() > _max.z())
-        {
-            _max.z() = p.z();
-        }
+    for (const auto& p : pointcloud)
+    {
+        _min = _min.cwiseMin(p);
+        _max = _max.cwiseMax(p);
     }
 
     // create the writer
@@ -136,15 +112,8 @@ inline bool exportLaz(
         point->intensity = intensity[i];
         point->gps_time = timestamps[i] * 1e9;
 
-        if (point->gps_time < min_ts)
-        {
-            min_ts = point->gps_time;
-        }
-
-        if (point->gps_time > max_ts)
-        {
-            max_ts = point->gps_time;
-        }
+        min_ts = std::min(min_ts, point->gps_time);
+        max_ts = std::max(max_ts, point->gps_time);
 
         if (point->gps_time == 0.0)
         {

core/include/observation_picking.h

@@ -140,7 +140,7 @@ class ObservationPicking
     bool grid001x001m = false;
     void render();
     void add_picked_to_current_observation(int index_picked, Eigen::Vector3d p);
-    void accept_current_observation(std::vector<Eigen::Affine3d> m_poses);
+    void accept_current_observation(const std::vector<Eigen::Affine3d>& m_poses);
     void import_observations(const std::string& filename);
     void export_observation(const std::string& filename);
     void add_intersection(Eigen::Vector3d translation);

core/include/point_clouds.h

@@ -91,7 +91,7 @@ class PointClouds
 
     bool load_pose_ETH(const std::string& fn, Eigen::Affine3d& m_increment);
     bool load_whu_tls(
-        std::vector<std::string> input_file_names,
+        const std::vector<std::string>& input_file_names,
         bool is_decimate,
         double bucket_x,
         double bucket_y,

core/src/observation_picking.cpp

@@ -144,7 +144,7 @@ void ObservationPicking::add_picked_to_current_observation(int index_picked, Eig
     }
 }
 
-void ObservationPicking::accept_current_observation(std::vector<Eigen::Affine3d> m_poses)
+void ObservationPicking::accept_current_observation(const std::vector<Eigen::Affine3d>& m_poses)
 {
     for (auto& [key, value] : current_observation)
     {

core/src/point_clouds.cpp

@@ -1221,7 +1221,7 @@ bool PointClouds::load_pc(PointCloud& pc, std::string input_file_name, bool load
 }
 
 bool PointClouds::load_whu_tls(
-    std::vector<std::string> input_file_names,
+    const std::vector<std::string>& input_file_names,
     bool is_decimate,
     double bucket_x,
     double bucket_y,

core_hd_mapping/src/odo_with_gnss_fusion.cpp

@@ -274,7 +274,7 @@ std::vector<Node> OdoWithGnssFusion::load_trajectory(const std::string& file_nam
     }
     else
     {
-        printf("Can't read trajectory from file %s\n", file_name.c_str());
+        std::cout << "Can't read trajectory from file " << file_name << std::endl;
         fflush(stdout);
         return trajectory;
     }