42 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void 46 setInputSource (cloud);
53 return (getInputSource ());
57 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void 60 if (cloud->points.empty ())
62 PCL_ERROR (
"[pcl::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
66 target_cloud_updated_ =
true;
70 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
bool 75 PCL_ERROR (
"[pcl::registration::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
80 if (target_cloud_updated_ && !force_no_recompute_)
82 tree_->setInputCloud (target_);
83 target_cloud_updated_ =
false;
88 if (correspondence_estimation_)
90 correspondence_estimation_->setSearchMethodTarget (tree_, force_no_recompute_);
91 correspondence_estimation_->setSearchMethodSource (tree_reciprocal_, force_no_recompute_reciprocal_);
101 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
bool 106 PCL_ERROR (
"[pcl::registration::%s::compute] No input source dataset was given!\n", getClassName ().c_str ());
110 if (source_cloud_updated_ && !force_no_recompute_reciprocal_)
112 tree_reciprocal_->setInputCloud (input_);
113 source_cloud_updated_ =
false;
119 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline double 121 const std::vector<float> &distances_a,
122 const std::vector<float> &distances_b)
124 unsigned int nr_elem =
static_cast<unsigned int> (std::min (distances_a.size (), distances_b.size ()));
125 Eigen::VectorXf map_a = Eigen::VectorXf::Map (&distances_a[0], nr_elem);
126 Eigen::VectorXf map_b = Eigen::VectorXf::Map (&distances_b[0], nr_elem);
127 return (static_cast<double> ((map_a - map_b).sum ()) / static_cast<double> (nr_elem));
131 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline double 135 double fitness_score = 0.0;
141 std::vector<int> nn_indices (1);
142 std::vector<float> nn_dists (1);
146 for (
size_t i = 0; i < input_transformed.
points.size (); ++i)
149 tree_->nearestKSearch (input_transformed.
points[i], 1, nn_indices, nn_dists);
152 if (nn_dists[0] <= max_range)
155 fitness_score += nn_dists[0];
161 return (fitness_score / nr);
163 return (std::numeric_limits<double>::max ());
168 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void 171 align (output, Matrix4::Identity ());
175 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void 182 if (output.
points.size () != indices_->size ())
183 output.
points.resize (indices_->size ());
185 output.
header = input_->header;
187 if (indices_->size () != input_->points.size ())
189 output.
width =
static_cast<uint32_t
> (indices_->size ());
194 output.
width =
static_cast<uint32_t
> (input_->width);
195 output.
height = input_->height;
200 for (
size_t i = 0; i < indices_->size (); ++i)
201 output.
points[i] = input_->points[(*indices_)[i]];
204 if (point_representation_ && !force_no_recompute_)
205 tree_->setPointRepresentation (point_representation_);
209 final_transformation_ = transformation_ = previous_transformation_ = Matrix4::Identity ();
213 for (
size_t i = 0; i < indices_->size (); ++i)
214 output.
points[i].data[3] = 1.0;
216 computeTransformation (output, guess);
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
void align(PointCloudSource &output)
Call the registration algorithm which estimates the transformation and returns the transformed source...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void setInputCloud(const PointCloudSourceConstPtr &cloud)
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target) ...
PointCloudSourceConstPtr const getInputCloud()
Get a pointer to the input point cloud dataset target.
PointCloudSource::ConstPtr PointCloudSourceConstPtr
double getFitnessScore(double max_range=std::numeric_limits< double >::max())
Obtain the Euclidean fitness score (e.g., sum of squared distances from the source to the target) ...
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...
uint32_t height
The point cloud height (if organized as an image-structure).
Eigen::Matrix< Scalar, 4, 4 > Matrix4
uint32_t width
The point cloud width (if organized as an image-structure).
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
bool initCompute()
Internal computation initalization.
pcl::PCLHeader header
The point cloud header.
PointCloudTarget::ConstPtr PointCloudTargetConstPtr
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).