NonLinearLS.cpp
Go to the documentation of this file.
00001 #include "NonLinearLS.h"
00002 
00003 Matrix4 SE3AxisAngle::exp(const Vector6 & mu)
00004 {
00005   Matrix3 w, w2, I;
00006   Matrix4 M;
00007 
00008   double A,B,C;
00009   double angle, angle2, angle4;
00010 
00011   w.setZero();
00012   w(0,1) = -mu[5];
00013   w(0,2) =  mu[4];
00014   w(1,0) =  mu[5];
00015   w(1,2) = -mu[3];
00016   w(2,0) = -mu[4];
00017   w(2,1) =  mu[3];
00018 
00019   w2 = w*w;
00020 
00021   angle2 = mu[3]*mu[3] + mu[4]*mu[4] + mu[5]*mu[5];
00022   angle  = sqrt(angle2);
00023   angle4 = angle2 * angle2;
00024 
00025   if (fabs(angle) < 1e-8)
00026   {
00027     A = 1 - (angle2)/6 + (angle4)/120;
00028     B = 0.5 - (angle2)/24 + (angle4)/720;
00029     C = 0;
00030   }else
00031   {
00032     A = sin(angle) / angle;
00033     B = (1 - cos(angle)) / angle2;
00034     C = (1-A) / angle2;
00035   }
00036 
00037   I.setIdentity();
00038   M.setIdentity();
00039   M.block<3,3>(0,0) =  I + w*A + w2*B;
00040   M.block<3,1>(0,3) = (I + w*B + w2*C) * mu.segment<3>(0);
00041 
00042   return M;
00043 }
00044 
00046 
00047 Vector6 SE3AxisAngle::log(const Matrix4& m) 
00048 {
00049   Vector6 v6;
00050   Vector3 axis;
00051   // cos(th) = (Tr(R)-1) / 2
00052   double C = (m(0,0)+m(1,1)+m(2,2)-1) * 0.5;
00053   double angle = acos(C);
00054   double angle2 = angle*angle;
00055   double S = sin(angle);
00056   double A,B;
00057 
00058   if(fabs(angle) < 1e-8)
00059   {
00060      A = 1 - (angle2)/6 + (angle2 * angle2)/120;
00061      B = 0;
00062   }else
00063   {
00064      A = sin(angle) / angle;
00065      B = (2*S - angle*(1+C)) / (2*angle2*S);
00066   }
00067 
00068   Matrix3 w = (m.block<3,3>(0,0) - m.block<3,3>(0,0).transpose()) / (2*A);
00069   Matrix3 I;
00070   I.setIdentity();
00071 
00072   axis(0) = w(2,1);   axis(1) = w(0,2);   axis(2) = w(1,0);
00073 
00074   v6.segment<3>(0) = (I - 0.5*w + B*w*w) * m.block<3,1>(0,3);
00075   v6.segment<3>(3) = axis;
00076 
00077   return v6;
00078 }
00079 
00081 
00082 /*
00083 Matrix4 SE3AxisAngle::expInv(const Vector6 & mu)
00084 {
00085 //  Matrix4 Md, Mi;
00086 //  Md = exp(mu);
00087 //  Mi.setIdentity();
00088 //  Mi.block<3,3>(0,0) =  Md.block<3,3>(0,0).transpose();
00089 //  Mi.block<3,1>(0,3) = -Mi.block<3,3>(0,0) * Md.block<3,1>(0,3);
00090 //  return Mi;
00091     return exp(-mu);
00092 }
00093 */
00094 
00096 
00097 /*void EdgeKProjectXYZ2UV::linearizeOplus()
00098 {
00099   // Jacobianas analiticas
00100 }*/
00101 
00102 
00103 
00104 
00105 
00106 void poseOptimization(OptimizationData &dat)
00107 {
00108   int ITERACIONES = 200;
00109 // Configuracion del resolvedor
00110 
00111   g2o::SparseOptimizer optimizer;
00112   optimizer.setMethod(g2o::SparseOptimizer::LevenbergMarquardt);
00113   optimizer.setVerbose(false);
00114 
00115   g2o::BlockSolverX::LinearSolverType * linearSolver;
00116 //  if (DENSE)
00117 //  {
00118 //     cerr << "MUY DENSO\n";
00119      linearSolver =
00120        new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
00121 //  }else
00122   //{
00123   // linearSolver =
00124    //    new g2o::LinearSolverCholmod<g2o::BlockSolverX::PoseMatrixType>();
00125        //new g2o::LinearSolverCSparse<g2o::BlockSolverX::PoseMatrixType>();
00126        //new g2o::LinearSolverPCG<g2o::BlockSolverX::PoseMatrixType>();
00127 //  }
00128 
00129 
00130   g2o::BlockSolverX * solver_ptr =
00131      new g2o::BlockSolverX(&optimizer,linearSolver);
00132 
00133 
00134   optimizer.setSolver(solver_ptr);
00135 
00136 /******************************************************************************/
00137 
00138   int vertex_id = 0;
00139 
00140 
00144   g2o::VertexSE3AxisAngle * v_se3 = new g2o::VertexSE3AxisAngle(dat.cal, dat.cam);
00145 
00146   v_se3->setId(vertex_id);
00147 
00148   optimizer.addVertex(v_se3);
00149   vertex_id++;
00150 
00151 
00155   for (size_t i=0; i < dat.pto3D.size(); ++i)
00156   {
00157     g2o::VertexPointXYZ * v_p = new g2o::VertexPointXYZ();
00158 
00159     v_p->setId(vertex_id);
00160     v_p->setMarginalized(true);
00161     v_p->estimate() = dat.pto3D[i];
00162 
00163     v_p->setFixed(true);
00164 
00165     g2o::EdgeProjectXYZ2UV * e = new g2o::EdgeProjectXYZ2UV();
00166     e->vertices()[0] = dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_se3);
00167     e->vertices()[1] = dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p);
00168     e->measurement() = dat.pto2D[i];
00169     e->inverseMeasurement() = -dat.pto2D[i];
00170 
00171     e->information() = Eigen::Matrix2d::Identity();
00172     e->setRobustKernel(false);
00173     e->setHuberWidth(1);
00174 
00175     optimizer.addEdge(e);
00176     optimizer.addVertex(v_p);
00177     vertex_id ++;
00178   }
00179 
00180 
00184 
00185   cout << "\n ----------------------------------- \n\n";
00186 
00187 
00188   optimizer.initializeOptimization();
00189   optimizer.setVerbose(false);
00190 
00191   optimizer.optimize(ITERACIONES);
00192 
00193   dat.cam = v_se3->getCamera();
00194 }
00195 


re_vision
Author(s): Dorian Galvez-Lopez
autogenerated on Sun Jan 5 2014 11:32:01