Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
incremental_registration.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2015, Michael 'v4hn' Goerner
6 * Copyright (c) 2015-, 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
38#ifndef PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_
39#define PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_
40
41namespace pcl {
42
43namespace registration {
44
45template <typename PointT, typename Scalar>
47: delta_transform_(Matrix4::Identity()), abs_transform_(Matrix4::Identity())
48{}
49
50template <typename PointT, typename Scalar>
51bool
53 const Matrix4& delta_estimate)
54{
55 assert(registration_);
56
57 if (!last_cloud_) {
58 last_cloud_ = cloud;
59 abs_transform_ = delta_transform_ = delta_estimate;
60 return (true);
61 }
62
63 registration_->setInputSource(cloud);
64 registration_->setInputTarget(last_cloud_);
65
66 {
68 registration_->align(p, delta_estimate);
69 }
70
71 bool converged = registration_->hasConverged();
72
73 if (converged) {
74 delta_transform_ = registration_->getFinalTransformation();
75 abs_transform_ *= delta_transform_;
76 last_cloud_ = cloud;
77 }
78
79 return (converged);
80}
81
82template <typename PointT, typename Scalar>
85{
86 return (delta_transform_);
87}
88
89template <typename PointT, typename Scalar>
92{
93 return (abs_transform_);
94}
95
96template <typename PointT, typename Scalar>
97inline void
99{
100 last_cloud_.reset();
101 delta_transform_ = abs_transform_ = Matrix4::Identity();
102}
103
104template <typename PointT, typename Scalar>
105inline void
107{
108 registration_ = registration;
109}
110
111} // namespace registration
112} // namespace pcl
113
114#endif /*PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_*/
PointCloud represents the base class in PCL for storing collections of 3D points.
void setRegistration(RegistrationPtr)
Set registration instance used to align clouds.
bool registerCloud(const PointCloudConstPtr &cloud, const Matrix4 &delta_estimate=Matrix4::Identity())
Register new point cloud incrementally.
Matrix4 getDeltaTransform() const
Get estimated transform between the last two registered clouds.
Matrix4 getAbsoluteTransform() const
Get estimated overall transform.
typename pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
typename pcl::Registration< PointT, PointT, Scalar >::Ptr RegistrationPtr
void reset()
Reset incremental Registration without resetting registration_.
typename pcl::Registration< PointT, PointT, Scalar >::Matrix4 Matrix4