diff --git a/apps/compare_trajectories/mandeye_compare_trajectories.cpp b/apps/compare_trajectories/mandeye_compare_trajectories.cpp index c52cd48c..81c2a4b6 100644 --- a/apps/compare_trajectories/mandeye_compare_trajectories.cpp +++ b/apps/compare_trajectories/mandeye_compare_trajectories.cpp @@ -26,8 +26,8 @@ namespace fs = std::filesystem; -const unsigned int window_width = 800; -const unsigned int window_height = 600; +const uint32_t window_width = 800; +const uint32_t window_height = 600; double camera_ortho_xy_view_zoom = 10; double camera_ortho_xy_view_shift_x = 0.0; double camera_ortho_xy_view_shift_y = 0.0; diff --git a/apps/hd_mapper/hd_mapper.cpp b/apps/hd_mapper/hd_mapper.cpp index b85938e8..1b082e97 100644 --- a/apps/hd_mapper/hd_mapper.cpp +++ b/apps/hd_mapper/hd_mapper.cpp @@ -26,8 +26,8 @@ Eigen::Vector3d GLWidgetGetOGLPos(int x, int y, float picking_plane_height); static bool show_demo_window = true; static bool show_another_window = false; // static ImVec4 clear_color = ImVec4(0.45f, 0.55f, 0.60f, 1.00f); -const unsigned int window_width = 800; -const unsigned int window_height = 600; +const uint32_t window_width = 800; +const uint32_t window_height = 600; int mouse_old_x, mouse_old_y; int mouse_buttons = 0; bool gui_mouse_down{ false }; diff --git a/apps/lidar_odometry_step_1/lidar_odometry.cpp b/apps/lidar_odometry_step_1/lidar_odometry.cpp index e22aa9c7..2fd5fe9d 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry.cpp @@ -463,7 +463,7 @@ void calculate_trajectory( } bool compute_step_1( - std::vector>& pointsPerFile, + const std::vector>& pointsPerFile, LidarOdometryParams& params, Trajectory& trajectory, std::vector& worker_data, @@ -681,7 +681,7 @@ bool compute_step_1( return true; } -void run_consistency(std::vector& worker_data, LidarOdometryParams& params) +void run_consistency(std::vector& worker_data, const LidarOdometryParams& params) { std::cout << "Point cloud consistency and trajectory smoothness START" << std::endl; for (int i = 0; i < params.num_constistency_iter; i++) @@ -1043,7 +1043,7 @@ void load_reference_point_clouds(const std::vector& input_file_name } std::string save_results_automatic( - LidarOdometryParams& params, std::vector& worker_data, std::string working_directory, double elapsed_seconds) + LidarOdometryParams& params, std::vector& worker_data, const std::string& working_directory, double elapsed_seconds) { fs::path outwd = get_next_result_path(working_directory); save_result(worker_data, params, outwd, elapsed_seconds); @@ -1186,7 +1186,7 @@ void save_processing_results_json(const LidarOdometryParams& params, const fs::p results["ndt_grid_indoor"]["resolution_X"] = params.in_out_params_indoor.resolution_X; results["ndt_grid_indoor"]["resolution_Y"] = params.in_out_params_indoor.resolution_Y; results["ndt_grid_indoor"]["resolution_Z"] = params.in_out_params_indoor.resolution_Z; - results["ndt_grid_indoor"]["number_of_buckets"] = static_cast(params.in_out_params_indoor.number_of_buckets); + results["ndt_grid_indoor"]["number_of_buckets"] = static_cast(params.in_out_params_indoor.number_of_buckets); results["ndt_grid_outdoor"]["bounding_box_min_X"] = params.in_out_params_outdoor.bounding_box_min_X; results["ndt_grid_outdoor"]["bounding_box_min_Y"] = params.in_out_params_outdoor.bounding_box_min_Y; @@ -1197,7 +1197,7 @@ void save_processing_results_json(const LidarOdometryParams& params, const fs::p results["ndt_grid_outdoor"]["resolution_X"] = params.in_out_params_outdoor.resolution_X; results["ndt_grid_outdoor"]["resolution_Y"] = params.in_out_params_outdoor.resolution_Y; results["ndt_grid_outdoor"]["resolution_Z"] = params.in_out_params_outdoor.resolution_Z; - results["ndt_grid_outdoor"]["number_of_buckets"] = static_cast(params.in_out_params_outdoor.number_of_buckets); + results["ndt_grid_outdoor"]["number_of_buckets"] = static_cast(params.in_out_params_outdoor.number_of_buckets); // Motion model correction (complex structure) results["motion_model_correction"]["om"] = params.motion_model_correction.om; diff --git a/apps/lidar_odometry_step_1/lidar_odometry.h b/apps/lidar_odometry_step_1/lidar_odometry.h index 467da8f2..0e828cac 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry.h +++ b/apps/lidar_odometry_step_1/lidar_odometry.h @@ -32,12 +32,12 @@ void calculate_trajectory( bool debugMsg, bool use_removie_imu_bias_from_first_stationary_scan); bool compute_step_1( - std::vector>& pointsPerFile, + const std::vector>& pointsPerFile, LidarOdometryParams& params, Trajectory& trajectory, std::vector& worker_data, const std::atomic& pause); -void run_consistency(std::vector& worker_data, LidarOdometryParams& params); +void run_consistency(std::vector& worker_data, const LidarOdometryParams& params); void filter_reference_buckets(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); @@ -45,7 +45,7 @@ void save_parameters_toml(const LidarOdometryParams& params, const fs::path& out 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); + LidarOdometryParams& params, std::vector& worker_data, const std::string& working_directory, double elapsed_seconds); 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 f449fe6e..4511c31d 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_gui.cpp @@ -254,7 +254,7 @@ std::string formatCompletionTime(double remainingSeconds) return "Calculating..."; auto now = std::chrono::system_clock::now(); - auto estimatedCompletion = now + std::chrono::seconds(static_cast(remainingSeconds)); + auto estimatedCompletion = now + std::chrono::seconds(static_cast(remainingSeconds)); auto completion_time_t = std::chrono::system_clock::to_time_t(estimatedCompletion); // Format as HH:MM:SS - Cross-platform time formatting @@ -299,7 +299,7 @@ std::vector> get_batches_of_points(std::string laz_file, i } #endif -int get_index(set s, int k) +int get_index(const set& s, int k) { int index = 0; for (auto u : s) @@ -349,7 +349,7 @@ void find_best_stretch( } /// std::vector best_trajectory = trajectory; - unsigned long long min_buckets = ULLONG_MAX; + uint64_t min_buckets = ULLONG_MAX; for (double x = 0.0; x < 0.2; x += 0.0005) { diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp index a2fed481..c67c1900 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp @@ -11,15 +11,14 @@ namespace fs = std::filesystem; // TODO(m.wlasiuk) : these 2 functions - core/utils // this function provides unique index -unsigned long long int get_index(const int16_t x, const int16_t y, const int16_t z) +uint64_t get_index(const int16_t x, const int16_t y, const int16_t z) { - return ((static_cast(x) << 32) & (0x0000FFFF00000000ull)) | - ((static_cast(y) << 16) & (0x00000000FFFF0000ull)) | - ((static_cast(z) << 0) & (0x000000000000FFFFull)); + return ((static_cast(x) << 32) & (0x0000FFFF00000000ull)) | ((static_cast(y) << 16) & (0x00000000FFFF0000ull)) | + ((static_cast(z) << 0) & (0x000000000000FFFFull)); } // this function provides unique index for input point p and 3D space decomposition into buckets b -unsigned long long int get_rgd_index(const Eigen::Vector3d p, const Eigen::Vector3d b) +uint64_t get_rgd_index(const Eigen::Vector3d p, const Eigen::Vector3d b) { int16_t x = static_cast(p.x() / b.x()); int16_t y = static_cast(p.y() / b.y()); @@ -153,7 +152,11 @@ void limit_covariance(Eigen::Matrix3d& io_cov) // std::cout << io_cov << std::endl; } -void update_rgd(NDT::GridParameters& rgd_params, NDTBucketMapType& buckets, std::vector& points_global, Eigen::Vector3d viewport) +void update_rgd( + const NDT::GridParameters& rgd_params, + NDTBucketMapType& buckets, + const std::vector& points_global, + const Eigen::Vector3d& viewport) { Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); @@ -233,10 +236,10 @@ void update_rgd(NDT::GridParameters& rgd_params, NDTBucketMapType& buckets, std: } void update_rgd_spherical_coordinates( - NDT::GridParameters& rgd_params, + const NDT::GridParameters& rgd_params, NDTBucketMapType& buckets, - std::vector& points_global, - std::vector& points_global_spherical) + const std::vector& points_global, + const std::vector& points_global_spherical) { Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z); @@ -315,7 +318,7 @@ void update_rgd_spherical_coordinates( } } -bool save_poses(const std::string file_name, const std::vector& m_poses, const 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); @@ -797,7 +800,7 @@ int MLvxCalib::GetImuIdToUse(const std::unordered_map& idToSn, return 0; } -fs::path get_next_result_path(const std::string working_directory) +fs::path get_next_result_path(const std::string& working_directory) { std::regex pattern(R"(lio_result_(\d+))"); int max_number = -1; diff --git a/apps/lidar_odometry_step_1/lidar_odometry_utils.h b/apps/lidar_odometry_step_1/lidar_odometry_utils.h index 60b53289..0729ae87 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils.h +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils.h @@ -151,8 +151,8 @@ struct LidarOdometryParams bool save_index_pose = false; }; -unsigned long long int get_index(const int16_t x, const int16_t y, const int16_t z); -unsigned long long int get_rgd_index(const Eigen::Vector3d p, const Eigen::Vector3d b); +uint64_t get_index(const int16_t x, const int16_t y, const int16_t z); +uint64_t get_rgd_index(const Eigen::Vector3d p, const Eigen::Vector3d b); // this function finds interpolated pose between two poses according to query_time Eigen::Matrix4d getInterpolatedPose(const std::map& trajectory, double query_time); @@ -162,16 +162,16 @@ std::vector decimate(const std::vector& points, double bucke // this function updates each bucket (mean value, covariance) in regular grid decomposition void update_rgd( - NDT::GridParameters& rgd_params, + const NDT::GridParameters& rgd_params, NDTBucketMapType& buckets, - std::vector& points_global, - Eigen::Vector3d viewport = Eigen::Vector3d(0, 0, 0)); + const std::vector& points_global, + const Eigen::Vector3d& viewport = Eigen::Vector3d(0, 0, 0)); void update_rgd_spherical_coordinates( - NDT::GridParameters& rgd_params, + const NDT::GridParameters& rgd_params, NDTBucketMapType& buckets, - std::vector& points_global, - std::vector& points_global_spherical); + const std::vector& points_global, + const std::vector& points_global_spherical); //! This function load inertial measurement unit data. //! This function expects a file with the following format: @@ -199,9 +199,9 @@ std::vector load_point_cloud( double filter_threshold_xy_outer, const std::unordered_map& calibrations); -bool save_poses(const std::string file_name, const std::vector& m_poses, const 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); +fs::path get_next_result_path(const std::string& working_directory); // this function performs main LiDAR odometry calculations void optimize_lidar_odometry( @@ -376,5 +376,5 @@ namespace MLvxCalib int GetImuIdToUse(const std::unordered_map& idToSn, const std::string& snToUse); } // namespace MLvxCalib -void Consistency(std::vector& worker_data, LidarOdometryParams& params); -void Consistency2(std::vector& worker_data, LidarOdometryParams& params); +void Consistency(std::vector& worker_data, const LidarOdometryParams& params); +void Consistency2(std::vector& worker_data, const LidarOdometryParams& params); 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 d71e09b2..53d808e4 100644 --- a/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp +++ b/apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp @@ -35,27 +35,27 @@ std::vector> nns(const std::vector& points_global, std::vector> nn; - std::vector> indexes; + std::vector> indexes; for (int i = 0; i < points_global.size(); i++) { - unsigned long long int index = get_rgd_index(points_global[i].point, search_radious); + uint64_t index = get_rgd_index(points_global[i].point, search_radious); indexes.emplace_back(index, i); } std::sort( indexes.begin(), indexes.end(), - [](const std::pair& a, const std::pair& b) + [](const std::pair& a, const std::pair& b) { return a.first < b.first; }); - std::unordered_map> buckets; + std::unordered_map> buckets; - for (unsigned int i = 0; i < indexes.size(); i++) + for (uint32_t i = 0; i < indexes.size(); i++) { - unsigned long long int index_of_bucket = indexes[i].first; + uint64_t index_of_bucket = indexes[i].first; if (buckets.contains(index_of_bucket)) buckets[index_of_bucket].second = i; else @@ -85,7 +85,7 @@ std::vector> nns(const std::vector& points_global, for (double z = -search_radious.z(); z <= search_radious.z(); z += search_radious.z()) { Eigen::Vector3d position_global = source.point + Eigen::Vector3d(x, y, z); - unsigned long long int index_of_bucket = get_rgd_index(position_global, search_radious); + uint64_t index_of_bucket = get_rgd_index(position_global, search_radious); if (buckets.contains(index_of_bucket)) { @@ -2420,8 +2420,8 @@ bool compute_step_2( acc_distance = acc_distance_tmp; std::cout << "please split data set into subsets" << std::endl; ts_failure = worker_data[i].intermediate_trajectory_timestamps[0].first; - std::cout << "calculations canceled for TIMESTAMP: " - << (long long int)worker_data[i].intermediate_trajectory_timestamps[0].first << std::endl; + std::cout << "calculations canceled for TIMESTAMP: " << (int64_t)worker_data[i].intermediate_trajectory_timestamps[0].first + << std::endl; return false; } @@ -2549,7 +2549,7 @@ void compute_step_2_fast_forward_motion(std::vector& worker_data, Li { } -void Consistency(std::vector& worker_data, LidarOdometryParams& params) +void Consistency(std::vector& worker_data, const LidarOdometryParams& params) { std::vector trajectory; std::vector trajectory_motion_model; @@ -2916,7 +2916,7 @@ void Consistency(std::vector& worker_data, LidarOdometryParams& para worker_data[indexes[i].first].intermediate_trajectory[indexes[i].second] = trajectory[i]; } -void Consistency2(std::vector& worker_data, LidarOdometryParams& params) +void Consistency2(std::vector& worker_data, const LidarOdometryParams& params) { // global_tmp.clear(); @@ -3160,7 +3160,7 @@ void Consistency2(std::vector& worker_data, LidarOdometryParams& par // update_rgd2(params.in_out_params, buckets, points_global, trajectory[points_global[0].index_pose].translation()); - std::vector> index_pairs; + std::vector> index_pairs; for (int i = 0; i < points_global.size(); i++) { auto index_of_bucket = get_rgd_index(points_global[i].point, b); @@ -3169,16 +3169,16 @@ void Consistency2(std::vector& worker_data, LidarOdometryParams& par std::sort( index_pairs.begin(), index_pairs.end(), - [](const std::pair& a, const std::pair& b) + [](const std::pair& a, const std::pair& b) { return a.first < b.first; }); NDTBucketMapType2 buckets; - for (unsigned int i = 0; i < index_pairs.size(); i++) + for (uint32_t i = 0; i < index_pairs.size(); i++) { - unsigned long long int index_of_bucket = index_pairs[i].first; + uint64_t index_of_bucket = index_pairs[i].first; if (buckets.contains(index_of_bucket)) buckets[index_of_bucket].index_end_exclusive = i; else @@ -3192,7 +3192,7 @@ void Consistency2(std::vector& worker_data, LidarOdometryParams& par { for (int i = b.second.index_begin_inclusive; i < b.second.index_end_exclusive; i++) { - long long unsigned int index_point_segment = points_global[index_pairs[i].second].index_point; + uint64_t index_point_segment = points_global[index_pairs[i].second].index_point; if (b.second.buckets.contains(index_point_segment)) b.second.buckets[index_point_segment].point_indexes.push_back(index_pairs[i].second); diff --git a/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp index 16e258fa..3efe5797 100644 --- a/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp +++ b/apps/livox_mid_360_intrinsic_calibration/livox_mid_360_intrinsic_calibration.cpp @@ -33,8 +33,8 @@ #define SAMPLE_PERIOD (1.0 / 200.0) namespace fs = std::filesystem; -const unsigned int window_width = 800; -const unsigned int window_height = 600; +const uint32_t window_width = 800; +const uint32_t window_height = 600; double camera_ortho_xy_view_zoom = 10; double camera_ortho_xy_view_shift_x = 0.0; double camera_ortho_xy_view_shift_y = 0.0; diff --git a/apps/manual_color/manual_color.cpp b/apps/manual_color/manual_color.cpp index 84caae6d..067c76b3 100644 --- a/apps/manual_color/manual_color.cpp +++ b/apps/manual_color/manual_color.cpp @@ -98,8 +98,8 @@ GLuint make_tex(const std::string& fn) float rot = 0; float width = 34; float height = 71; -const unsigned int window_width = 500; -const unsigned int window_height = 400; +const uint32_t window_width = 500; +const uint32_t window_height = 400; int mouse_old_x, mouse_old_y; int mouse_buttons = 0; float rotate_x = 0.0, rotate_y = 0.0; diff --git a/apps/multi_session_registration/multi_session_registration.cpp b/apps/multi_session_registration/multi_session_registration.cpp index d2334d33..fbdbe721 100644 --- a/apps/multi_session_registration/multi_session_registration.cpp +++ b/apps/multi_session_registration/multi_session_registration.cpp @@ -676,11 +676,11 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = + const std::vector& source = sessions[edges[index_active_edge].index_session_to] .point_clouds_container.point_clouds[edges[index_active_edge].index_to] .points_local; - std::vector target = + const std::vector& target = sessions[edges[index_active_edge].index_session_from] .point_clouds_container.point_clouds[edges[index_active_edge].index_from] .points_local; @@ -962,11 +962,11 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = + const std::vector& source = sessions[edges[index_active_edge].index_session_to] .point_clouds_container.point_clouds[edges[index_active_edge].index_to] .points_local; - std::vector target = + const std::vector& target = ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; if (icp.compute(source, target, sr, number_of_iterations, m_pose)) @@ -978,11 +978,11 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = + const std::vector& source = sessions[edges[index_active_edge].index_session_to] .point_clouds_container.point_clouds[edges[index_active_edge].index_to] .points_local; - std::vector target = + const std::vector& target = sessions[edges[index_active_edge].index_session_from] .point_clouds_container.point_clouds[edges[index_active_edge].index_from] .points_local; @@ -1079,11 +1079,11 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = + const std::vector& source = sessions[edges[index_active_edge].index_session_to] .point_clouds_container.point_clouds[edges[index_active_edge].index_to] .points_local; - std::vector target = + const std::vector& target = ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; if (icp.compute(source, target, sr, number_of_iterations, m_pose)) @@ -1095,11 +1095,11 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = + const std::vector& source = sessions[edges[index_active_edge].index_session_to] .point_clouds_container.point_clouds[edges[index_active_edge].index_to] .points_local; - std::vector target = + const std::vector& target = sessions[edges[index_active_edge].index_session_from] .point_clouds_container.point_clouds[edges[index_active_edge].index_from] .points_local; @@ -1195,11 +1195,11 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = + const std::vector& source = sessions[edges[index_active_edge].index_session_to] .point_clouds_container.point_clouds[edges[index_active_edge].index_to] .points_local; - std::vector target = + const std::vector& target = ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; if (icp.compute(source, target, sr, number_of_iterations, m_pose)) @@ -1213,11 +1213,11 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = + const std::vector& source = sessions[edges[index_active_edge].index_session_to] .point_clouds_container.point_clouds[edges[index_active_edge].index_to] .points_local; - std::vector target = + const std::vector& target = sessions[edges[index_active_edge].index_session_from] .point_clouds_container.point_clouds[edges[index_active_edge].index_from] .points_local; @@ -1315,11 +1315,11 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = + const std::vector& source = sessions[edges[index_active_edge].index_session_to] .point_clouds_container.point_clouds[edges[index_active_edge].index_to] .points_local; - std::vector target = + const std::vector& target = ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; if (icp.compute(source, target, sr, number_of_iterations, m_pose)) @@ -1333,11 +1333,11 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = + const std::vector& source = sessions[edges[index_active_edge].index_session_to] .point_clouds_container.point_clouds[edges[index_active_edge].index_to] .points_local; - std::vector target = + const std::vector& target = sessions[edges[index_active_edge].index_session_from] .point_clouds_container.point_clouds[edges[index_active_edge].index_from] .points_local; @@ -1435,11 +1435,11 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = + const std::vector& source = sessions[edges[index_active_edge].index_session_to] .point_clouds_container.point_clouds[edges[index_active_edge].index_to] .points_local; - std::vector target = + const std::vector& target = ground_truth; // sessions[edges[index_active_edge].index_session_from].point_clouds_container.point_clouds[edges[index_active_edge].index_from].points_local; if (icp.compute(source, target, sr, number_of_iterations, m_pose)) @@ -1453,11 +1453,11 @@ void loop_closure_gui() PairWiseICP icp; auto m_pose = affine_matrix_from_pose_tait_bryan(edges[index_active_edge].relative_pose_tb); - std::vector source = + const std::vector& source = sessions[edges[index_active_edge].index_session_to] .point_clouds_container.point_clouds[edges[index_active_edge].index_to] .points_local; - std::vector target = + const std::vector& target = sessions[edges[index_active_edge].index_session_from] .point_clouds_container.point_clouds[edges[index_active_edge].index_from] .points_local; @@ -1525,7 +1525,7 @@ void loop_closure_gui() void save_trajectories_to_laz( const Session& session, - std::string output_file_name, + const std::string& output_file_name, float curve_consecutive_distance_meters, float not_curve_consecutive_distance_meters, bool is_trajectory_export_downsampling) @@ -1660,7 +1660,7 @@ void createDXFPolyline(const std::string& filename, const std::vector #include -const unsigned int window_width = 800; -const unsigned int window_height = 600; +const uint32_t window_width = 800; +const uint32_t window_height = 600; double camera_ortho_xy_view_zoom = 10; double camera_ortho_xy_view_shift_x = 0.0; double camera_ortho_xy_view_shift_y = 0.0; @@ -72,19 +72,18 @@ std::vector get_lowest_points_indexes(const PointCloud& pc, Eigen::Vector2d { std::vector indexes; - std::vector> indexes_tuple; + std::vector> indexes_tuple; for (size_t i = 0; i < pc.points_local.size(); i++) { - unsigned long long int index = get_rgd_index_2D(pc.points_local[i], bucket_dim_xy); + uint64_t index = get_rgd_index_2D(pc.points_local[i], bucket_dim_xy); indexes_tuple.emplace_back(index, i, pc.points_local[i].z()); } std::sort( indexes_tuple.begin(), indexes_tuple.end(), - [](const std::tuple& a, - const std::tuple& b) + [](const std::tuple& a, const std::tuple& b) { return (std::get<0>(a) == std::get<0>(b)) ? (std::get<2>(a) < std::get<2>(b)) : (std::get<0>(a) < std::get<0>(b)); }); diff --git a/apps/quick_start_demo/quick_start_demo.cpp b/apps/quick_start_demo/quick_start_demo.cpp index 2c99c88b..805f950c 100644 --- a/apps/quick_start_demo/quick_start_demo.cpp +++ b/apps/quick_start_demo/quick_start_demo.cpp @@ -29,8 +29,8 @@ #define SAMPLE_PERIOD (1.0 / 200.0) namespace fs = std::filesystem; -const unsigned int window_width = 800; -const unsigned int window_height = 600; +const uint32_t window_width = 800; +const uint32_t window_height = 600; double camera_ortho_xy_view_zoom = 10; double camera_ortho_xy_view_shift_x = 0.0; double camera_ortho_xy_view_shift_y = 0.0; @@ -599,7 +599,7 @@ bool compute_step_2_demo(std::vector &worker_data, LidarOdometryPara acc_distance = acc_distance_tmp; std::cout << "please split data set into subsets" << std::endl; ts_failure = worker_data[i].intermediate_trajectory_timestamps[0].first; - // std::cout << "calculations canceled for TIMESTAMP: " << (long long int)worker_data[i].intermediate_trajectory_timestamps[0].first << std::endl; + // std::cout << "calculations canceled for TIMESTAMP: " << (int64_t)worker_data[i].intermediate_trajectory_timestamps[0].first << std::endl; return false; } @@ -1061,7 +1061,7 @@ int main(int argc, char *argv[]) } wd.original_points = points; - for (unsigned long long int k = 0; k < wd.original_points.size(); k++) + for (uint64_t k = 0; k < wd.original_points.size(); k++) { Point3Di &p = wd.original_points[k]; auto lower = std::lower_bound(wd.intermediate_trajectory_timestamps.begin(), wd.intermediate_trajectory_timestamps.end(), p.timestamp, diff --git a/apps/single_session_manual_coloring/single_session_manual_coloring.cpp b/apps/single_session_manual_coloring/single_session_manual_coloring.cpp index 2e892bd6..caf68f29 100644 --- a/apps/single_session_manual_coloring/single_session_manual_coloring.cpp +++ b/apps/single_session_manual_coloring/single_session_manual_coloring.cpp @@ -33,8 +33,8 @@ #include #include -const unsigned int window_width = 800; -const unsigned int window_height = 600; +const uint32_t window_width = 800; +const uint32_t window_height = 600; double camera_ortho_xy_view_zoom = 10; double camera_ortho_xy_view_shift_x = 0.0; double camera_ortho_xy_view_shift_y = 0.0; diff --git a/core/include/hash_utils.h b/core/include/hash_utils.h index 35973eb6..b4887ea7 100644 --- a/core/include/hash_utils.h +++ b/core/include/hash_utils.h @@ -3,7 +3,7 @@ #include // this function provides unique index -unsigned long long int get_index(const int16_t x, const int16_t y, const int16_t z); +uint64_t get_index(const int16_t x, const int16_t y, const int16_t z); // this function provides unique index for input point p and 3D space decomposition into buckets b -unsigned long long int get_rgd_index(const Eigen::Vector3d p, const Eigen::Vector3d b); +uint64_t get_rgd_index(const Eigen::Vector3d p, const Eigen::Vector3d b); diff --git a/core/include/icp.h b/core/include/icp.h index 2a3aedad..187dcea9 100644 --- a/core/include/icp.h +++ b/core/include/icp.h @@ -25,10 +25,10 @@ class ICP struct Job { - long long unsigned int index_begin_inclusive; - long long unsigned int index_end_exclusive; + uint64_t index_begin_inclusive; + uint64_t index_end_exclusive; }; - std::vector get_jobs(long long unsigned int size, int num_threads = 8); + std::vector get_jobs(uint64_t size, int num_threads = 8); ICP() { diff --git a/core/include/ndt.h b/core/include/ndt.h index c6043788..158a2ee9 100644 --- a/core/include/ndt.h +++ b/core/include/ndt.h @@ -20,7 +20,7 @@ class NDT int number_of_buckets_X; int number_of_buckets_Y; int number_of_buckets_Z; - long long unsigned int number_of_buckets; + uint64_t number_of_buckets; double resolution_X; double resolution_Y; double resolution_Z; @@ -29,15 +29,15 @@ class NDT struct PointBucketIndexPair { int index_of_point; - long long unsigned int index_of_bucket; + uint64_t index_of_bucket; int index_pose; }; struct Bucket { - long long unsigned int index_begin; - long long unsigned int index_end; - long long unsigned int number_of_points; + uint64_t index_begin; + uint64_t index_end; + uint64_t number_of_points; Eigen::Vector3d mean; Eigen::Matrix3d cov; Eigen::Vector3d normal_vector; @@ -48,23 +48,23 @@ class NDT Eigen::Vector3d mean; Eigen::Matrix3d cov; Eigen::Vector3d normal_vector; - std::vector point_indexes; + std::vector point_indexes; bool valid = false; }; struct Bucket2 { int classification = 0; // 1 - ceiling, 2 - floor - long long unsigned int index_begin_inclusive; - long long unsigned int index_end_exclusive; - // long long unsigned int number_of_points; - std::unordered_map buckets; + uint64_t index_begin_inclusive; + uint64_t index_end_exclusive; + // uint64_t number_of_points; + std::unordered_map buckets; }; struct Job { - long long unsigned int index_begin_inclusive; - long long unsigned int index_end_exclusive; + uint64_t index_begin_inclusive; + uint64_t index_end_exclusive; }; enum PoseConvention @@ -117,7 +117,7 @@ class NDT std::vector& buckets, GridParameters& rgd_params, int num_threads = std::thread::hardware_concurrency()); - std::vector get_jobs(long long unsigned int size, int num_threads = std::thread::hardware_concurrency()); + std::vector get_jobs(uint64_t size, int num_threads = std::thread::hardware_concurrency()); void reindex( std::vector& points, std::vector& index_pair, NDT::GridParameters& rgd_params, int num_threads); void reindex( @@ -163,14 +163,14 @@ class NDT bool compute_cov_mean( std::vector& points, std::vector& index_pair, - std::map& buckets, + std::map& buckets, GridParameters& rgd_params, int num_threads = std::thread::hardware_concurrency()); void build_rgd( std::vector& points, std::vector& index_pair, - std::map& buckets, + std::map& buckets, GridParameters& rgd_params, int num_threads = std::thread::hardware_concurrency()); diff --git a/core/include/point_cloud.h b/core/include/point_cloud.h index 8bf9d3c4..f8c6f631 100644 --- a/core/include/point_cloud.h +++ b/core/include/point_cloud.h @@ -25,7 +25,7 @@ class PointCloud int number_of_buckets_X = 0; int number_of_buckets_Y = 0; int number_of_buckets_Z = 0; - long long unsigned int number_of_buckets = 0; + uint64_t number_of_buckets = 0; double resolution_X = 2.0; double resolution_Y = 2.0; double resolution_Z = 2.0; @@ -34,21 +34,21 @@ class PointCloud struct PointBucketIndexPair { int index_of_point = 0; - long long unsigned int index_of_bucket = 0; + uint64_t index_of_bucket = 0; // int index_pose; }; struct Bucket { - long long unsigned int index_begin = 0; - long long unsigned int index_end = 0; - long long unsigned int number_of_points = 0; + uint64_t index_begin = 0; + uint64_t index_end = 0; + uint64_t number_of_points = 0; }; struct Job { - long long unsigned int index_begin_inclusive = 0; - long long unsigned int index_end_exclusive = 0; + uint64_t index_begin_inclusive = 0; + uint64_t index_end_exclusive = 0; }; struct NearestNeighbour @@ -79,11 +79,9 @@ class PointCloud visible = true; gizmo = false; - }; - ~PointCloud() - { - ; - }; + } + + ~PointCloud() = default; GridParameters rgd_params; std::vector index_pairs; @@ -133,14 +131,14 @@ class PointCloud bool fixed_ka = false; bool load(const std::string& file_name); - bool load_pc(std::string input_file_name, bool load_cache_mode); + bool load_pc(const std::string& input_file_name, bool load_cache_mode); void update_from_gui(); - bool save_as_global(std::string file_name); + bool save_as_global(const std::string& file_name); // rgd bool build_rgd(); void grid_calculate_params(std::vector& points, GridParameters& params); void reindex(std::vector& ip, std::vector& points, GridParameters rgd_params); - std::vector get_jobs(long long unsigned int size, int num_threads); + std::vector get_jobs(uint64_t size, int num_threads); void cout_rgd(); std::vector> nns(PointCloud& pc_target, double search_radious); void clean(); diff --git a/core/include/registration_plane_feature.h b/core/include/registration_plane_feature.h index 2207f437..e40c3a2e 100644 --- a/core/include/registration_plane_feature.h +++ b/core/include/registration_plane_feature.h @@ -44,10 +44,10 @@ class RegistrationPlaneFeature struct Job { - long long unsigned int index_begin_inclusive; - long long unsigned int index_end_exclusive; + uint64_t index_begin_inclusive; + uint64_t index_end_exclusive; }; - std::vector get_jobs(long long unsigned int size, int num_threads = 8); + std::vector get_jobs(uint64_t size, int num_threads = 8); RegistrationPlaneFeature() { diff --git a/core/include/structures.h b/core/include/structures.h index b574ee3d..a28d40d9 100644 --- a/core/include/structures.h +++ b/core/include/structures.h @@ -310,20 +310,20 @@ struct PointCloudWithPose struct Job { - long long unsigned int index_begin_inclusive; - long long unsigned int index_end_exclusive; + uint64_t index_begin_inclusive; + uint64_t index_end_exclusive; }; -inline std::vector get_jobs(long long unsigned int size, int num_threads) +inline std::vector get_jobs(uint64_t size, int num_threads) { int hc = size / num_threads; if (hc < 1) hc = 1; std::vector jobs; - for (long long unsigned int i = 0; i < size; i += hc) + for (uint64_t i = 0; i < size; i += hc) { - long long unsigned int sequence_length = hc; + uint64_t sequence_length = hc; if (i + hc >= size) { sequence_length = size - i; diff --git a/core/include/surface.h b/core/include/surface.h index aa9197cf..822023b0 100644 --- a/core/include/surface.h +++ b/core/include/surface.h @@ -3,16 +3,15 @@ #include #include -inline unsigned long long int get_index_2D(const int16_t x, const int16_t y /*, const int16_t z*/) +inline uint64_t get_index_2D(const int16_t x, const int16_t y /*, const int16_t z*/) { - // return ((static_cast(x) << 32) & (0x0000FFFF00000000ull)) | - // ((static_cast(y) << 16) & (0x00000000FFFF0000ull)) | - // ((static_cast(z) << 0) & (0x000000000000FFFFull)); - return ((static_cast(x) << 16) & (0x00000000FFFF0000ull)) | - ((static_cast(y) << 0) & (0x000000000000FFFFull)); + // return ((static_cast(x) << 32) & (0x0000FFFF00000000ull)) | + // ((static_cast(y) << 16) & (0x00000000FFFF0000ull)) | + // ((static_cast(z) << 0) & (0x000000000000FFFFull)); + return ((static_cast(x) << 16) & (0x00000000FFFF0000ull)) | ((static_cast(y) << 0) & (0x000000000000FFFFull)); } -inline unsigned long long int get_rgd_index_2D(const Eigen::Vector3d p, const Eigen::Vector2d b) +inline uint64_t get_rgd_index_2D(const Eigen::Vector3d p, const Eigen::Vector2d b) { int16_t x = static_cast(p.x() / b.x()); int16_t y = static_cast(p.y() / b.y()); diff --git a/core/include/utils.hpp b/core/include/utils.hpp index ff5cdde9..bc404884 100644 --- a/core/include/utils.hpp +++ b/core/include/utils.hpp @@ -23,8 +23,8 @@ constexpr const char* xText = "Longitudinal (forward/backward)"; constexpr const char* yText = "Lateral (left/right)"; constexpr const char* zText = "Vertical (up/down)"; -const unsigned int window_width = 800; -const unsigned int window_height = 600; +const uint32_t window_width = 800; +const uint32_t window_height = 600; const float camera_transition_speed = 1.0f; // higher = faster diff --git a/core/src/hash_utils.cpp b/core/src/hash_utils.cpp index e866947c..3748225c 100644 --- a/core/src/hash_utils.cpp +++ b/core/src/hash_utils.cpp @@ -2,14 +2,13 @@ #include -unsigned long long int get_index(const int16_t x, const int16_t y, const int16_t z) +uint64_t get_index(const int16_t x, const int16_t y, const int16_t z) { - return ((static_cast(x) << 32) & (0x0000FFFF00000000ull)) | - ((static_cast(y) << 16) & (0x00000000FFFF0000ull)) | - ((static_cast(z) << 0) & (0x000000000000FFFFull)); + return ((static_cast(x) << 32) & (0x0000FFFF00000000ull)) | ((static_cast(y) << 16) & (0x00000000FFFF0000ull)) | + ((static_cast(z) << 0) & (0x000000000000FFFFull)); } -unsigned long long int get_rgd_index(const Eigen::Vector3d p, const Eigen::Vector3d b) +uint64_t get_rgd_index(const Eigen::Vector3d p, const Eigen::Vector3d b) { int16_t x = static_cast(p.x() / b.x()); int16_t y = static_cast(p.y() / b.y()); diff --git a/core/src/icp.cpp b/core/src/icp.cpp index 59107c69..ddd3509c 100644 --- a/core/src/icp.cpp +++ b/core/src/icp.cpp @@ -8,16 +8,16 @@ #include #include -std::vector ICP::get_jobs(long long unsigned int size, int num_threads) +std::vector ICP::get_jobs(uint64_t size, int num_threads) { int hc = size / num_threads; if (hc < 1) hc = 1; std::vector jobs; - for (long long unsigned int i = 0; i < size; i += hc) + for (uint64_t i = 0; i < size; i += hc) { - long long unsigned int sequence_length = hc; + uint64_t sequence_length = hc; if (i + hc >= size) { sequence_length = size - i; diff --git a/core/src/local_shape_features.cpp b/core/src/local_shape_features.cpp index 628869c7..dfcf1918 100644 --- a/core/src/local_shape_features.cpp +++ b/core/src/local_shape_features.cpp @@ -5,27 +5,27 @@ bool LocalShapeFeatures::calculate_local_shape_features(std::vector& points, const Params& params) { - std::vector> indexes; + std::vector> indexes; for (int i = 0; i < points.size(); i++) { - unsigned long long int index = get_rgd_index(points[i].coordinates_global, params.search_radious); + uint64_t index = get_rgd_index(points[i].coordinates_global, params.search_radious); indexes.emplace_back(index, i); } std::sort( indexes.begin(), indexes.end(), - [](const std::pair& a, const std::pair& b) + [](const std::pair& a, const std::pair& b) { return a.first < b.first; }); - std::unordered_map> buckets; + std::unordered_map> buckets; - for (unsigned int i = 0; i < indexes.size(); i++) + for (uint32_t i = 0; i < indexes.size(); i++) { - unsigned long long int index_of_bucket = indexes[i].first; + uint64_t index_of_bucket = indexes[i].first; if (buckets.contains(index_of_bucket)) { buckets[index_of_bucket].second = i; @@ -37,7 +37,7 @@ bool LocalShapeFeatures::calculate_local_shape_features(std::vector& index) + const auto hessian_fun = [&](const std::pair& index) { int index_element_source = index.second; Eigen::Vector3d source = points[index_element_source].coordinates_global; @@ -53,7 +53,7 @@ bool LocalShapeFeatures::calculate_local_shape_features(std::vector& point_cloud_global, max_z += in_out_params.bounding_box_extension; min_z -= in_out_params.bounding_box_extension; - long long unsigned int number_of_buckets_X = ((max_x - min_x) / in_out_params.resolution_X) + 1; - long long unsigned int number_of_buckets_Y = ((max_y - min_y) / in_out_params.resolution_Y) + 1; - long long unsigned int number_of_buckets_Z = ((max_z - min_z) / in_out_params.resolution_Z) + 1; + uint64_t number_of_buckets_X = ((max_x - min_x) / in_out_params.resolution_X) + 1; + uint64_t number_of_buckets_Y = ((max_y - min_y) / in_out_params.resolution_Y) + 1; + uint64_t number_of_buckets_Z = ((max_z - min_z) / in_out_params.resolution_Z) + 1; in_out_params.number_of_buckets_X = number_of_buckets_X; in_out_params.number_of_buckets_Y = number_of_buckets_Y; in_out_params.number_of_buckets_Z = number_of_buckets_Z; - in_out_params.number_of_buckets = static_cast(number_of_buckets_X) * - static_cast(number_of_buckets_Y) * static_cast(number_of_buckets_Z); + in_out_params.number_of_buckets = static_cast(number_of_buckets_X) * static_cast(number_of_buckets_Y) * + static_cast(number_of_buckets_Z); in_out_params.bounding_box_max_X = max_x; in_out_params.bounding_box_min_X = min_x; @@ -110,15 +110,15 @@ void NDT::grid_calculate_params(const std::vector& point_cloud_global, max_z += in_out_params.bounding_box_extension; min_z -= in_out_params.bounding_box_extension; - long long unsigned int number_of_buckets_X = ((max_x - min_x) / in_out_params.resolution_X) + 1; - long long unsigned int number_of_buckets_Y = ((max_y - min_y) / in_out_params.resolution_Y) + 1; - long long unsigned int number_of_buckets_Z = ((max_z - min_z) / in_out_params.resolution_Z) + 1; + uint64_t number_of_buckets_X = ((max_x - min_x) / in_out_params.resolution_X) + 1; + uint64_t number_of_buckets_Y = ((max_y - min_y) / in_out_params.resolution_Y) + 1; + uint64_t number_of_buckets_Z = ((max_z - min_z) / in_out_params.resolution_Z) + 1; in_out_params.number_of_buckets_X = number_of_buckets_X; in_out_params.number_of_buckets_Y = number_of_buckets_Y; in_out_params.number_of_buckets_Z = number_of_buckets_Z; - in_out_params.number_of_buckets = static_cast(number_of_buckets_X) * - static_cast(number_of_buckets_Y) * static_cast(number_of_buckets_Z); + in_out_params.number_of_buckets = static_cast(number_of_buckets_X) * static_cast(number_of_buckets_Y) * + static_cast(number_of_buckets_Z); in_out_params.bounding_box_max_X = max_x; in_out_params.bounding_box_min_X = min_x; @@ -131,15 +131,15 @@ void NDT::grid_calculate_params(const std::vector& point_cloud_global, void NDT::grid_calculate_params( GridParameters& in_out_params, double min_x, double max_x, double min_y, double max_y, double min_z, double max_z) { - long long unsigned int number_of_buckets_X = ((max_x - min_x) / in_out_params.resolution_X) + 1; - long long unsigned int number_of_buckets_Y = ((max_y - min_y) / in_out_params.resolution_Y) + 1; - long long unsigned int number_of_buckets_Z = ((max_z - min_z) / in_out_params.resolution_Z) + 1; + uint64_t number_of_buckets_X = ((max_x - min_x) / in_out_params.resolution_X) + 1; + uint64_t number_of_buckets_Y = ((max_y - min_y) / in_out_params.resolution_Y) + 1; + uint64_t number_of_buckets_Z = ((max_z - min_z) / in_out_params.resolution_Z) + 1; in_out_params.number_of_buckets_X = number_of_buckets_X; in_out_params.number_of_buckets_Y = number_of_buckets_Y; in_out_params.number_of_buckets_Z = number_of_buckets_Z; - in_out_params.number_of_buckets = static_cast(number_of_buckets_X) * - static_cast(number_of_buckets_Y) * static_cast(number_of_buckets_Z); + in_out_params.number_of_buckets = static_cast(number_of_buckets_X) * static_cast(number_of_buckets_Y) * + static_cast(number_of_buckets_Z); in_out_params.bounding_box_max_X = max_x; in_out_params.bounding_box_min_X = min_x; @@ -149,16 +149,16 @@ void NDT::grid_calculate_params( in_out_params.bounding_box_min_Z = min_z; } -std::vector NDT::get_jobs(long long unsigned int size, int num_threads) +std::vector NDT::get_jobs(uint64_t size, int num_threads) { int hc = size / num_threads; if (hc < 1) hc = 1; std::vector jobs; - for (long long unsigned int i = 0; i < size; i += hc) + for (uint64_t i = 0; i < size; i += hc) { - long long unsigned int sequence_length = hc; + uint64_t sequence_length = hc; if (i + hc >= size) { sequence_length = size - i; @@ -211,13 +211,13 @@ void reindex_job( continue; } - long long unsigned int ix = (p.x - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; - long long unsigned int iy = (p.y - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; - long long unsigned int iz = (p.z - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; + uint64_t ix = (p.x - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; + uint64_t iy = (p.y - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; + uint64_t iz = (p.z - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; - (*pairs)[ii].index_of_bucket = ix * static_cast(rgd_params.number_of_buckets_Y) * - static_cast(rgd_params.number_of_buckets_Z) + - iy * static_cast(rgd_params.number_of_buckets_Z) + iz; + (*pairs)[ii].index_of_bucket = + ix * static_cast(rgd_params.number_of_buckets_Y) * static_cast(rgd_params.number_of_buckets_Z) + + iy * static_cast(rgd_params.number_of_buckets_Z) + iz; } } @@ -257,13 +257,13 @@ void reindex_jobi( continue; } - long long unsigned int ix = (p.point.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; - long long unsigned int iy = (p.point.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; - long long unsigned int iz = (p.point.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; + uint64_t ix = (p.point.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; + uint64_t iy = (p.point.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; + uint64_t iz = (p.point.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; - (*pairs)[ii].index_of_bucket = ix * static_cast(rgd_params.number_of_buckets_Y) * - static_cast(rgd_params.number_of_buckets_Z) + - iy * static_cast(rgd_params.number_of_buckets_Z) + iz; + (*pairs)[ii].index_of_bucket = + ix * static_cast(rgd_params.number_of_buckets_Y) * static_cast(rgd_params.number_of_buckets_Z) + + iy * static_cast(rgd_params.number_of_buckets_Z) + iz; } } @@ -342,8 +342,8 @@ void build_rgd_job(int i, NDT::Job* job, std::vector* int ind = ii; if (ind == 0) { - long long unsigned int index_of_bucket = (*index_pair)[ind].index_of_bucket; - long long unsigned int index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; + uint64_t index_of_bucket = (*index_pair)[ind].index_of_bucket; + uint64_t index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; (*buckets)[index_of_bucket].index_begin = ind; if (index_of_bucket != index_of_bucket_1) @@ -361,8 +361,8 @@ void build_rgd_job(int i, NDT::Job* job, std::vector* } else if (ind + 1 < (*index_pair).size()) { - long long unsigned int index_of_bucket = (*index_pair)[ind].index_of_bucket; - long long unsigned int index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; + uint64_t index_of_bucket = (*index_pair)[ind].index_of_bucket; + uint64_t index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; if (index_of_bucket != index_of_bucket_1) { @@ -377,8 +377,8 @@ void build_rgd_final_job(int i, NDT::Job* job, std::vector* buckets { for (size_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) { - long long unsigned int index_begin = (*buckets)[ii].index_begin; - long long unsigned int index_end = (*buckets)[ii].index_end; + uint64_t index_begin = (*buckets)[ii].index_begin; + uint64_t index_end = (*buckets)[ii].index_end; if (index_end > index_begin) { (*buckets)[ii].number_of_points = index_end - index_begin; @@ -4027,7 +4027,7 @@ bool NDT::compute_cov_mean( bool NDT::compute_cov_mean( std::vector& points, std::vector& index_pair, - std::map& buckets, + std::map& buckets, GridParameters& rgd_params, int num_threads) { @@ -4145,7 +4145,7 @@ bool NDT::compute_cov_mean( void NDT::build_rgd( std::vector& points, std::vector& index_pair, - std::map& buckets, + std::map& buckets, NDT::GridParameters& rgd_params, int num_threads) { diff --git a/core/src/nmea.cpp b/core/src/nmea.cpp index 91ea3f3d..350965ff 100644 --- a/core/src/nmea.cpp +++ b/core/src/nmea.cpp @@ -48,7 +48,7 @@ namespace hd_mapping::nmea { checksum ^= nmea[i]; } - unsigned int expected; + uint32_t expected; std::istringstream iss(nmea.substr(asterisk + 1, 2)); iss >> std::hex >> expected; return checksum == expected; diff --git a/core/src/pair_wise_iterative_closest_point.cpp b/core/src/pair_wise_iterative_closest_point.cpp index 8fadc8bb..e6faec77 100644 --- a/core/src/pair_wise_iterative_closest_point.cpp +++ b/core/src/pair_wise_iterative_closest_point.cpp @@ -145,27 +145,27 @@ bool PairWiseICP::compute( for (int iter = 0; iter < number_of_iterations; iter++) { - std::vector> indexes; + std::vector> indexes; for (int i = 0; i < target.size(); i++) { - unsigned long long int index = get_rgd_index(target[i], { search_radious, search_radious, search_radious }); + uint64_t index = get_rgd_index(target[i], { search_radious, search_radious, search_radious }); indexes.emplace_back(index, i); } std::sort( indexes.begin(), indexes.end(), - [](const std::pair& a, const std::pair& b) + [](const std::pair& a, const std::pair& b) { return a.first < b.first; }); - std::unordered_map> buckets; + std::unordered_map> buckets; - for (unsigned int i = 0; i < indexes.size(); i++) + for (uint32_t i = 0; i < indexes.size(); i++) { - unsigned long long int index_of_bucket = indexes[i].first; + uint64_t index_of_bucket = indexes[i].first; if (buckets.contains(index_of_bucket)) { buckets[index_of_bucket].second = i; @@ -207,7 +207,7 @@ bool PairWiseICP::compute( for (double z = -search_radious; z <= search_radious; z += search_radious) { Eigen::Vector3d source_point_global_ext = source_point_global + Eigen::Vector3d(x, y, z); - unsigned long long int index_of_bucket = + uint64_t index_of_bucket = get_rgd_index(source_point_global_ext, { search_radious, search_radious, search_radious }); if (buckets.contains(index_of_bucket)) @@ -381,27 +381,27 @@ bool PairWiseICP::compute_fast( // std::cout << "PairWiseICP::compute" << std::endl; bool multithread = true; - std::vector> indexes; + std::vector> indexes; for (int i = 0; i < target.size(); i++) { - unsigned long long int index = get_rgd_index(target[i], { search_radious, search_radious, search_radious }); + uint64_t index = get_rgd_index(target[i], { search_radious, search_radious, search_radious }); indexes.emplace_back(index, i); } std::sort( indexes.begin(), indexes.end(), - [](const std::pair& a, const std::pair& b) + [](const std::pair& a, const std::pair& b) { return a.first < b.first; }); - std::unordered_map> buckets; + std::unordered_map> buckets; - for (unsigned int i = 0; i < indexes.size(); i++) + for (uint32_t i = 0; i < indexes.size(); i++) { - unsigned long long int index_of_bucket = indexes[i].first; + uint64_t index_of_bucket = indexes[i].first; if (buckets.contains(index_of_bucket)) { buckets[index_of_bucket].second = i; @@ -444,7 +444,7 @@ bool PairWiseICP::compute_fast( for (double z = -search_radious; z <= search_radious; z += search_radious) { Eigen::Vector3d source_point_global_ext = source_point_global + Eigen::Vector3d(x, y, z); - unsigned long long int index_of_bucket = + uint64_t index_of_bucket = get_rgd_index(source_point_global_ext, { search_radious, search_radious, search_radious }); if (buckets.contains(index_of_bucket)) diff --git a/core/src/point_cloud.cpp b/core/src/point_cloud.cpp index 6954a6cf..c8408a5a 100644 --- a/core/src/point_cloud.cpp +++ b/core/src/point_cloud.cpp @@ -90,7 +90,7 @@ bool PointCloud::load(const std::string& file_name) return true; } -bool PointCloud::load_pc(std::string input_file_name, bool load_cache_mode) +bool PointCloud::load_pc(const std::string& input_file_name, bool load_cache_mode) { double min_ts = std::numeric_limits::max(); double max_ts = std::numeric_limits::lowest(); @@ -291,7 +291,7 @@ void PointCloud::update_from_gui() m_pose = affine_matrix_from_pose_tait_bryan(pose); } -bool PointCloud::save_as_global(std::string file_name) +bool PointCloud::save_as_global(const std::string& file_name) { std::ofstream outfile; outfile.open(file_name); @@ -327,13 +327,13 @@ void build_rgd_job( { // std::cout << "build_rgd_job:[" << i << "]" << std::endl; - for (long long unsigned int ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) + for (uint64_t ii = job->index_begin_inclusive; ii < job->index_end_exclusive; ii++) { - long long unsigned int ind = ii; + uint64_t ind = ii; if (ind == 0) { - long long unsigned int index_of_bucket = (*index_pair)[ind].index_of_bucket; - long long unsigned int index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; + uint64_t index_of_bucket = (*index_pair)[ind].index_of_bucket; + uint64_t index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; (*buckets)[index_of_bucket].index_begin = ind; if (index_of_bucket != index_of_bucket_1) @@ -351,8 +351,8 @@ void build_rgd_job( } else if (ind + 1 < (*index_pair).size()) { - long long unsigned int index_of_bucket = (*index_pair)[ind].index_of_bucket; - long long unsigned int index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; + uint64_t index_of_bucket = (*index_pair)[ind].index_of_bucket; + uint64_t index_of_bucket_1 = (*index_pair)[ind + 1].index_of_bucket; if (index_of_bucket != index_of_bucket_1) { @@ -371,8 +371,8 @@ void build_rgd_final_job(int i, PointCloud::Job* job, std::vectorindex_begin_inclusive << " " << job->index_end_exclusive << " " << (*buckets).size() << std::endl; - long long unsigned int index_begin = (*buckets)[ii].index_begin; - long long unsigned int index_end = (*buckets)[ii].index_end; + uint64_t index_begin = (*buckets)[ii].index_begin; + uint64_t index_end = (*buckets)[ii].index_end; if (index_begin != -1 && index_end != -1) { (*buckets)[ii].number_of_points = index_end - index_begin; @@ -478,15 +478,15 @@ void PointCloud::grid_calculate_params(std::vector& points, Gri min_z -= params.bounding_box_extension; max_z += params.bounding_box_extension; - long long unsigned int number_of_buckets_X = ((max_x - min_x) / params.resolution_X) + 1; - long long unsigned int number_of_buckets_Y = ((max_y - min_y) / params.resolution_Y) + 1; - long long unsigned int number_of_buckets_Z = ((max_z - min_z) / params.resolution_Z) + 1; + uint64_t number_of_buckets_X = ((max_x - min_x) / params.resolution_X) + 1; + uint64_t number_of_buckets_Y = ((max_y - min_y) / params.resolution_Y) + 1; + uint64_t number_of_buckets_Z = ((max_z - min_z) / params.resolution_Z) + 1; params.number_of_buckets_X = number_of_buckets_X; params.number_of_buckets_Y = number_of_buckets_Y; params.number_of_buckets_Z = number_of_buckets_Z; - params.number_of_buckets = static_cast(number_of_buckets_X) * - static_cast(number_of_buckets_Y) * static_cast(number_of_buckets_Z); + params.number_of_buckets = static_cast(number_of_buckets_X) * static_cast(number_of_buckets_Y) * + static_cast(number_of_buckets_Z); params.bounding_box_max_X = max_x; params.bounding_box_min_X = min_x; @@ -513,13 +513,13 @@ void reindex_job( (*pairs)[ii].index_of_bucket = 0; //(*pairs)[ii].index_pose = p.index_pose; - long long unsigned int ix = (p.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; - long long unsigned int iy = (p.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; - long long unsigned int iz = (p.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; + uint64_t ix = (p.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; + uint64_t iy = (p.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; + uint64_t iz = (p.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; - (*pairs)[ii].index_of_bucket = ix * static_cast(rgd_params.number_of_buckets_Y) * - static_cast(rgd_params.number_of_buckets_Z) + - iy * static_cast(rgd_params.number_of_buckets_Z) + iz; + (*pairs)[ii].index_of_bucket = + ix * static_cast(rgd_params.number_of_buckets_Y) * static_cast(rgd_params.number_of_buckets_Z) + + iy * static_cast(rgd_params.number_of_buckets_Z) + iz; } } @@ -553,16 +553,16 @@ void PointCloud::reindex(std::vector& ip, std::vector PointCloud::get_jobs(long long unsigned int size, int num_threads) +std::vector PointCloud::get_jobs(uint64_t size, int num_threads) { int hc = size / num_threads; if (hc < 1) hc = 1; std::vector jobs; - for (long long unsigned int i = 0; i < size; i += hc) + for (uint64_t i = 0; i < size; i += hc) { - long long unsigned int sequence_length = hc; + uint64_t sequence_length = hc; if (i + hc >= size) { sequence_length = size - i; @@ -625,13 +625,13 @@ void nearest_neighbours_job( if (p.z() < rgd_params.bounding_box_min_Z || p.z() > rgd_params.bounding_box_max_Z) continue; - long long unsigned int ix = (p.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; - long long unsigned int iy = (p.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; - long long unsigned int iz = (p.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; + uint64_t ix = (p.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; + uint64_t iy = (p.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; + uint64_t iz = (p.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; - long long unsigned int index_bucket = ix * static_cast(rgd_params.number_of_buckets_Y) * - static_cast(rgd_params.number_of_buckets_Z) + - iy * static_cast(rgd_params.number_of_buckets_Z) + iz; + uint64_t index_bucket = + ix * static_cast(rgd_params.number_of_buckets_Y) * static_cast(rgd_params.number_of_buckets_Z) + + iy * static_cast(rgd_params.number_of_buckets_Z) + iz; if (index_bucket >= 0 && index_bucket < rgd_params.number_of_buckets) { @@ -662,7 +662,7 @@ void nearest_neighbours_job( else stz = 2; - long long unsigned int index_next_bucket; + uint64_t index_next_bucket; int iter; int number_of_points_in_bucket; int l_begin; @@ -1205,13 +1205,13 @@ void compute_normal_vectors_job( Eigen::Vector3d mean(0.0, 0.0, 0.0); int number_of_points_nn = 0; - long long unsigned int ix = (point.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; - long long unsigned int iy = (point.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; - long long unsigned int iz = (point.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; + uint64_t ix = (point.x() - rgd_params.bounding_box_min_X) / rgd_params.resolution_X; + uint64_t iy = (point.y() - rgd_params.bounding_box_min_Y) / rgd_params.resolution_Y; + uint64_t iz = (point.z() - rgd_params.bounding_box_min_Z) / rgd_params.resolution_Z; - long long unsigned int index_bucket = ix * static_cast(rgd_params.number_of_buckets_Y) * - static_cast(rgd_params.number_of_buckets_Z) + - iy * static_cast(rgd_params.number_of_buckets_Z) + iz; + uint64_t index_bucket = + ix * static_cast(rgd_params.number_of_buckets_Y) * static_cast(rgd_params.number_of_buckets_Z) + + iy * static_cast(rgd_params.number_of_buckets_Z) + iz; /////////////////////////////////// if (index_bucket >= 0 && index_bucket < rgd_params.number_of_buckets) @@ -1243,7 +1243,7 @@ void compute_normal_vectors_job( else stz = 2; - long long unsigned int index_next_bucket; + uint64_t index_next_bucket; int iter; int number_of_points_in_bucket; int l_begin; @@ -1358,7 +1358,7 @@ void compute_normal_vectors_job( else stz = 2; - long long unsigned int index_next_bucket; + uint64_t index_next_bucket; int iter; int number_of_points_in_bucket; int l_begin; diff --git a/core/src/registration_plane_feature.cpp b/core/src/registration_plane_feature.cpp index 37d9ecd6..215a3bf7 100644 --- a/core/src/registration_plane_feature.cpp +++ b/core/src/registration_plane_feature.cpp @@ -11,16 +11,16 @@ #include #include -std::vector RegistrationPlaneFeature::get_jobs(long long unsigned int size, int num_threads) +std::vector RegistrationPlaneFeature::get_jobs(uint64_t size, int num_threads) { int hc = size / num_threads; if (hc < 1) hc = 1; std::vector jobs; - for (long long unsigned int i = 0; i < size; i += hc) + for (uint64_t i = 0; i < size; i += hc) { - long long unsigned int sequence_length = hc; + uint64_t sequence_length = hc; if (i + hc >= size) { sequence_length = size - i; diff --git a/core/src/surface.cpp b/core/src/surface.cpp index f237d69f..1242b2eb 100644 --- a/core/src/surface.cpp +++ b/core/src/surface.cpp @@ -442,27 +442,27 @@ std::vector Surface::get_filtered_indexes( lowest_points.emplace_back(pc[lowest_points_indexes[i]], false); } - std::vector> indexes; + std::vector> indexes; for (int i = 0; i < lowest_points.size(); i++) { - unsigned long long int index = get_rgd_index_2D(lowest_points[i].first, bucket_dim_xy); + uint64_t index = get_rgd_index_2D(lowest_points[i].first, bucket_dim_xy); indexes.emplace_back(index, i); } std::sort( indexes.begin(), indexes.end(), - [](const std::pair& a, const std::pair& b) + [](const std::pair& a, const std::pair& b) { return a.first < b.first; }); - std::unordered_map> buckets; + std::unordered_map> buckets; - for (unsigned int i = 0; i < indexes.size(); i++) + for (uint32_t i = 0; i < indexes.size(); i++) { - unsigned long long int index_of_bucket = indexes[i].first; + uint64_t index_of_bucket = indexes[i].first; if (buckets.contains(index_of_bucket)) { buckets[index_of_bucket].second = i; @@ -474,7 +474,7 @@ std::vector Surface::get_filtered_indexes( } } - const auto hessian_fun = [&](const std::pair& index) + const auto hessian_fun = [&](const std::pair& index) { int index_element_source = index.second; Eigen::Vector3d source = lowest_points[index_element_source].first; @@ -491,7 +491,7 @@ std::vector Surface::get_filtered_indexes( //{ Eigen::Vector3d position_global = source + Eigen::Vector3d(x, y, 0); - unsigned long long int index_of_bucket = get_rgd_index_2D(position_global, bucket_dim_xy); + uint64_t index_of_bucket = get_rgd_index_2D(position_global, bucket_dim_xy); if (buckets.contains(index_of_bucket)) { @@ -595,11 +595,11 @@ std::vector Surface::get_filtered_indexes( // std::cout << (int)p.second; //} - /*std::vector> indexes; + /*std::vector> indexes; for (int i = 0; i < lowest_points.size(); i++) { - unsigned long long int index = get_rgd_index_2D(lowest_points[i], bucket_dim_xy); + uint64_t index = get_rgd_index_2D(lowest_points[i], bucket_dim_xy); indexes.emplace_back(index, i); }*/ @@ -626,27 +626,27 @@ std::vector Surface::get_points_without_surface( // std::vector out_indexes; - std::vector> indexes; + std::vector> indexes; for (int i = 0; i < vertices.size(); i++) { - unsigned long long int index = get_rgd_index_2D(vertices[i].translation(), bucket_dim_xy); + uint64_t index = get_rgd_index_2D(vertices[i].translation(), bucket_dim_xy); indexes.emplace_back(index, i); } std::sort( indexes.begin(), indexes.end(), - [](const std::pair& a, const std::pair& b) + [](const std::pair& a, const std::pair& b) { return a.first < b.first; }); - std::unordered_map> buckets; + std::unordered_map> buckets; - for (unsigned int i = 0; i < indexes.size(); i++) + for (uint32_t i = 0; i < indexes.size(); i++) { - unsigned long long int index_of_bucket = indexes[i].first; + uint64_t index_of_bucket = indexes[i].first; if (buckets.contains(index_of_bucket)) { buckets[index_of_bucket].second = i; @@ -660,7 +660,7 @@ std::vector Surface::get_points_without_surface( for (int i = 0; i < points.size(); i++) { - unsigned long long int index_of_bucket = get_rgd_index_2D(points[i], bucket_dim_xy); + uint64_t index_of_bucket = get_rgd_index_2D(points[i], bucket_dim_xy); if (buckets.contains(index_of_bucket)) { int index = buckets[index_of_bucket].first;