default_convergence_criteria.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2012-, 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.
 *
 * $Id$
 *
 */

#ifndef PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_H_
#define PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_H_

#include <pcl/registration/eigen.h>
#include <pcl/correspondence.h>
#include <pcl/registration/convergence_criteria.h>

namespace pcl
{
  namespace registration
  {
    template <typename Scalar = float>
    class DefaultConvergenceCriteria : public ConvergenceCriteria
    {
      public:
        typedef boost::shared_ptr<DefaultConvergenceCriteria<Scalar> > Ptr;
        typedef boost::shared_ptr<const DefaultConvergenceCriteria<Scalar> > ConstPtr;

        typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;

        enum ConvergenceState
        {
          CONVERGENCE_CRITERIA_NOT_CONVERGED,
          CONVERGENCE_CRITERIA_ITERATIONS,
          CONVERGENCE_CRITERIA_TRANSFORM,
          CONVERGENCE_CRITERIA_ABS_MSE,
          CONVERGENCE_CRITERIA_REL_MSE,
          CONVERGENCE_CRITERIA_NO_CORRESPONDENCES
        };

        DefaultConvergenceCriteria (const int &iterations, const Matrix4 &transform, const pcl::Correspondences &correspondences)
          : iterations_ (iterations)
          , transformation_ (transform)
          , correspondences_ (correspondences)
          , correspondences_prev_mse_ (std::numeric_limits<double>::max ())
          , correspondences_cur_mse_ (std::numeric_limits<double>::max ())
          , max_iterations_ (100)                 // 100 iterations
          , failure_after_max_iter_ (false)
          , rotation_threshold_ (0.99999)         // 0.256 degrees
          , translation_threshold_ (3e-4 * 3e-4)  // 0.0003 meters
          , mse_threshold_relative_ (0.00001)     // 0.001% of the previous MSE (relative error)
          , mse_threshold_absolute_ (1e-12)       // MSE (absolute error)
          , iterations_similar_transforms_ (0)
          , max_iterations_similar_transforms_ (0)
          , convergence_state_ (CONVERGENCE_CRITERIA_NOT_CONVERGED)
        {
        }
      
        virtual ~DefaultConvergenceCriteria () {}

        inline void
        setMaximumIterationsSimilarTransforms (const int nr_iterations) { max_iterations_similar_transforms_ = nr_iterations; }

        inline int
        getMaximumIterationsSimilarTransforms () const { return (max_iterations_similar_transforms_); }

        inline void
        setMaximumIterations (const int nr_iterations) { max_iterations_ = nr_iterations; }

        inline int
        getMaximumIterations () const { return (max_iterations_); }

        inline void
        setFailureAfterMaximumIterations (const bool failure_after_max_iter) { failure_after_max_iter_ = failure_after_max_iter; }

        inline bool
        getFailureAfterMaximumIterations () const { return (failure_after_max_iter_); }

        inline void
        setRotationThreshold (const double threshold) { rotation_threshold_ = threshold; }

        inline double
        getRotationThreshold () const { return (rotation_threshold_); }

        inline void
        setTranslationThreshold (const double threshold) { translation_threshold_ = threshold; }

        inline double
        getTranslationThreshold () const { return (translation_threshold_); }

        inline void
        setRelativeMSE (const double mse_relative) { mse_threshold_relative_ = mse_relative; }

        inline double
        getRelativeMSE () const { return (mse_threshold_relative_); }

        inline void
        setAbsoluteMSE (const double mse_absolute) { mse_threshold_absolute_ = mse_absolute; }

        inline double
        getAbsoluteMSE () const { return (mse_threshold_absolute_); }


        virtual bool
        hasConverged ();

        ConvergenceState
        getConvergenceState ()
        {
          return (convergence_state_);
        }

        inline void
        setConvergenceState(ConvergenceState c)
        {
          convergence_state_ = c;
        }

      protected:

        inline double
        calculateMSE (const pcl::Correspondences &correspondences) const
        {
          double mse = 0;
          for (size_t i = 0; i < correspondences.size (); ++i)
            mse += correspondences[i].distance;
          mse /= double (correspondences.size ());
          return (mse);
        }

        const int &iterations_;

        const Matrix4 &transformation_;

        const pcl::Correspondences &correspondences_;

        double correspondences_prev_mse_;

        double correspondences_cur_mse_;

        int max_iterations_;

        bool failure_after_max_iter_;

        double rotation_threshold_;

        double translation_threshold_;

        double mse_threshold_relative_;

        double mse_threshold_absolute_;

        int iterations_similar_transforms_;

        int max_iterations_similar_transforms_;

        ConvergenceState convergence_state_;

      public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     };
  }
}

#include <pcl/registration/impl/default_convergence_criteria.hpp>

#endif    // PCL_REGISTRATION_DEFAULT_CONVERGENCE_CRITERIA_H_