40 #ifndef PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_ 41 #define PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_ 44 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
double 48 const Matrix4 &transformation_matrix)
const 50 double fitness_score = 0.0;
55 input_transformed.
resize (cloud_src->size ());
56 for (
size_t i = 0; i < cloud_src->size (); ++i)
58 const PointSource &src = cloud_src->points[i];
59 PointTarget &tgt = input_transformed.
points[i];
60 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));
61 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));
62 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));
66 if (!force_no_recompute_)
68 tree_->setPointRepresentation (point_rep);
69 tree_->setInputCloud (cloud_tgt);
72 std::vector<int> nn_indices (1);
73 std::vector<float> nn_dists (1);
77 for (
size_t i = 0; i < input_transformed.
points.size (); ++i)
80 tree_->nearestKSearch (input_transformed.
points[i], 1, nn_indices, nn_dists);
83 if (nn_dists[0] > max_range_)
87 fitness_score += nn_dists[0];
92 return (fitness_score / nr);
94 return (std::numeric_limits<double>::max ());
97 #endif // PCL_REGISTRATION_TRANSFORMATION_VALIDATION_EUCLIDEAN_IMPL_H_ std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void resize(size_t n)
Resize the cloud.