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/SoftPosit.h ** 00010 ** Module : ICLGeom ** 00011 ** Authors: Christian Groszewski ** 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/DynMatrix.h> 00035 #include <ICLMath/FixedVector.h> 00036 #include <ICLUtils/Macros.h> 00037 #include <ICLUtils/Thread.h> 00038 #include <ICLUtils/Point32f.h> 00039 #include <ICLUtils/BasicTypes.h> 00040 00041 #ifdef ICL_HAVE_QT 00042 #include<ICLQt/DrawWidget.h> 00043 #endif 00044 00045 #ifdef WIN32 00046 #undef max 00047 #endif 00048 00049 namespace icl{ 00050 namespace geom{ 00051 class ICLGeom_API SoftPosit{ 00052 private: 00053 //M 00054 unsigned int nbWorldPts; 00055 //N 00056 unsigned int nbImagePts; 00057 00058 math::DynMatrix<icl64f> centeredImage; 00059 00060 math::DynMatrix<icl64f> distMat; 00061 00062 math::DynMatrix<icl64f> assignMat; 00063 00064 double beta; 00065 00066 double betaFinal; 00067 00068 static const double betaUpdate;// not allowed in clang = 1.05; 00069 00070 static const double betaZero;;// not allowed in clang = 0.0004; 00071 #ifdef ICL_HAVE_QT 00072 qt::ICLDrawWidget *dw; 00073 #endif 00074 math::DynMatrix<icl64f> iAdj; 00075 math::DynMatrix<icl64f> wAdj; 00076 00077 //expected or random pose 00078 //math::DynMatrix<icl64f> Q1; 00079 //expected or random pose 00080 //math::DynMatrix<icl64f> Q2; 00081 //squared distances 00082 math::DynMatrix<icl64f> d; 00083 00084 math::DynMatrix<icl64f> w; 00085 //object points 00086 std::vector<math::DynMatrix<icl64f> > P; 00087 //image points 00088 std::vector<math::DynMatrix<icl64f> > p; 00089 00090 math::DynMatrix<icl64f> L; 00091 math::DynMatrix<icl64f> invL; 00092 00093 math::DynMatrix<icl64f> U; 00094 math::DynMatrix<icl64f> s; 00095 math::DynMatrix<icl64f> V; 00096 math::DynMatrix<icl64f> svdResult; 00097 00098 math::DynMatrix<icl64f> eye2_2; 00099 00100 math::DynMatrix<icl64f> r1T; 00101 math::DynMatrix<icl64f> r2T; 00102 math::DynMatrix<icl64f> r3T; 00103 00104 math::DynMatrix<icl64f> projectedU; 00105 math::DynMatrix<icl64f> projectedV; 00106 math::DynMatrix<icl64f> replicatedProjectedU; 00107 math::DynMatrix<icl64f> replicatedProjectedV; 00108 00109 math::DynMatrix<icl64f> col1; 00110 math::DynMatrix<icl64f> wkxj; 00111 math::DynMatrix<icl64f> col2; 00112 math::DynMatrix<icl64f> wkyj; 00113 00114 math::DynMatrix<icl64f> pts2d; 00115 00116 math::DynMatrix<icl64f> summedByColAssign; 00117 00118 00119 math::DynMatrix<icl64f> weightedUi; 00120 math::DynMatrix<icl64f> weightedVi; 00121 00122 math::DynMatrix<icl64f> R1; 00123 math::DynMatrix<icl64f> R2; 00124 math::DynMatrix<icl64f> R3; 00125 00126 math::DynMatrix<icl64f> ROT; 00127 00128 math::DynMatrix<icl64f> T; 00129 double Tz; 00130 double Tx; 00131 double Ty; 00132 00133 double sumNonslack; 00134 00135 double sum; 00136 00137 double alpha; 00138 00139 bool draw; 00140 00141 math::DynMatrix<icl64f>& cross(math::DynMatrix<icl64f> &x, math::DynMatrix<icl64f> &y, math::DynMatrix<icl64f> &r); 00142 00143 void maxPosRatio(math::DynMatrix<icl64f> &assignMat, math::DynMatrix<icl64f> &pos, math::DynMatrix<icl64f> &ratios); 00144 00145 math::DynMatrix<icl64f> &sinkhornImp(math::DynMatrix<icl64f> &M); 00146 00147 double cond(math::DynMatrix<icl64f> &A); 00148 00149 double max(math::DynMatrix<icl64f> s); 00150 00151 00152 public: 00153 SoftPosit(); 00154 00155 ~SoftPosit(); 00156 00157 void init(); 00158 00159 math::DynMatrix<icl64f> getRotationMat(){ 00160 return ROT; 00161 } 00162 00163 math::DynMatrix<icl64f> getTranslation(){ 00164 return T; 00165 } 00166 00167 //unused 00168 int numMatches(math::DynMatrix<icl64f> &assignMat); 00169 00170 void softPosit(math::DynMatrix<icl64f> imagePts, math::DynMatrix<icl64f> worldPts, double beta0, int noiseStd, math::DynMatrix<icl64f> initRot, 00171 math::DynMatrix<icl64f> initTrans, double focalLength, math::DynMatrix<icl64f> center = math::DynMatrix<icl64f>(2,0), bool draw = true); 00172 #ifdef ICL_HAVE_QT 00173 void softPosit(math::DynMatrix<icl64f> imagePts, math::DynMatrix<icl64f> imageAdj, math::DynMatrix<icl64f> worldPts, 00174 math::DynMatrix<icl64f> worldAdj, double beta0, int noiseStd, math::DynMatrix<icl64f> initRot, 00175 math::DynMatrix<icl64f> initTrans, double focalLength, qt::ICLDrawWidget &w, 00176 math::DynMatrix<icl64f> center = math::DynMatrix<icl64f>(2,0), bool draw = true); 00177 #endif 00178 void softPosit(std::vector<utils::Point32f> imagePts, std::vector<math::FixedColVector<double,3> > worldPts, 00179 double beta0, int noiseStd, math::DynMatrix<icl64f> initRot, math::DynMatrix<icl64f> initTrans, 00180 double focalLength, math::DynMatrix<icl64f> center = math::DynMatrix<icl64f>(2,0)); 00181 #ifdef ICL_HAVE_QT 00182 void softPosit(std::vector<utils::Point32f> imagePts, math::DynMatrix<icl64f> imageAdj, std::vector<math::FixedColVector<double,3> > worldPts, 00183 math::DynMatrix<icl64f> worldAdj, double beta0, int noiseStd, math::DynMatrix<icl64f> initRot, 00184 math::DynMatrix<icl64f> initTrans, double focalLength, qt::ICLDrawWidget &w, 00185 math::DynMatrix<icl64f> center = math::DynMatrix<icl64f>(2,0), bool draw=true); 00186 #endif 00187 void proj3dto2d(math::DynMatrix<icl64f> pts3d, math::DynMatrix<icl64f> &rot, math::DynMatrix<icl64f> &trans, 00188 double flength, int objdim, math::DynMatrix<icl64f> ¢er, math::DynMatrix<icl64f> &pts2d); 00189 00190 bool isNullMatrix(const math::DynMatrix<icl64f> &M){ 00191 bool isNull = true; 00192 for(unsigned int i=0;i<M.cols();++i){ 00193 for(unsigned int j=0;j<M.rows();++j){ 00194 if(M[i+j*M.cols()] != 0){ 00195 isNull = false; 00196 return isNull; 00197 } 00198 } 00199 } 00200 return isNull; 00201 } 00202 00203 #ifdef ICL_HAVE_QT 00204 void visualize(const math::DynMatrix<icl64f> & imagePts, const math::DynMatrix<icl64f> &projWorldPts, unsigned int delay=200); 00205 00206 void visualize(qt::ICLDrawWidget &w,const math::DynMatrix<icl64f> & imagePts, const math::DynMatrix<icl64f> &imageAdj, 00207 const math::DynMatrix<icl64f> &projWorldPts, const math::DynMatrix<icl64f> &worldAdj, unsigned int delay=200); 00208 #endif 00209 }; 00210 } // namespace geom 00211 } 00212