 |
Image Component Library (ICL)
|
|
Image Component Library (ICL)
|
00001 /******************************************************************** 00002 ** Image Component Library (ICL) ** 00003 ** ** 00004 ** Copyright (C) 2006-2013 CITEC, University of Bielefeld ** 00005 ** Neuroinformatics Group ** 00006 ** Website: www.iclcv.org and ** 00007 ** http://opensource.cit-ec.de/projects/icl ** 00008 ** ** 00009 ** File : ICLGeom/src/ICLGeom/PoseEstimator.h ** 00010 ** Module : ICLGeom ** 00011 ** Authors: Christof Elbrechter, Robert Haschke ** 00012 ** ** 00013 ** ** 00014 ** GNU LESSER GENERAL PUBLIC LICENSE ** 00015 ** This file may be used under the terms of the GNU Lesser General ** 00016 ** Public License version 3.0 as published by the ** 00017 ** ** 00018 ** Free Software Foundation and appearing in the file LICENSE.LGPL ** 00019 ** included in the packaging of this file. Please review the ** 00020 ** following information to ensure the license requirements will ** 00021 ** be met: http://www.gnu.org/licenses/lgpl-3.0.txt ** 00022 ** ** 00023 ** The development of this software was supported by the ** 00024 ** Excellence Cluster EXC 277 Cognitive Interaction Technology. ** 00025 ** The Excellence Cluster EXC 277 is a grant of the Deutsche ** 00026 ** Forschungsgemeinschaft (DFG) in the context of the German ** 00027 ** Excellence Initiative. ** 00028 ** ** 00029 ********************************************************************/ 00030 00031 #pragma once 00032 00033 #include <ICLUtils/CompatMacros.h> 00034 #include <ICLGeom/GeomDefs.h> 00035 00036 namespace icl{ 00037 namespace geom{ 00038 00039 00041 00055 class PoseEstimator{ 00056 00058 PoseEstimator(){} 00059 00060 public: 00062 enum MapMode { 00063 Translation, 00064 RigidBody, 00065 Similarity, 00066 Affine 00067 }; 00068 00070 00075 template<class T> ICLGeom_API 00076 static math::FixedMatrix<T,4,4> map(const math::DynMatrix<T> &Xs, const math::DynMatrix<T> &Ys, MapMode mode=RigidBody) 00077 throw (math::IncompatibleMatrixDimensionException,math::SingularMatrixException); 00078 00080 00081 template<class T, unsigned int NUM_POINTS> 00082 static math::FixedMatrix<T,4,4> map(const math::FixedMatrix<T,NUM_POINTS,3> &Xs, 00083 const math::FixedMatrix<T,NUM_POINTS,3> &Ys, MapMode mode=RigidBody) 00084 throw (math::IncompatibleMatrixDimensionException,math::SingularMatrixException){ 00085 return map(Xs.dyn(),Ys.dyn(), mode); 00086 } 00087 00089 00090 template<class T, unsigned int NUM_POINTS> 00091 static math::FixedMatrix<T,4,4> map(const math::FixedMatrix<T,NUM_POINTS,4> &Xs, 00092 const math::FixedMatrix<T,NUM_POINTS,4> &Ys, MapMode mode=RigidBody) 00093 throw (math::IncompatibleMatrixDimensionException,math::SingularMatrixException){ 00094 return map(Xs.dyn(),Ys.dyn(), mode); 00095 } 00096 00098 static Mat map(const std::vector<Vec> &Xs, const std::vector<Vec> &Ys, MapMode mode=RigidBody) 00099 throw (utils::ICLException,math::SingularMatrixException,math::IncompatibleMatrixDimensionException){ 00100 ICLASSERT_THROW(Xs.size() == Ys.size(), utils::ICLException("PoseEstimator::map: need same number of input- and output-points")); 00101 math::DynMatrix<double> XsD(Xs.size(),3),YsD(Ys.size(),3); 00102 for(unsigned int i=0;i<Xs.size();++i){ 00103 std::copy(Xs[i].begin(),Xs[i].begin()+3, XsD.col_begin(i)); 00104 std::copy(Ys[i].begin(),Ys[i].begin()+3, YsD.col_begin(i)); 00105 } 00106 return map(XsD,YsD,mode); 00107 } 00108 00110 template<class T> ICLGeom_API 00111 static math::FixedMatrix<T,3,3> quaternion_to_rotation_matrix(T w, T x, T y, T z); 00112 }; 00113 00114 } // namespace geom 00115 }
1.7.6.1
