41 #ifndef PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_
42 #define PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_
48 namespace registration
51 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
double
55 const Matrix4 &transformation_matrix)
const
57 double fitness_score = 0.0;
62 input_transformed.
resize (cloud_src->size ());
63 for (std::size_t i = 0; i < cloud_src->size (); ++i)
65 const PointSource &src = cloud_src->points[i];
66 PointTarget &tgt = input_transformed.
points[i];
67 tgt.x =
static_cast<float> (transformation_matrix (0, 0) * src.x + transformation_matrix (0, 1) * src.y + transformation_matrix (0, 2) * src.z + transformation_matrix (0, 3));
68 tgt.y =
static_cast<float> (transformation_matrix (1, 0) * src.x + transformation_matrix (1, 1) * src.y + transformation_matrix (1, 2) * src.z + transformation_matrix (1, 3));
69 tgt.z =
static_cast<float> (transformation_matrix (2, 0) * src.x + transformation_matrix (2, 1) * src.y + transformation_matrix (2, 2) * src.z + transformation_matrix (2, 3));
73 if (!force_no_recompute_)
75 tree_->setPointRepresentation (point_rep);
76 tree_->setInputCloud (cloud_tgt);
79 std::vector<int> nn_indices (1);
80 std::vector<float> nn_dists (1);
84 for (std::size_t i = 0; i < input_transformed.
points.size (); ++i)
87 tree_->nearestKSearch (input_transformed.
points[i], 1, nn_indices, nn_dists);
90 if (nn_dists[0] > max_range_)
94 fitness_score += nn_dists[0];
99 return (fitness_score / nr);
100 return (std::numeric_limits<double>::max ());
106 #endif // PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void resize(std::size_t n)
Resize the cloud.