40 #ifndef PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_
41 #define PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_
43 template <
typename GraphT,
typename Po
intT>
void
46 if (!registration_method_)
48 PCL_ERROR (
"[pcl::PairwiseGraphRegistration::computeRegistration] No registration method set!\n");
52 typename std::vector<GraphHandlerVertex>::iterator last_vx_it = last_vertices_.begin ();
53 if (last_aligned_vertex_ == boost::graph_traits<GraphT>::null_vertex ())
55 last_aligned_vertex_ = *last_vx_it;
60 registration_method_->setInputTarget (boost::get_cloud<PointT> (last_aligned_vertex_, *(graph_handler_->getGraph ())));
61 for(; last_vx_it < last_vertices_.end (); ++last_vx_it)
63 registration_method_->setInputCloud (boost::get_cloud<PointT> (*last_vx_it, *(graph_handler_->getGraph ())));
65 const Eigen::Matrix4f last_aligned_vertex_pose = boost::get_pose (last_aligned_vertex_, *(graph_handler_->getGraph ()));
68 const Eigen::Matrix4f guess = last_aligned_vertex_pose.transpose () * boost::get_pose (*last_vx_it, *(graph_handler_->getGraph ()));
69 registration_method_->align (fake_cloud, guess);
71 registration_method_->align (fake_cloud);
73 const Eigen::Matrix4f global_ref_final_tr = last_aligned_vertex_pose * registration_method_->getFinalTransformation ();
74 boost::set_estimate<PointT> (*last_vx_it, global_ref_final_tr, *(graph_handler_->getGraph ()));
75 last_aligned_vertex_ = *last_vx_it;
76 registration_method_->setInputTarget (boost::get_cloud<PointT> (last_aligned_vertex_, *(graph_handler_->getGraph ())));
79 #endif //PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_
PairwiseGraphRegistration class aligns the clouds two by two