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/SQFitter.h ** 00010 ** Module : ICLGeom ** 00011 ** Authors: Sergius Gaulik ** 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 <ICLMath/FixedMatrix.h> 00035 #include <ICLMath/LevenbergMarquardtFitter.h> 00036 #include <ICLGeom/PointCloudObject.h> 00037 #include <vector> 00038 00039 namespace icl{ 00040 namespace geom{ 00041 00043 struct ICLGeom_API SQParams { 00044 math::Vec3 size; 00045 math::Vec3 pos; 00046 math::Vec3 euler; 00047 std::vector<float> shape; 00048 00049 icl::math::FixedMatrix<float,4,4> getTransformationMatrix() const; 00050 SQParams () : shape(2, 1.0) {} 00051 }; 00052 00053 class ICLGeom_API SQFitter{ 00054 public: 00055 typedef icl::math::LevenbergMarquardtFitter<float> LM; 00056 00057 private: 00058 00060 struct shapeError { 00061 LM::Matrix operator()(const LM::Params &p, const LM::Matrix &vx); 00062 }; 00063 00065 struct eulerPosError { 00066 math::Vec3 size; 00067 std::vector<float> shape; 00068 00069 LM::Matrix operator()(const LM::Params &p, const LM::Matrix &vx); 00070 }; 00071 00072 public: 00074 SQFitter(icl::utils::SmartPtr<Vec> camCenter=utils::SmartPtr<Vec>()); 00075 00077 bool fit(icl::geom::PointCloudObject& pcObj, 00078 const std::string& sShapePreference="bsc", 00079 size_t maxNumPoints=0); 00080 00082 const SQParams& getParams() const {return params;} 00083 00085 void setCameraCenter(const Vec &c); 00086 00087 private: 00089 void fitSQ(LM::Matrix &Mx, math::Vec3 &viewDir, const std::string& sShapePreference); 00091 void preProcess(LM::Matrix &Mx, math::Vec3 &viewDir, 00092 const std::string& sShapePreference, 00093 icl::math::FixedMatrix<float,3,3> &R, 00094 math::Vec3 ¢er, math::Vec3 &size, math::Vec3 &origin, 00095 float &scale); 00097 LM::Result fitShape(int i, icl::math::FixedMatrix<float,3,3> &R, 00098 LM::Matrix &Mx, SQParams ¶ms, 00099 math::Vec3 &size, math::Vec3 &euler); 00100 00101 private: 00102 SQFitter::shapeError sError; 00103 SQFitter::eulerPosError ePError; 00104 LM::FunctionMat sErrorFunc; 00105 LM::FunctionMat ePErrorFunc; 00106 LM shapeLM; 00107 LM eulerPosLM; 00108 SQParams params; 00109 icl::utils::SmartPtr<Vec> camCenter; 00110 }; 00111 typedef icl::utils::SmartPtr<SQFitter> SQFitterPtr; 00112 00113 } // namespace geom 00114 }