Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
pairwise_graph_registration.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41#ifndef PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_
42#define PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_
43
44namespace pcl {
45
46template <typename GraphT, typename PointT>
47void
48PairwiseGraphRegistration<GraphT, PointT>::computeRegistration()
49{
50 if (!registration_method_) {
51 PCL_ERROR("[pcl::PairwiseGraphRegistration::computeRegistration] No registration "
52 "method set!\n");
53 return;
54 }
55
56 typename std::vector<GraphHandlerVertex>::iterator last_vx_it =
57 last_vertices_.begin();
58 if (last_aligned_vertex_ == boost::graph_traits<GraphT>::null_vertex()) {
59 last_aligned_vertex_ = *last_vx_it;
60 ++last_vx_it;
61 }
62
63 pcl::PointCloud<PointT> fake_cloud;
64 registration_method_->setInputTarget(
65 boost::get_cloud<PointT>(last_aligned_vertex_, *(graph_handler_->getGraph())));
66 for (; last_vx_it < last_vertices_.end(); ++last_vx_it) {
67 registration_method_->setInputCloud(
68 boost::get_cloud<PointT>(*last_vx_it, *(graph_handler_->getGraph())));
69
70 const Eigen::Matrix4f last_aligned_vertex_pose =
71 boost::get_pose(last_aligned_vertex_, *(graph_handler_->getGraph()));
72 if (!incremental_) {
73 const Eigen::Matrix4f guess =
74 last_aligned_vertex_pose.transpose() *
75 boost::get_pose(*last_vx_it, *(graph_handler_->getGraph()));
76 registration_method_->align(fake_cloud, guess);
77 }
78 else
79 registration_method_->align(fake_cloud);
80
81 const Eigen::Matrix4f global_ref_final_tr =
82 last_aligned_vertex_pose * registration_method_->getFinalTransformation();
83 boost::set_estimate<PointT>(
84 *last_vx_it, global_ref_final_tr, *(graph_handler_->getGraph()));
85 last_aligned_vertex_ = *last_vx_it;
86 registration_method_->setInputTarget(
87 boost::get_cloud<PointT>(last_aligned_vertex_, *(graph_handler_->getGraph())));
88 }
89}
90
91} // namespace pcl
92
93#endif // PCL_REGISTRATION_IMPL_PAIRWISE_GRAPH_REGISTRATION_HPP_
PointCloud represents the base class in PCL for storing collections of 3D points.