Add a MetaICP class

registration/CMakeLists.txt

@@ -40,6 +40,7 @@ if(build)
         "include/pcl/${SUBSYS_NAME}/icp_nl.h"
         "include/pcl/${SUBSYS_NAME}/lum.h"
         "include/pcl/${SUBSYS_NAME}/elch.h"
+        "include/pcl/${SUBSYS_NAME}/meta_registration.h"
         "include/pcl/${SUBSYS_NAME}/ndt.h"
         "include/pcl/${SUBSYS_NAME}/ndt_2d.h"
         "include/pcl/${SUBSYS_NAME}/ppf_registration.h"
@@ -98,6 +99,7 @@ if(build)
         "include/pcl/${SUBSYS_NAME}/impl/icp_nl.hpp"
         "include/pcl/${SUBSYS_NAME}/impl/elch.hpp"
         "include/pcl/${SUBSYS_NAME}/impl/lum.hpp"
+        "include/pcl/${SUBSYS_NAME}/impl/meta_registration.hpp"
         "include/pcl/${SUBSYS_NAME}/impl/ndt.hpp"
         "include/pcl/${SUBSYS_NAME}/impl/ndt_2d.hpp"
         "include/pcl/${SUBSYS_NAME}/impl/ppf_registration.hpp"

registration/include/pcl/registration/impl/meta_registration.hpp

@@ -0,0 +1,101 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2015, Michael 'v4hn' Goerner
+ *  Copyright (c) 2015-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_
+#define PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_
+
+template <typename PointT, typename Scalar>
+pcl::registration::MetaRegistration<PointT, Scalar>::MetaRegistration () :
+  abs_transform_ (Matrix4::Identity ())
+{}
+
+template <typename PointT, typename Scalar> bool
+pcl::registration::MetaRegistration<PointT, Scalar>::registerCloud (const PointCloudConstPtr& new_cloud, const Matrix4& delta_estimate)
+{
+  assert (registration_);
+
+  PointCloudPtr new_cloud_transformed (new pcl::PointCloud<PointT> ());
+
+  if (!full_cloud_)
+  {
+    pcl::transformPointCloud(*new_cloud, *new_cloud_transformed, delta_estimate);
+    full_cloud_ = new_cloud_transformed;
+    abs_transform_ = delta_estimate;
+    return (true);
+  }
+
+  registration_->setInputSource (new_cloud);
+  registration_->setInputTarget (full_cloud_);
+
+  registration_->align (*new_cloud_transformed, abs_transform_ * delta_estimate);
+
+  bool converged = registration_->hasConverged ();
+
+  if (converged)
+  {
+    abs_transform_ = registration_->getFinalTransformation ();
+    *full_cloud_ += *new_cloud_transformed;
+  }
+
+  return (converged);
+}
+
+template <typename PointT, typename Scalar> inline typename pcl::registration::MetaRegistration<PointT, Scalar>::Matrix4
+pcl::registration::MetaRegistration<PointT, Scalar>::getAbsoluteTransform () const
+{
+  return (abs_transform_);
+}
+
+template <typename PointT, typename Scalar> inline void
+pcl::registration::MetaRegistration<PointT, Scalar>::reset ()
+{
+  full_cloud_.reset ();
+  abs_transform_ = Matrix4::Identity ();
+}
+
+template <typename PointT, typename Scalar> inline void
+pcl::registration::MetaRegistration<PointT, Scalar>::setRegistration (RegistrationPtr reg)
+{
+  registration_ = reg;
+}
+
+template <typename PointT, typename Scalar> inline typename pcl::registration::MetaRegistration<PointT, Scalar>::PointCloudConstPtr
+pcl::registration::MetaRegistration<PointT, Scalar>::getMetaCloud () const
+{
+  return full_cloud_;
+}
+#endif /*PCL_REGISTRATION_IMPL_META_REGISTRATION_HPP_*/

registration/include/pcl/registration/meta_registration.h

@@ -0,0 +1,131 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2015, Michael 'v4hn' Goerner
+ *  Copyright (c) 2015-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef PCL_REGISTRATION_META_REGISTRATION_H_
+#define PCL_REGISTRATION_META_REGISTRATION_H_
+
+#include <pcl/point_cloud.h>
+#include <pcl/registration/registration.h>
+
+namespace pcl {
+  namespace registration {
+
+    /** \brief Meta @ref Registration class
+      * \note This method accumulates all registered points and becomes more costly with each registered point cloud.
+      *
+      * This class provides a way to register a stream of clouds where each cloud
+      * will be aligned to the conglomerate of all previous clouds.
+      *
+      * \code
+      * IterativeClosestPoint<PointXYZ,PointXYZ>::Ptr icp (new IterativeClosestPoint<PointXYZ,PointXYZ>);
+      * icp->setMaxCorrespondenceDistance (0.05);
+      * icp->setMaximumIterations (50);
+      *
+      * MetaRegistration<PointXYZ> mreg;
+      * mreg.setRegistration (icp);
+      *
+      * while (true)
+      * {
+      *   PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
+      *   read_cloud (*cloud);
+      *   mreg.registerCloud (cloud);
+      *
+      *   PointCloud<PointXYZ>::Ptr tmp (new PointCloud<PointXYZ>);
+      *   transformPointCloud (*cloud, *tmp, mreg.getAbsoluteTransform ());
+      *   write_cloud (*tmp);
+      * }
+      * \endcode
+      *
+      * \author Michael 'v4hn' Goerner
+      * \ingroup registration
+      */
+    template <typename PointT, typename Scalar = float>
+    class MetaRegistration {
+      public:
+        typedef typename pcl::PointCloud<PointT>::Ptr PointCloudPtr;
+        typedef typename pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr;
+
+        typedef typename pcl::Registration<PointT,PointT,Scalar>::Ptr RegistrationPtr;
+        typedef typename pcl::Registration<PointT,PointT,Scalar>::Matrix4 Matrix4;
+
+        MetaRegistration ();
+
+        /** \brief Empty destructor */
+        virtual ~MetaRegistration () {};
+
+        /** \brief Register new point cloud
+          * \note You have to set a valid registration object with @ref setICP before using this
+          * \param[in] cloud point cloud to register
+          * \param[in] delta_estimate estimated transform between last registered cloud and this one
+          * \return true if ICP converged
+          */
+        bool
+        registerCloud (const PointCloudConstPtr& cloud, const Matrix4& delta_estimate = Matrix4::Identity ());
+
+        /** \brief Get estimated transform of the last registered cloud */
+        inline Matrix4
+        getAbsoluteTransform () const;
+
+        /** \brief Reset MetaICP without resetting registration_ */
+        inline void
+        reset ();
+
+        /** \brief Set registration instance used to align clouds */
+        inline void
+        setRegistration (RegistrationPtr);
+
+        /** \brief get accumulated meta point cloud */
+        inline PointCloudConstPtr
+        getMetaCloud () const;
+      protected:
+
+        /** \brief registered accumulated point cloud */
+        PointCloudPtr full_cloud_;
+
+        /** \brief registration instance to align clouds */
+        RegistrationPtr registration_;
+
+        /** \brief estimated transform */
+        Matrix4 abs_transform_;
+    };
+
+  }
+}
+
+#include <pcl/registration/impl/meta_registration.hpp>
+
+#endif /*PCL_REGISTRATION_META_REGISTRATION_H_*/