47 #ifndef TRIMMED_ICP_H_
48 #define TRIMMED_ICP_H_
50 #include <pcl/registration/transformation_estimation_svd.h>
51 #include <pcl/kdtree/kdtree_flann.h>
52 #include <pcl/correspondence.h>
53 #include <pcl/point_cloud.h>
54 #include <pcl/pcl_exports.h>
61 template<
typename Po
intT,
typename Scalar>
68 typedef typename Eigen::Matrix<Scalar, 4, 4>
Matrix4;
72 : new_to_old_energy_ratio_ (0.99f)
86 target_points_ = target;
87 kdtree_.setInputCloud (target);
99 align (
const PointCloud& source_points,
int num_source_points_to_use,
Matrix4& guess_and_result)
const
101 int num_trimmed_source_points = num_source_points_to_use, num_source_points =
static_cast<int> (source_points.
size ());
103 if ( num_trimmed_source_points >= num_source_points )
105 printf (
"WARNING in 'TrimmedICP::%s()': the user-defined number of source points of interest is greater or equal to "
106 "the total number of source points. Trimmed ICP will work correctly but won't be very efficient. Either set "
107 "the number of source points to use to a lower value or use standard ICP.\n", __func__);
108 num_trimmed_source_points = num_source_points;
112 pcl::Correspondences full_src_to_tgt (num_source_points), trimmed_src_to_tgt (num_trimmed_source_points);
116 std::vector<int> target_index (1);
117 std::vector<float> sqr_dist_to_target (1);
118 float old_energy, energy = std::numeric_limits<float>::max ();
125 for (
int i = 0 ; i < num_source_points ; ++i )
131 kdtree_.nearestKSearch (transformed_source_point, 1, target_index, sqr_dist_to_target);
134 full_src_to_tgt[i].index_query = i;
135 full_src_to_tgt[i].index_match = target_index[0];
136 full_src_to_tgt[i].distance = sqr_dist_to_target[0];
146 for (
int i = 0 ; i < num_trimmed_source_points ; ++i )
148 trimmed_src_to_tgt[i].index_query = full_src_to_tgt[i].index_query;
149 trimmed_src_to_tgt[i].index_match = full_src_to_tgt[i].index_match;
150 energy += full_src_to_tgt[i].distance;
153 this->estimateRigidTransformation (source_points, *target_points_, trimmed_src_to_tgt, guess_and_result);
157 while ( energy/old_energy < new_to_old_energy_ratio_ );
163 setNewToOldEnergyRatio (
float ratio)
166 new_to_old_energy_ratio_ = 0.99f;
168 new_to_old_energy_ratio_ = ratio;