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)[i];
66 PointTarget &tgt = input_transformed[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 (
const auto& point: input_transformed)
87 tree_->nearestKSearch (point, 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_
void resize(std::size_t count)
Resizes the container to contain count elements.