RelPose.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2009 by Ulrich Friedrich Klank <klank@in.tum.de>
00003  *
00004  * This program is free software; you can redistribute it and/or modify
00005  * it under the terms of the GNU General Public License as published by
00006  * the Free Software Foundation; either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 
00019 /************************************************************************
00020                         RelPose.cpp - Copyright klank
00021 **************************************************************************/
00022 
00023 #include "RelPose.h"
00024 #include "XMLTag.h"
00025 #include "Object.h"
00026 
00027 // Constructors/Destructors
00028 //
00029 
00030 using namespace cop;
00031 
00032 
00033 void TransformPointLocally(Matrix m, double x_in, double y_in, double z_in, double &x_out,double &y_out,double &z_out, double scale = 1.0)
00034 {
00035   ColumnVector d(4);
00036   d << x_in / scale << y_in  / scale << z_in / scale<< 1;
00037   ColumnVector f = m * d;
00038 //  cout << m << " \n* \n" << d << "\n=\n" << f;
00039   x_out = f.element(0);
00040   y_out = f.element(1);
00041   z_out = f.element(2);
00042 
00043 }
00044 
00045 ReturnMatrix GetExtremePoses(const Matrix& cov)
00046 {
00047   Matrix m(6, 12);
00048   try
00049   {
00050 
00051   int i = cov.nrows() * cov.ncols() ;
00052   if(i != 0 && !cov.is_zero())
00053   {
00054     memset((Real*)m.Store(), 0, sizeof(Real) * 72);
00055     DiagonalMatrix eigenValues;
00056     Matrix eigenVectors, V;
00057             //Decompose Covariance
00058     SVD(cov, eigenValues, eigenVectors, V, false);
00059     ColumnVector ExMax(6), ExMin(6), Diff(6);
00060     Real EigVal;
00061     for(int i = 1; i <= 6; i++)
00062     {
00063       Diff = eigenVectors.column(i);
00064       EigVal = eigenValues.element(i-1);
00065       for (int j = 0; j<6; j++)
00066       {
00067           Diff.element(j)*=EigVal;
00068       }
00069       ExMax = Diff; // transposed i-th eigenvector
00070       ExMin = Diff*(-1);
00071       m.column(2*i-1)=ExMax;
00072       m.column(2*i)=ExMin;
00073     }
00074     m.release();
00075     return m;
00076   }
00077   }catch(...)
00078   {
00079    printf("Big problem in Extreme Poses\n");
00080   }
00081   return m;
00082 }
00083 
00084 
00085 void cop::RelPoseToRect(RelPose* pose, LocatedObjectID_t relation, MinimalCalibration* calib, int &r1, int &c1, int &r2, int &c2)
00086 {
00087   int  max_r = 0, max_c = 0, min_r = calib->height, min_c = calib->width;
00088   try
00089   {
00090     Matrix extreme = GetExtremePoses(pose->GetCovariance(relation));
00091     Matrix m  = pose->GetMatrix(relation);
00092     for(int vp=0; vp<12;vp++)
00093     {
00094        double x = extreme.element(0, vp);
00095        double y = extreme.element(1, vp);
00096        double z = extreme.element(2, vp);
00097        double  xt, yt, zt, row, column;
00098 
00099        TransformPointLocally(m, x,y,z, xt, yt, zt);
00100        calib->Project3DPoint(xt, yt, zt, row, column);
00101        if(max_r < row && row < calib->height)
00102          max_r = row;
00103        if(min_r > row && row > 0)
00104          min_r  = row;
00105        if(max_c < column && column < calib->width)
00106          max_c = column;
00107        if(min_c > column && column > 0)
00108          min_c  = column;
00109     }
00110   }
00111   catch(const char* text)
00112   {
00113     printf("Error  in RelPoseToRect: %s\n", text);
00114     throw "RelPoseToRect: Error creating search space rectangle";
00115   }
00116   catch(...)
00117   {
00118     throw "RelPoseToRect: Error creating search space rectangle";
00119   }
00120 #ifndef MIN
00121 #define MIN(a,b)  ((a) > (b) ? (b) : (a))
00122 #endif
00123 
00124 #ifndef MAX
00125 #define MAX(a,b)  ((a) < (b) ? (b) : (a))
00126 #endif
00127   c1 = MAX(0, min_c*0.9);
00128   c2 = MIN(calib->width, max_c*1.1);
00129   r1 = MAX(0, min_r*0.9);
00130   r2 = MIN(calib->height, max_r*1.1);
00131 }
00132 
00133 
00134 
00135 #ifndef NO_LOLIBADDED
00136 
00137 
00138 
00139 RelPose::RelPose(jlo::LazyLocatedObjectLoader* loader) :
00140 jlo::LocatedObject(loader),
00141 m_qualityMeasure(0.0)
00142 {
00143 
00144 }
00145 RelPose::RelPose(jlo::LazyLocatedObjectLoader* loader, LocatedObjectID_t id, LocatedObjectID_t parentID, Matrix m, Matrix cov, std::string name) :
00146 jlo::LocatedObject(loader, id, parentID, m, cov),
00147 m_qualityMeasure(0.0)
00148 {
00149   m_mapstring = name;
00150 }
00151 
00152 RelPose::RelPose(jlo::LocatedObject& pose) :
00153 jlo::LocatedObject(pose),
00154 m_qualityMeasure(0.0)
00155 {
00156 }
00157 
00158 
00159 
00160 /*RelPose::RelPose(RelPose* pose, Matrix m, Matrix cov) :
00161 jlo::LocatedObject(pose),
00162 m_qualityMeasure(0.0)
00163 {
00164   Set(m, cov);
00165 }*/
00166 
00167 /*RelPose::RelPose(jlo::LocatedObject* pose, Matrix m, Matrix cov) :
00168 jlo::LocatedObject(pose),
00169 m_qualityMeasure(0.0)
00170 {
00171   Set(m, cov);
00172 }*/
00173 
00174 /*void RelPose::TransformPointLocally(const double& x_in, const double& y_in, const double& z_in, double& x_out, double& y_out, double& z_out, const double& scale)
00175 {
00176 //      ColumnVector cm = GetRotation();
00177 //      cout << cm << "\n";
00178 
00179   Matrix m = GetMatrix();
00180   ColumnVector d(4);
00181   d << x_in / scale << y_in  / scale << z_in / scale<< 1;
00182   ColumnVector f = m * d;
00183 //      cout << m << " \n* \n" << d << "\n=\n" << f;
00184   x_out = f.element(0);
00185   y_out = f.element(1);
00186   z_out = f.element(2);
00187   //cout << x_out << ", " << y_out << ", " << z_out << "\n";
00188 }
00189 */
00190 XMLTag* RelPose::Save()
00191 {
00192   XMLTag* ret = new XMLTag(XML_NODE_RELPOSE);
00193   if(m_mapstring.length() > 0)
00194     ret->AddProperty(XML_ATTRIBUTE_LOID, m_mapstring);
00195   else
00196     ret->AddProperty(XML_ATTRIBUTE_LOID, m_uniqueID);
00197 
00198   return ret;
00199 }
00200 
00201 #ifdef NO_LO_SERVICE_AVAILABLE
00202 XMLTag* RelPose::SaveComplete()
00203 {
00204   XMLTag* ret = new XMLTag(XML_NODE_RELPOSE);
00205   Matrix m = GetMatrix(ID_WORLD);
00206   Matrix cov = GetCovarianceMatrix(ID_WORLD);
00207 
00208   ret->AddProperty(XML_ATTRIBUTE_LOID, m_uniqueID);
00209   ret->AddProperty(XML_ATTRIBUTE_LOIDFATHER, ID_WORLD);
00210   ret->AddChild(XMLTag::Tag(GetMatrix(ID_WORLD), XML_NODE_MATRIX));
00211   ret->AddChild(XMLTag::Tag(GetCovarianceMatrix(ID_WORLD), XML_NODE_COVARIANCE));
00212 
00213   return ret;
00214 }
00215 #else
00216 XMLTag* RelPose::SaveComplete()
00217 {
00218   XMLTag* ret = new XMLTag(XML_NODE_RELPOSE);
00219 
00220   if(m_mapstring.length() > 0)
00221     ret->AddProperty(XML_ATTRIBUTE_LOID, m_mapstring);
00222   else
00223     ret->AddProperty(XML_ATTRIBUTE_LOID, m_uniqueID);
00224 
00225   if(m_uniqueID != ID_WORLD)
00226     ret->AddProperty(XML_ATTRIBUTE_LOIDFATHER, m_parentID);
00227   ret->AddChild(XMLTag::Tag(GetMatrix(0), XML_NODE_MATRIX));
00228   ret->AddChild(XMLTag::Tag(GetCovarianceMatrix(0), XML_NODE_COVARIANCE));
00229 
00230   return ret;
00231 }
00232 #endif
00233 
00234 
00235 #else
00236 RelPose::RelPose ( ) :
00237   m_qualityMeasure(0.0)
00238 {
00239 
00240 initAttributes();
00241 }
00242 
00243 RelPose::RelPose (Pose pose, Object* relation) :
00244   m_pose(pose),
00245   m_relation(relation),
00246   m_qualityMeasure(0.0)
00247 {
00248 
00249 }
00250 
00251 RelPose::RelPose (XMLTag* tag) :
00252   m_qualityMeasure(0.0)
00253 {
00254   if(tag != NULL)
00255   {
00256     if(tag->GetName().compare(XML_NODE_RELPOSE) == 0)
00257     {
00258       m_pose = Pose(tag->GetChild(0));
00259       if(tag->CountChildren() > 1)
00260         m_relation = (Object*)Elem::ElemFactory(tag->GetChild(1));
00261       else
00262         m_relation = NULL;
00263     }
00264     else
00265       throw "WRONG NODE";
00266   }
00267 }
00268 
00269 RelPose::~RelPose ( ) { }
00270 
00271 //
00272 // Methods
00273 //
00274 
00275 
00276 // Accessor methods
00277 //
00278 #define MAX_DEPTH 5
00279 
00280 inline bool CheckCircles(Object* relation, int depth)
00281 {
00282   if(depth <= 0)
00283     return false;
00284   if(relation != NULL && relation->m_relPose != NULL)
00285   {
00286     return CheckCircles(relation->m_relPose->m_relation, depth - 1);
00287   }
00288   return true;
00289 }
00290 XMLTag* RelPose::Save()
00291 {
00292   XMLTag* ret = new XMLTag(XML_NODE_RELPOSE);
00293   ret->AddChild(m_pose.Save());
00294   if(m_relation != NULL)
00295   {
00296     if ( CheckCircles ( m_relation, MAX_DEPTH) )
00297       ret->AddChild(m_relation->Save());
00298   }
00299   return ret;
00300 }
00301 
00302 RelPose& RelPose::operator +=( const Pose &p)
00303 {
00304   this->m_pose += p;
00305   return *this;
00306 }
00307 
00308 RelPose RelPose::operator + (const Pose &p) const
00309 {
00310   RelPose rel (this->m_pose, this->m_relation);
00311   rel.m_pose += p;
00312   return rel;
00313 }
00314 
00315 
00316 RelPose RelPose::operator - ( const int &levels ) const
00317 {
00318   RelPose rel(this->m_pose, this->m_relation);
00319   if(m_relation != NULL)
00320   {
00321     if(levels > 0)
00322     {
00323       if(m_relation->m_relPose != NULL)
00324         rel = (*m_relation->m_relPose) + m_pose;
00325       else
00326         rel.m_relation = NULL;
00327       if(levels > 1)
00328         rel = rel - (levels - 1);
00329     }
00330   }
00331   return rel;
00332 }
00333 
00334 
00335 
00336 
00337 // Public attribute accessor methods
00338 //
00339 
00340 // Protected static attribute accessor methods
00341 //
00342 
00343 
00344 // Protected attribute accessor methods
00345 //
00346 
00347 
00348 // Private static attribute accessor methods
00349 //
00350 
00351 
00352 // Private attribute accessor methods
00353 //
00354 
00355 
00356 // Other methods
00357 //
00358 
00359 void RelPose::initAttributes ( ) {
00360 }
00361 #endif
00362 
00363 
00364 Matrix RelPose::GetMatrix(LocatedObjectID_t id)
00365 {
00366   if(id == 0 || m_parentID == id)
00367     return LocatedObject::Get();
00368   else
00369   {
00370     RelPose* pose = RelPoseFactory::GetRelPose(m_uniqueID, id);
00371     if(pose != NULL)
00372     {
00373       Matrix m =  pose->GetMatrix(0);
00374       RelPoseFactory::FreeRelPose(&pose, true);
00375       return m;
00376     }
00377     else
00378     {
00379       throw ("Error requesting Matrix");
00380     }
00381   }
00382 }
00383 
00384 Matrix RelPose::GetCovariance(LocatedObjectID_t id)
00385 {
00386   if(id == 0 || m_parentID == id)
00387     return LocatedObject::GetCovariance();
00388   else
00389   {
00390     RelPose* pose = RelPoseFactory::GetRelPose(m_uniqueID, id);
00391     if(pose != NULL)
00392     {
00393       Matrix m =  pose->GetCovariance(0);
00394       RelPoseFactory::FreeRelPose(&pose, true);
00395       return m;
00396     }
00397     else
00398     {
00399       throw("Error requesting cov\n");
00400     }
00401   }
00402 }
00403 
00404 
00405 
00406 namespace sgp_copy
00407 {
00408 
00409 typedef struct
00410 {
00411   double x;
00412   double y;
00413   double z;
00414 }
00415 Point3D;
00416 
00417 typedef struct
00418 {
00419   double sx; double sxy; double sxz;
00420   double syx; double sy; double syz;
00421   double szx; double szy; double sz;
00422 }
00423 CovariancePoint;
00424 
00425 
00436 double det(CovariancePoint cov)
00437 {
00438   return cov.sx * cov.sy * cov.sz -
00439          cov.sx * cov.szy * cov.syz +
00440          cov.sxy * cov.szy * cov.szx -
00441          cov.sxy * cov.syx * cov.sz +
00442          cov.sxz * cov.syx * cov.syz -
00443          cov.sxz * cov.sy * cov.szx;
00444 }
00480 double prob(Point3D point_test, Point3D mean, CovariancePoint cov)
00481 {
00482   double c_temp, expo, det_temp;
00483   double dx =  mean.x - point_test.x;
00484   double dy =  mean.y - point_test.y;
00485   double dz =  mean.z - point_test.z;
00486   double cov_bla = cov.szx * cov.sxy * cov.syz - cov.szx * cov.sxz *  cov.sy - cov.syx * cov.sxy * cov.sz
00487          + cov.syx * cov.sxz * cov.szy + cov.sx * cov.sy * cov.sz - cov.sx * cov.syz * cov.szy;
00488   if(cov_bla == 0.0)
00489   {
00490     cov.sx = 0.0000001;
00491     cov.sy = 0.0000001;
00492     cov.sz = 0.0000001;
00493     cov_bla = cov.szx * cov.sxy * cov.syz - cov.szx * cov.sxz *  cov.sy - cov.syx * cov.sxy * cov.sz
00494          + cov.syx * cov.sxz * cov.szy + cov.sx * cov.sy * cov.sz - cov.sx * cov.syz * cov.szy;
00495   }
00496   double t1 = (( dx * (cov.sy   * cov.sz  - cov.syz * cov.szy)  / cov_bla)-
00497     ( dy * (-cov.szx * cov.syz + cov.syx * cov.sz)   / cov_bla) -
00498     ( dz * (cov.szx  * cov.sy  - cov.syx * cov.szy)  / cov_bla)) * dx;
00499   double t2 =  ((-dx * (cov.sxy  * cov.sz  - cov.sxz * cov.szy)) / cov_bla +
00500     ( dy * (-cov.szx * cov.sxz + cov.sx  * cov.sz)   / cov_bla) +
00501     ( dz * (cov.szx  * cov.sxy - cov.sx  * cov.szy)  / cov_bla)) * dy;
00502   double t3 = (( dx * (cov.sxy  * cov.syz - cov.sxz * cov.sy)   / cov_bla) -
00503     ( dy * (-cov.syx * cov.sxz + cov.sx  * cov.syz)  / cov_bla) +
00504     ( dz * (-cov.syx * cov.sxy + cov.sx  * cov.sy)   / cov_bla)) * dz;
00505 
00506 
00507   /*printf("cov_bla %f, dx %f dy %f dz %f\n",cov_bla,  dx, dy, dz);*/
00508   det_temp = det(cov);
00509   if(det_temp == 0)
00510     det_temp = 0.00000001;
00511   c_temp = 1.0 /( pow(2.0 * M_PI, 1.5) * sqrt(fabs(det_temp)));
00512   /*printf("t1 %f ,t2 %f, t3 %f, cov_bla = %f\n", t1, t2, t3, cov_bla);*/
00513 
00514   expo = (-1.0/2.0) * (t1 + t2 + t3);
00515 
00516   /*printf("expo = %f\n", expo);*/
00517 
00518   c_temp *= exp(expo);
00519   /*printf("ctemp = %f\n", c_temp);*/
00520   return c_temp;
00521 }
00522 
00523 }
00524 
00525 double RelPose::ProbabilityOfCorrespondence(RelPose* pose, bool no_rotation)
00526 {
00527   using namespace sgp_copy;
00528   double equality = 0.0;
00529   if(no_rotation)
00530   {
00531     Point3D pose_this;
00532     Point3D pose_in;
00533     CovariancePoint cov;
00534 
00537     pose_this.x = 0.0;
00538     pose_this.y = 0.0;
00539     pose_this.z = 0.0;
00540 
00541     Matrix m = pose->GetMatrix(m_uniqueID);
00542 
00543     pose_in.x = m.element(0,3);
00544     pose_in.y = m.element(1,3);
00545     pose_in.z = m.element(2,3);
00546 
00547     Matrix cov1 = GetCovariance(m_uniqueID);
00548 
00549     cov.sx  = cov1.element(0,0);
00550     cov.sxy = cov1.element(0,1);
00551     cov.sxz = cov1.element(0,2);
00552 
00553     cov.syx = cov1.element(1,0);
00554     cov.sy  = cov1.element(1,1);
00555     cov.syz = cov1.element(1,2);
00556 
00557     cov.szx = cov1.element(2,0);
00558     cov.szy = cov1.element(2,1);
00559     cov.sz  = cov1.element(2,2);
00560 
00561     equality = prob(pose_in, pose_this, cov);
00562   }
00563   else
00564     throw "RelPose::ProbabilityOfCorrespondence with Rotation is not yet implemented";
00565 
00566   return equality;
00567 }
00568 
00569 double RelPose::DistanceTo(LocatedObjectID_t id)
00570 {
00571   Matrix m =  GetMatrix(id);
00572   double dist = sqrt( m.element(0,3) * m.element(0,3) + m.element(1,3) * m.element(1,3) + m.element(2,3) * m.element(2,3))  +
00573                 fabs(1 - m.element(0,0)) + fabs(1 - m.element(1,1)) + fabs(1 - m.element(2,2));
00574   return dist;
00575 }
00576 


cognitive_perception
Author(s): Ulrich F Klank
autogenerated on Mon Oct 6 2014 10:48:45