45 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
50 if (cloud->points.empty()) {
51 PCL_ERROR(
"[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n",
52 getClassName().c_str());
55 source_cloud_updated_ =
true;
59 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
64 if (cloud->points.empty()) {
65 PCL_ERROR(
"[pcl::%s::setInputTarget] Invalid or empty point cloud dataset given!\n",
66 getClassName().c_str());
70 target_cloud_updated_ =
true;
73 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
78 PCL_ERROR(
"[pcl::registration::%s::compute] No input target dataset was given!\n",
79 getClassName().c_str());
84 if (target_cloud_updated_ && !force_no_recompute_) {
85 tree_->setInputCloud(target_);
86 target_cloud_updated_ =
false;
90 if (correspondence_estimation_) {
91 correspondence_estimation_->setSearchMethodTarget(tree_, force_no_recompute_);
92 correspondence_estimation_->setSearchMethodSource(tree_reciprocal_,
93 force_no_recompute_reciprocal_);
103 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
108 PCL_ERROR(
"[pcl::registration::%s::compute] No input source dataset was given!\n",
109 getClassName().c_str());
113 if (source_cloud_updated_ && !force_no_recompute_reciprocal_) {
114 tree_reciprocal_->setInputCloud(input_);
115 source_cloud_updated_ =
false;
120 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
123 const std::vector<float>& distances_a,
const std::vector<float>& distances_b)
125 unsigned int nr_elem =
126 static_cast<unsigned int>(std::min(distances_a.size(), distances_b.size()));
127 Eigen::VectorXf map_a = Eigen::VectorXf::Map(distances_a.data(), nr_elem);
128 Eigen::VectorXf map_b = Eigen::VectorXf::Map(distances_b.data(), nr_elem);
129 return (
static_cast<double>((map_a - map_b).sum()) /
static_cast<double>(nr_elem));
132 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
136 double fitness_score = 0.0;
143 std::vector<float> nn_dists(1);
147 for (
const auto& point : input_transformed) {
149 tree_->nearestKSearch(point, 1, nn_indices, nn_dists);
152 if (nn_dists[0] <= max_range) {
154 fitness_score += nn_dists[0];
160 return (fitness_score / nr);
161 return (std::numeric_limits<double>::max());
164 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
168 align(output, Matrix4::Identity());
171 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
180 output.
resize(indices_->size());
182 output.
header = input_->header;
184 if (indices_->size() != input_->size()) {
185 output.
width = indices_->size();
189 output.
width =
static_cast<std::uint32_t
>(input_->width);
190 output.
height = input_->height;
195 for (std::size_t i = 0; i < indices_->size(); ++i)
196 output[i] = (*input_)[(*indices_)[i]];
199 if (point_representation_ && !force_no_recompute_)
200 tree_->setPointRepresentation(point_representation_);
204 final_transformation_ = transformation_ = previous_transformation_ =
209 for (std::size_t i = 0; i < indices_->size(); ++i)
210 output[i].data[3] = 1.0;
212 computeTransformation(output, guess);
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
bool initCompute()
Internal computation initialization.
virtual void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
virtual void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Eigen::Matrix< Scalar, 4, 4 > Matrix4
void align(PointCloudSource &output)
Call the registration algorithm which estimates the transformation and returns the transformed source...
double getFitnessScore(double max_range=std::numeric_limits< double >::max())
Obtain the Euclidean fitness score (e.g., mean of squared distances from the source to the target)
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
IndicesAllocator<> Indices
Type used for indices in PCL.