diff --git a/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp b/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp index c01219eb..3880cc48 100644 --- a/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp +++ b/apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp @@ -10,7 +10,7 @@ #include -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)) diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index 66c58b22..e22aa9c7 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/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& 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(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& worker_data, std::string file.close(); } -void load_reference_point_clouds(std::vector input_file_names, LidarOdometryParams& params) +void load_reference_point_clouds(const std::vector& 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 run_lidar_odometry(std::string input_dir, LidarOdometryParams& params) +std::vector run_lidar_odometry(const std::string& input_dir, LidarOdometryParams& params) { Session session; std::vector worker_data; diff --git a/apps/lidar_odometry_step_1/lidar_odometry.h b/apps/lidar_odometry_step_1/lidar_odometry.h index dac31857..467da8f2 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.h +++ b/apps/lidar_odometry_step_1/lidar_odometry.h @@ -39,13 +39,13 @@ bool compute_step_1( const std::atomic& pause); void run_consistency(std::vector& worker_data, LidarOdometryParams& params); void filter_reference_buckets(LidarOdometryParams& params); -void load_reference_point_clouds(std::vector input_file_names, LidarOdometryParams& params); +void load_reference_point_clouds(const std::vector& input_file_names, LidarOdometryParams& params); void save_result(std::vector& 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& worker_data, std::string output_file_name); std::string save_results_automatic( LidarOdometryParams& params, std::vector& worker_data, std::string working_directory, double elapsed_seconds); -std::vector run_lidar_odometry(std::string input_dir, LidarOdometryParams& params); +std::vector run_lidar_odometry(const std::string& input_dir, LidarOdometryParams& params); // bool SaveParametersToTomlFile(const std::string &filepath, const LidarOdometryParams ¶ms); // bool LoadParametersFromTomlFile(const std::string &filepath, LidarOdometryParams ¶ms); diff --git a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp index 141e4235..f449fe6e 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -271,7 +271,8 @@ std::string formatCompletionTime(double remainingSeconds) return std::string(timeStr); } -std::vector> get_batches_of_points(std::string laz_file, int point_count_threshold, std::vector prev_points) +#if 0 +std::vector> get_batches_of_points(std::string laz_file, int point_count_threshold, conststd::vector& prev_points) { std::vector> res_points; std::vector points = load_point_cloud(laz_file, false, 0, 10000, {}); @@ -296,6 +297,7 @@ std::vector> get_batches_of_points(std::string laz_file, i } return res_points; } +#endif int get_index(set 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; } } diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index f5ed58c5..a2fed481 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/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 m_poses, std::vector filenames) +bool save_poses(const std::string file_name, const std::vector& m_poses, const std::vector& 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& points_out, - std::vector index_poses_i, - std::vector& intermediate_trajectory, + const std::vector& index_poses_i, + const std::vector& intermediate_trajectory, const Eigen::Affine3d& m_pose) { if (!std::filesystem::exists(filename)) diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index e953f155..60b53289 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -199,7 +199,7 @@ std::vector load_point_cloud( double filter_threshold_xy_outer, const std::unordered_map& calibrations); -bool save_poses(const std::string file_name, std::vector m_poses, std::vector filenames); +bool save_poses(const std::string file_name, const std::vector& m_poses, const std::vector& filenames); fs::path get_next_result_path(const std::string working_directory); @@ -261,7 +261,7 @@ void optimize_icp( std::vector& intermediate_trajectory, std::vector& intermediate_trajectory_motion_model, NDT::GridParameters& rgd_params, - /*NDTBucketMapType &buckets*/ std::vector points_global, + /*NDTBucketMapType &buckets*/ const std::vector& points_global, bool useMultithread /*, bool add_pitch_roll_constraint, const std::vector> &imu_roll_pitch*/ ); @@ -286,8 +286,8 @@ void compute_step_2_fast_forward_motion(std::vector& worker_data, Li bool loadLaz( const std::string& filename, std::vector& points_out, - std::vector index_poses_i, - std::vector& intermediate_trajectory, + const std::vector& index_poses_i, + const std::vector& intermediate_trajectory, const Eigen::Affine3d& inverse_pose); bool load_poses(const fs::path& poses_file, std::vector& out_poses); bool load_trajectory_csv( diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp index 1244017f..d71e09b2 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -29,7 +29,7 @@ namespace } }; } // namespace -std::vector> nns(std::vector points_global, const std::vector& indexes_for_nn) +std::vector> nns(const std::vector& points_global, const std::vector& indexes_for_nn) { Eigen::Vector3d search_radious = { 0.1, 0.1, 0.1 }; @@ -127,7 +127,7 @@ void optimize_icp( std::vector& intermediate_trajectory, std::vector& intermediate_trajectory_motion_model, NDT::GridParameters& rgd_params, - /*NDTBucketMapType &buckets*/ std::vector points_global, + /*NDTBucketMapType &buckets*/ const std::vector& points_global, bool useMultithread /*, bool add_pitch_roll_constraint, const std::vector> &imu_roll_pitch*/ ) diff --git a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp index fb386485..239a13c4 100644 --- a/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp +++ b/apps/mandeye_raw_data_viewer/mandeye_raw_data_viewer.cpp @@ -1043,13 +1043,10 @@ void loadFiles(std::vector 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); diff --git a/apps/manual_color/manual_color.cpp b/apps/manual_color/manual_color.cpp index d0020011..84caae6d 100644 --- a/apps/manual_color/manual_color.cpp +++ b/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& point_picked, std::vector point_pickedInPointcloud) +void imagePicker( + const std::string& name, ImTextureID tex1, std::vector& point_picked, const std::vector& point_pickedInPointcloud) { ImGuiIO& io = ImGui::GetIO(); static float zoom = 0.1f; diff --git a/apps/quick_start_demo/quick_start_demo.cpp b/apps/quick_start_demo/quick_start_demo.cpp index 175bf6e1..2c99c88b 100644 --- a/apps/quick_start_demo/quick_start_demo.cpp +++ b/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; } } diff --git a/core/include/export_laz.h b/core/include/export_laz.h index aacaecef..c433b788 100644 --- a/core/include/export_laz.h +++ b/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::lowest(), std::numeric_limits::lowest(), std::numeric_limits::lowest()); - Eigen::Vector3d _min(std::numeric_limits::max(), std::numeric_limits::max(), std::numeric_limits::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::max()); + Eigen::Vector3d _max = Eigen::Vector3d::Constant(std::numeric_limits::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) { diff --git a/core/include/observation_picking.h b/core/include/observation_picking.h index 3608a11a..fe44fa9a 100644 --- a/core/include/observation_picking.h +++ b/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 m_poses); + void accept_current_observation(const std::vector& m_poses); void import_observations(const std::string& filename); void export_observation(const std::string& filename); void add_intersection(Eigen::Vector3d translation); diff --git a/core/include/point_clouds.h b/core/include/point_clouds.h index d761bcb0..aa94c2b5 100644 --- a/core/include/point_clouds.h +++ b/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 input_file_names, + const std::vector& input_file_names, bool is_decimate, double bucket_x, double bucket_y, diff --git a/core/src/observation_picking.cpp b/core/src/observation_picking.cpp index 86b539b1..e4034f5f 100644 --- a/core/src/observation_picking.cpp +++ b/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 m_poses) +void ObservationPicking::accept_current_observation(const std::vector& m_poses) { for (auto& [key, value] : current_observation) { diff --git a/core/src/point_clouds.cpp b/core/src/point_clouds.cpp index 1f3cfe4e..d2c5f438 100644 --- a/core/src/point_clouds.cpp +++ b/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 input_file_names, + const std::vector& input_file_names, bool is_decimate, double bucket_x, double bucket_y, diff --git a/core_hd_mapping/src/odo_with_gnss_fusion.cpp b/core_hd_mapping/src/odo_with_gnss_fusion.cpp index a49eba46..123c5579 100644 --- a/core_hd_mapping/src/odo_with_gnss_fusion.cpp +++ b/core_hd_mapping/src/odo_with_gnss_fusion.cpp @@ -274,7 +274,7 @@ std::vector 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; }