00001 // g2o - General Graph Optimization 00002 // Copyright (C) 2011 H. Strasdat 00003 // 00004 // g2o is free software: you can redistribute it and/or modify 00005 // it under the terms of the GNU Lesser General Public License as published 00006 // by the Free Software Foundation, either version 3 of the License, or 00007 // (at your option) any later version. 00008 // 00009 // g2o 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 Lesser General Public License for more details. 00013 // 00014 // You should have received a copy of the GNU Lesser General Public License 00015 // along with this program. If not, see <http://www.gnu.org/licenses/>. 00016 00017 00018 #ifndef SEVEN_DOF_EXPMAP_TYPES 00019 #define SEVEN_DOF_EXPMAP_TYPES 00020 00021 #include "g2o/core/base_vertex.h" 00022 #include "g2o/core/base_binary_edge.h" 00023 #include "g2o/types/sba/types_six_dof_expmap.h" 00024 #include "g2o/math_groups/sim3.h" 00025 00026 namespace g2o { 00027 00028 using namespace Eigen; 00029 00035 class VertexSim3Expmap : public BaseVertex<7, Sim3> 00036 { 00037 public: 00038 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00039 VertexSim3Expmap(); 00040 virtual bool read(std::istream& is); 00041 virtual bool write(std::ostream& os) const; 00042 00043 virtual void setToOrigin() { 00044 _estimate = Sim3(); 00045 } 00046 00047 virtual void oplus(double* update_) 00048 { 00049 00050 Vector7d update; 00051 for (int i=0; i<7; i++) 00052 update[i]=update_[i]; 00053 00054 if (_fix_scale) 00055 update[6] = 0; 00056 00057 Sim3 s(update); 00058 estimate()=s*estimate(); 00059 } 00060 00061 Vector2d _principle_point; 00062 Vector2d _focal_length; 00063 00064 Vector2d cam_map(const Vector2d & v) const 00065 { 00066 Vector2d res; 00067 res[0] = v[0]*_focal_length[0] + _principle_point[0]; 00068 res[1] = v[1]*_focal_length[1] + _principle_point[1]; 00069 return res; 00070 } 00071 00072 bool _fix_scale; 00073 00074 00075 protected: 00076 }; 00077 00081 class EdgeSim3 : public BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap> 00082 { 00083 public: 00084 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00085 EdgeSim3(); 00086 virtual bool read(std::istream& is); 00087 virtual bool write(std::ostream& os) const; 00088 void computeError() 00089 { 00090 const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[0]); 00091 const VertexSim3Expmap* v2 = static_cast<const VertexSim3Expmap*>(_vertices[1]); 00092 00093 Sim3 C(_measurement); 00094 Sim3 error_=C*v1->estimate()*v2->estimate().inverse(); 00095 _error = error_.log(); 00096 } 00097 00098 virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 1.;} 00099 virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/) 00100 { 00101 VertexSim3Expmap* v1 = static_cast<VertexSim3Expmap*>(_vertices[0]); 00102 VertexSim3Expmap* v2 = static_cast<VertexSim3Expmap*>(_vertices[1]); 00103 if (from.count(v1) > 0) 00104 v2->estimate()=measurement()*v1->estimate(); 00105 else 00106 v1->estimate()=measurement().inverse()*v2->estimate(); 00107 } 00108 }; 00109 00110 00111 00112 class EdgeSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2d, VertexPointXYZ, VertexSim3Expmap> 00113 { 00114 public: 00115 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00116 EdgeSim3ProjectXYZ(); 00117 virtual bool read(std::istream& is); 00118 virtual bool write(std::ostream& os) const; 00119 00120 void computeError() 00121 { 00122 const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[1]); 00123 const VertexPointXYZ* v2 = static_cast<const VertexPointXYZ*>(_vertices[0]); 00124 00125 Vector2d obs(_measurement); 00126 _error = obs-v1->cam_map(project(v1->estimate().map(v2->estimate()))); 00127 } 00128 00129 // virtual void linearizeOplus(); 00130 00131 }; 00132 00133 } // end namespace 00134 00135 #endif