49 #include <pcl/registration/transformation_estimation_svd.h>
50 #include <pcl/kdtree/kdtree_flann.h>
51 #include <pcl/correspondence.h>
52 #include <pcl/point_cloud.h>
53 #include <pcl/pcl_exports.h>
55 #include <pcl/recognition/ransac_based/auxiliary.h>
61 template<
typename Po
intT,
typename Scalar>
68 using Matrix4 =
typename Eigen::Matrix<Scalar, 4, 4>;
72 : new_to_old_energy_ratio_ (0.99f)
85 target_points_ = target;
86 kdtree_.setInputCloud (target);
100 int num_trimmed_source_points = num_source_points_to_use, num_source_points =
static_cast<int> (source_points.
size ());
102 if ( num_trimmed_source_points >= num_source_points )
104 printf (
"WARNING in 'TrimmedICP::%s()': the user-defined number of source points of interest is greater or equal to "
105 "the total number of source points. Trimmed ICP will work correctly but won't be very efficient. Either set "
106 "the number of source points to use to a lower value or use standard ICP.\n", __func__);
107 num_trimmed_source_points = num_source_points;
111 pcl::Correspondences full_src_to_tgt (num_source_points), trimmed_src_to_tgt (num_trimmed_source_points);
116 std::vector<float> sqr_dist_to_target (1);
117 float old_energy, energy = std::numeric_limits<float>::max ();
124 for (
int i = 0 ; i < num_source_points ; ++i )
127 aux::transform (guess_and_result, source_points[i], transformed_source_point);
130 kdtree_.nearestKSearch (transformed_source_point, 1, target_index, sqr_dist_to_target);
133 full_src_to_tgt[i].index_query = i;
134 full_src_to_tgt[i].index_match = target_index[0];
135 full_src_to_tgt[i].distance = sqr_dist_to_target[0];
145 for (
int i = 0 ; i < num_trimmed_source_points ; ++i )
147 trimmed_src_to_tgt[i].index_query = full_src_to_tgt[i].index_query;
148 trimmed_src_to_tgt[i].index_match = full_src_to_tgt[i].index_match;
149 energy += full_src_to_tgt[i].distance;
152 this->estimateRigidTransformation (source_points, *target_points_, trimmed_src_to_tgt, guess_and_result);
156 while ( energy/old_energy < new_to_old_energy_ratio_ );
165 new_to_old_energy_ratio_ = 0.99f;
167 new_to_old_energy_ratio_ = ratio;
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< const PointCloud< PointT > > ConstPtr
~TrimmedICP() override=default
float new_to_old_energy_ratio_
pcl::KdTreeFLANN< PointT > kdtree_
PointCloudConstPtr target_points_
typename PointCloud::ConstPtr PointCloudConstPtr
void init(const PointCloudConstPtr &target)
Call this method before calling align().
static bool compareCorrespondences(const pcl::Correspondence &a, const pcl::Correspondence &b)
typename Eigen::Matrix< Scalar, 4, 4 > Matrix4
void setNewToOldEnergyRatio(float ratio)
void align(const PointCloud &source_points, int num_source_points_to_use, Matrix4 &guess_and_result) const
The method performs trimmed ICP, i.e., it rigidly registers the source to the target (passed to the i...
void transform(const T t[12], const T p[3], T out[3])
The first 9 elements of 't' are treated as a 3x3 matrix (row major order) and the last 3 as a transla...
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Correspondence represents a match between two entities (e.g., points, descriptors,...
A point structure representing Euclidean xyz coordinates.