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;
140 input_transformed.
resize (input_->size ());
141 for (
size_t i = 0; i < input_->size (); ++i)
143 const PointSource &src = input_->points[i];
144 PointTarget &tgt = input_transformed.
points[i];
145 tgt.x =
static_cast<float> (final_transformation_ (0, 0) * src.x + final_transformation_ (0, 1) * src.y + final_transformation_ (0, 2) * src.z + final_transformation_ (0, 3));
146 tgt.y =
static_cast<float> (final_transformation_ (1, 0) * src.x + final_transformation_ (1, 1) * src.y + final_transformation_ (1, 2) * src.z + final_transformation_ (1, 3));
147 tgt.z =
static_cast<float> (final_transformation_ (2, 0) * src.x + final_transformation_ (2, 1) * src.y + final_transformation_ (2, 2) * src.z + final_transformation_ (2, 3));
150 std::vector<int> nn_indices (1);
151 std::vector<float> nn_dists (1);
155 for (
size_t i = 0; i < input_transformed.
points.size (); ++i)
157 Eigen::Vector4f p1 = Eigen::Vector4f (input_transformed.
points[i].x,
158 input_transformed.
points[i].y,
159 input_transformed.
points[i].z, 0);
161 tree_->nearestKSearch (input_transformed.
points[i], 1, nn_indices, nn_dists);
164 if (nn_dists[0] > max_range)
167 Eigen::Vector4f p2 = Eigen::Vector4f (target_->points[nn_indices[0]].x,
168 target_->points[nn_indices[0]].y,
169 target_->points[nn_indices[0]].z, 0);
171 fitness_score += fabs ((p1-p2).squaredNorm ());
176 return (fitness_score / nr);
178 return (std::numeric_limits<double>::max ());
183 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
186 align (output, Matrix4::Identity ());
190 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
197 if (output.
points.size () != indices_->size ())
198 output.
points.resize (indices_->size ());
200 output.
header = input_->header;
202 if (indices_->size () != input_->points.size ())
204 output.
width =
static_cast<uint32_t
> (indices_->size ());
209 output.
width =
static_cast<uint32_t
> (input_->width);
210 output.
height = input_->height;
215 for (
size_t i = 0; i < indices_->size (); ++i)
216 output.
points[i] = input_->points[(*indices_)[i]];
219 if (point_representation_ && !force_no_recompute_)
220 tree_->setPointRepresentation (point_representation_);
224 final_transformation_ = transformation_ = previous_transformation_ = Matrix4::Identity ();
228 for (
size_t i = 0; i < indices_->size (); ++i)
229 output.
points[i].data[3] = 1.0;
231 computeTransformation (output, guess);