Point Cloud Library (PCL)  1.9.1-dev
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 
41 template <typename PointT, typename Scalar>
43  delta_transform_ (Matrix4::Identity ()),
44  abs_transform_ (Matrix4::Identity ())
45 {}
46 
47 template <typename PointT, typename Scalar> bool
49 {
50  assert (registration_);
51 
52  if (!last_cloud_)
53  {
54  last_cloud_ = cloud;
55  abs_transform_ = delta_transform_ = delta_estimate;
56  return (true);
57  }
58 
59  registration_->setInputSource (cloud);
60  registration_->setInputTarget (last_cloud_);
61 
62  {
64  registration_->align (p, delta_estimate);
65  }
66 
67  bool converged = registration_->hasConverged ();
68 
69  if ( converged ){
70  delta_transform_ = registration_->getFinalTransformation ();
72  last_cloud_ = cloud;
73  }
74 
75  return (converged);
76 }
77 
78 template <typename PointT, typename Scalar> inline typename pcl::registration::IncrementalRegistration<PointT, Scalar>::Matrix4
80 {
81  return (delta_transform_);
82 }
83 
84 template <typename PointT, typename Scalar> inline typename pcl::registration::IncrementalRegistration<PointT, Scalar>::Matrix4
86 {
87  return (abs_transform_);
88 }
89 
90 template <typename PointT, typename Scalar> inline void
92 {
93  last_cloud_.reset ();
94  delta_transform_ = abs_transform_ = Matrix4::Identity ();
95 }
96 
97 template <typename PointT, typename Scalar> inline void
99 {
100  registration_ = registration;
101 }
102 
103 #endif /*PCL_REGISTRATION_IMPL_INCREMENTAL_REGISTRATION_HPP_*/
bool registerCloud(const PointCloudConstPtr &cloud, const Matrix4 &delta_estimate=Matrix4::Identity())
Register new point cloud incrementally.
typename pcl::Registration< PointT, PointT, Scalar >::Ptr RegistrationPtr
typename pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
PointCloudConstPtr last_cloud_
last registered point cloud
typename pcl::Registration< PointT, PointT, Scalar >::Matrix4 Matrix4
void reset()
Reset incremental Registration without resetting registration_.
RegistrationPtr registration_
registration instance to align clouds
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Matrix4 getAbsoluteTransform() const
Get estimated overall transform.
Matrix4 getDeltaTransform() const
Get estimated transform between the last two registered clouds.
void setRegistration(RegistrationPtr)
Set registration instance used to align clouds.