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