42 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
45 if (cloud->points.empty ())
47 PCL_ERROR (
"[pcl::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
51 target_cloud_updated_ =
true;
55 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
bool
60 PCL_ERROR (
"[pcl::registration::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
65 if (target_cloud_updated_ && !force_no_recompute_)
67 tree_->setInputCloud (target_);
68 target_cloud_updated_ =
false;
73 if (correspondence_estimation_)
75 correspondence_estimation_->setSearchMethodTarget (tree_, force_no_recompute_);
76 correspondence_estimation_->setSearchMethodSource (tree_reciprocal_, force_no_recompute_reciprocal_);
86 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
bool
91 PCL_ERROR (
"[pcl::registration::%s::compute] No input source dataset was given!\n", getClassName ().c_str ());
95 if (source_cloud_updated_ && !force_no_recompute_reciprocal_)
97 tree_reciprocal_->setInputCloud (input_);
98 source_cloud_updated_ =
false;
104 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline double
106 const std::vector<float> &distances_a,
107 const std::vector<float> &distances_b)
109 unsigned int nr_elem =
static_cast<unsigned int> (std::min (distances_a.size (), distances_b.size ()));
110 Eigen::VectorXf map_a = Eigen::VectorXf::Map (&distances_a[0], nr_elem);
111 Eigen::VectorXf map_b = Eigen::VectorXf::Map (&distances_b[0], nr_elem);
112 return (
static_cast<double> ((map_a - map_b).sum ()) /
static_cast<double> (nr_elem));
116 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline double
120 double fitness_score = 0.0;
126 std::vector<int> nn_indices (1);
127 std::vector<float> nn_dists (1);
131 for (std::size_t i = 0; i < input_transformed.
points.size (); ++i)
134 tree_->nearestKSearch (input_transformed.
points[i], 1, nn_indices, nn_dists);
137 if (nn_dists[0] <= max_range)
140 fitness_score += nn_dists[0];
146 return (fitness_score / nr);
147 return (std::numeric_limits<double>::max ());
152 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
155 align (output, Matrix4::Identity ());
159 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
inline void
166 if (output.
points.size () != indices_->size ())
167 output.
points.resize (indices_->size ());
169 output.
header = input_->header;
171 if (indices_->size () != input_->points.size ())
179 output.
height = input_->height;
184 for (std::size_t i = 0; i < indices_->size (); ++i)
185 output.
points[i] = input_->points[(*indices_)[i]];
188 if (point_representation_ && !force_no_recompute_)
189 tree_->setPointRepresentation (point_representation_);
193 final_transformation_ = transformation_ = previous_transformation_ = Matrix4::Identity ();
197 for (std::size_t i = 0; i < indices_->size (); ++i)
198 output.
points[i].data[3] = 1.0;
200 computeTransformation (output, guess);