diff --git a/registration/include/pcl/registration/impl/registration.hpp b/registration/include/pcl/registration/impl/registration.hpp index b246958927e..480958b72e8 100644 --- a/registration/include/pcl/registration/impl/registration.hpp +++ b/registration/include/pcl/registration/impl/registration.hpp @@ -86,32 +86,33 @@ Registration::initCompute() target_cloud_updated_ = false; } + if (!PCLBase::initCompute()) + return (false); + // Update the correspondence estimation if (correspondence_estimation_) { correspondence_estimation_->setSearchMethodTarget(tree_, force_no_recompute_); correspondence_estimation_->setSearchMethodSource(tree_reciprocal_, force_no_recompute_reciprocal_); + correspondence_estimation_->setIndicesSource(indices_); } // Note: we /cannot/ update the search method on all correspondence rejectors, because // we know nothing about them. If they should be cached, they must be cached // individually. - return (PCLBase::initCompute()); + return (true); } template bool Registration::initComputeReciprocal() { - if (!input_) { - PCL_ERROR("[pcl::registration::%s::compute] No input source dataset was given!\n", - getClassName().c_str()); + if (!PCLBase::initCompute()) return (false); - } if (source_cloud_updated_ && !force_no_recompute_reciprocal_) { - tree_reciprocal_->setInputCloud(input_); + tree_reciprocal_->setInputCloud(input_, indices_); source_cloud_updated_ = false; } return (true); diff --git a/test/registration/test_registration.cpp b/test/registration/test_registration.cpp index 0eda177b8a2..6dc8957df63 100644 --- a/test/registration/test_registration.cpp +++ b/test/registration/test_registration.cpp @@ -231,6 +231,45 @@ TEST (PCL, IterativeClosestPoint) EXPECT_EQ (transformation (3, 3), 1); } +TEST (PCL, IterativeClosestPoint_setIndices) +{ + pcl::IndicesPtr indices (new pcl::Indices); + for (std::size_t i = 0; i < cloud_source.size(); i += 2) + indices->push_back(i); + + IterativeClosestPoint reg_indices; + PointCloud::ConstPtr source (cloud_source.makeShared()); + reg_indices.setInputSource(source); + reg_indices.setInputTarget(cloud_target.makeShared()); + reg_indices.setIndices(indices); + reg_indices.setMaximumIterations(50); + reg_indices.setTransformationEpsilon(1e-8); + reg_indices.setMaxCorrespondenceDistance(0.05); + + PointCloud cloud_reg_indices; + reg_indices.align(cloud_reg_indices); + + PointCloud::Ptr source_cropped (new PointCloud); + pcl::copyPointCloud(*source, *indices, *source_cropped); + + IterativeClosestPoint reg_cropped; + reg_cropped.setInputSource(source_cropped); + reg_cropped.setInputTarget(cloud_target.makeShared()); + reg_cropped.setMaximumIterations(50); + reg_cropped.setTransformationEpsilon(1e-8); + reg_cropped.setMaxCorrespondenceDistance(0.05); + + PointCloud cloud_reg_cropped; + reg_cropped.align(cloud_reg_cropped); + + Eigen::Matrix4f trans_indices = reg_indices.getFinalTransformation(); + Eigen::Matrix4f trans_cropped = reg_cropped.getFinalTransformation(); + + for (int y = 0; y < 4; y++) + for (int x = 0; x < 4; x++) + EXPECT_NEAR(trans_indices(y, x), trans_cropped(y, x), 1e-5); +} + TEST (PCL, IterativeClosestPointWithNormals) { IterativeClosestPointWithNormals reg_float;