PoseEstimator.h

Go to the documentation of this file.

/********************************************************************
**                Image Component Library (ICL)                    **
**                                                                 **
** Copyright (C) 2006-2013 CITEC, University of Bielefeld          **
**                         Neuroinformatics Group                  **
** Website: www.iclcv.org and                                      **
**          http://opensource.cit-ec.de/projects/icl               **
**                                                                 **
** File   : ICLGeom/src/ICLGeom/PoseEstimator.h                    **
** Module : ICLGeom                                                **
** Authors: Christof Elbrechter, Robert Haschke                    **
**                                                                 **
**                                                                 **
** GNU LESSER GENERAL PUBLIC LICENSE                               **
** This file may be used under the terms of the GNU Lesser General **
** Public License version 3.0 as published by the                  **
**                                                                 **
** Free Software Foundation and appearing in the file LICENSE.LGPL **
** included in the packaging of this file.  Please review the      **
** following information to ensure the license requirements will   **
** be met: http://www.gnu.org/licenses/lgpl-3.0.txt                **
**                                                                 **
** The development of this software was supported by the           **
** Excellence Cluster EXC 277 Cognitive Interaction Technology.    **
** The Excellence Cluster EXC 277 is a grant of the Deutsche       **
** Forschungsgemeinschaft (DFG) in the context of the German       **
** Excellence Initiative.                                          **
**                                                                 **
********************************************************************/

#pragma once

#include <ICLUtils/CompatMacros.h>
#include <ICLGeom/GeomDefs.h>

namespace icl{
  namespace geom{



    class PoseEstimator{

      PoseEstimator(){}

      public:
      enum MapMode {
        Translation,  
        RigidBody,    
        Similarity,   
        Affine        
      };


      template<class T> ICLGeom_API
      static math::FixedMatrix<T,4,4> map(const math::DynMatrix<T> &Xs, const math::DynMatrix<T> &Ys, MapMode mode=RigidBody)
      ;


      template<class T, unsigned int NUM_POINTS>
      static math::FixedMatrix<T,4,4> map(const math::FixedMatrix<T,NUM_POINTS,3> &Xs,
                                          const math::FixedMatrix<T,NUM_POINTS,3> &Ys, MapMode mode=RigidBody)
        {
        return map(Xs.dyn(),Ys.dyn(), mode);
      }


      template<class T, unsigned int NUM_POINTS>
      static math::FixedMatrix<T,4,4> map(const math::FixedMatrix<T,NUM_POINTS,4> &Xs,
                                    const math::FixedMatrix<T,NUM_POINTS,4> &Ys, MapMode mode=RigidBody)
        {
        return map(Xs.dyn(),Ys.dyn(), mode);
      }

      static Mat map(const std::vector<Vec> &Xs, const std::vector<Vec> &Ys, MapMode mode=RigidBody)
      {
        ICLASSERT_THROW(Xs.size() == Ys.size(), utils::ICLException("PoseEstimator::map: need same number of input- and output-points"));
        math::DynMatrix<double> XsD(Xs.size(),3),YsD(Ys.size(),3);
        for(unsigned int i=0;i<Xs.size();++i){
          std::copy(Xs[i].begin(),Xs[i].begin()+3, XsD.col_begin(i));
          std::copy(Ys[i].begin(),Ys[i].begin()+3, YsD.col_begin(i));
        }
        return map(XsD,YsD,mode);
      }

      template<class T> ICLGeom_API
      static math::FixedMatrix<T,3,3> quaternion_to_rotation_matrix(T w, T x, T y, T z);
    };

  } // namespace geom
}