How do I get it to work properly?

[kazu-321]

Hello!

I'm using ROS2 to estimate position by matching a PCD map with Point Cloud2 data.

However, I don't know how to adjust the parameters, and currently it's vibrating as shown in the image.

(Red is small GICP, blue is NDT OMP, and the hidden light blue is the correct position based on EKF.)

Could you please tell me how to set the following values?

• Should translation and rotation mask be set to 0 or 1, or are values like 0.9999 also possible?
• Should translation_eps be changed depending on the control period?
• If the initial position is given almost accurately, should max_correspondence_distance be set small?

Environment

• PCD, rosbag
• Ubuntu 22.04
• ROS 2 humble
• repository

initialize

num_threads = 4;                                       // Number of threads to be used
downsampling_resolution = 0.5;                         // Downsampling resolution
num_neighbors = 10;                                    // Number of neighbor points used for normal and covariance estimation
max_correspondence_distance = 1.0;                     // Maximum correspondence distance between points (e.g., triming threshold)
init_T_target_source = Eigen::Isometry3d::Identity();  // Initial transformation from target to source
target = std::make_shared<PointCloud>(target_points_);
target = voxelgrid_sampling_omp(*target, downsampling_resolution, num_threads);
target_tree = std::make_shared<KdTree<PointCloud>>(target, KdTreeBuilderOMP(num_threads));
estimate_covariances_omp(*target, *target_tree, num_neighbors, num_threads);

in callback

// GICP + OMP-based parallel reduction
Registration<GICPFactor, ParallelReductionOMP, RestrictDoFFactor> registration;
registration.reduction.num_threads = num_threads;
registration.rejector.max_dist_sq = max_correspondence_distance * max_correspondence_distance;
registration.criteria.translation_eps = 0.001;
this->get_parameter("mask_x", mask_x);
this->get_parameter("mask_y", mask_y);
this->get_parameter("mask_z", mask_z);
this->get_parameter("mask_yaw", mask_yaw);
registration.general_factor.set_translation_mask(Eigen::Vector3d(mask_x, mask_y, mask_z));  // Set translation mask
registration.general_factor.set_rotation_mask(Eigen::Vector3d(0, 0, mask_yaw));             // Set rotation mask
this->get_parameter("max_iterations", max_iterations);
registration.optimizer.max_iterations = max_iterations;

[kazu-321]

Also, loading large PCD data takes a long time.

If there's a better way to load it, please let me know.

https://github.com/kazu-321/small_gicp_ros2/blob/master/src/small_gicp_ros2.cpp#L43-L62