1 // g2o - General Graph Optimization 2 // Copyright (C) 2011 H. Strasdat 3 // All rights reserved. 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are 7 // met: 8 // 9 // * Redistributions of source code must retain the above copyright notice, 10 // this list of conditions and the following disclaimer. 11 // * Redistributions in binary form must reproduce the above copyright 12 // notice, this list of conditions and the following disclaimer in the 13 // documentation and/or other materials provided with the distribution. 14 // 15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 27 #ifndef G2O_SIX_DOF_TYPES_EXPMAP 28 #define G2O_SIX_DOF_TYPES_EXPMAP 29 30 #include "g2o/core/base_vertex.h" 31 #include "g2o/core/base_binary_edge.h" 32 #include "g2o/core/base_unary_edge.h" 33 #include "g2o/types/slam3d/se3_ops.h" 34 #include "types_sba.h" 35 #include <Eigen/Geometry> 36 37 namespace g2o { 38 namespace types_six_dof_expmap { 39 void init(); 40 } 41 42 class G2O_TYPES_SBA_API CameraParameters : public g2o::Parameter 43 { 44 public: 45 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 46 CameraParameters(); 47 CameraParameters(number_t focal_length,const Vector2 & principle_point,number_t baseline)48 CameraParameters(number_t focal_length, 49 const Vector2 & principle_point, 50 number_t baseline) 51 : focal_length(focal_length), 52 principle_point(principle_point), 53 baseline(baseline){} 54 55 Vector2 cam_map (const Vector3 & trans_xyz) const; 56 57 Vector3 stereocam_uvu_map (const Vector3 & trans_xyz) const; 58 read(std::istream & is)59 virtual bool read (std::istream& is){ 60 is >> focal_length; 61 is >> principle_point[0]; 62 is >> principle_point[1]; 63 is >> baseline; 64 return true; 65 } 66 write(std::ostream & os)67 virtual bool write (std::ostream& os) const { 68 os << focal_length << " "; 69 os << principle_point.x() << " "; 70 os << principle_point.y() << " "; 71 os << baseline << " "; 72 return true; 73 } 74 75 number_t focal_length; 76 Vector2 principle_point; 77 number_t baseline; 78 }; 79 80 /** 81 * \brief SE3 Vertex parameterized internally with a transformation matrix 82 and externally with its exponential map 83 */ 84 class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{ 85 public: 86 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 87 88 VertexSE3Expmap(); 89 90 bool read(std::istream& is); 91 92 bool write(std::ostream& os) const; 93 setToOriginImpl()94 virtual void setToOriginImpl() { 95 _estimate = SE3Quat(); 96 } 97 oplusImpl(const number_t * update_)98 virtual void oplusImpl(const number_t* update_) { 99 Eigen::Map<const Vector6> update(update_); 100 setEstimate(SE3Quat::exp(update)*estimate()); 101 } 102 }; 103 104 105 /** 106 * \brief 6D edge between two Vertex6 107 */ 108 class G2O_TYPES_SBA_API EdgeSE3Expmap : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>{ 109 public: 110 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 111 EdgeSE3Expmap(); 112 113 bool read(std::istream& is); 114 115 bool write(std::ostream& os) const; 116 computeError()117 void computeError() { 118 const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]); 119 const VertexSE3Expmap* v2 = static_cast<const VertexSE3Expmap*>(_vertices[1]); 120 121 SE3Quat C(_measurement); 122 SE3Quat error_= v2->estimate().inverse()*C*v1->estimate(); 123 _error = error_.log(); 124 } 125 126 virtual void linearizeOplus(); 127 }; 128 129 130 class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2, VertexSBAPointXYZ, VertexSE3Expmap>{ 131 public: 132 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 133 134 EdgeProjectXYZ2UV(); 135 136 bool read(std::istream& is); 137 138 bool write(std::ostream& os) const; 139 computeError()140 void computeError() { 141 const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]); 142 const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]); 143 const CameraParameters * cam = static_cast<const CameraParameters *>(parameter(0)); 144 _error = measurement() - cam->cam_map(v1->estimate().map(v2->estimate())); 145 } 146 147 virtual void linearizeOplus(); 148 149 CameraParameters * _cam; 150 }; 151 152 153 class G2O_TYPES_SBA_API EdgeProjectPSI2UV : public g2o::BaseMultiEdge<2, Vector2> 154 { 155 public: 156 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 157 EdgeProjectPSI2UV()158 EdgeProjectPSI2UV() { 159 resize(3); 160 resizeParameters(1); 161 installParameter(_cam, 0); 162 } 163 164 virtual bool read (std::istream& is); 165 virtual bool write (std::ostream& os) const; 166 void computeError (); 167 virtual void linearizeOplus (); 168 CameraParameters * _cam; 169 }; 170 171 172 173 //Stereo Observations 174 // U: left u 175 // V: left v 176 // U: right u 177 class G2O_TYPES_SBA_API EdgeProjectXYZ2UVU : public BaseBinaryEdge<3, Vector3, VertexSBAPointXYZ, VertexSE3Expmap>{ 178 public: 179 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 180 181 EdgeProjectXYZ2UVU(); 182 183 bool read(std::istream& is); 184 185 bool write(std::ostream& os) const; 186 computeError()187 void computeError(){ 188 const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]); 189 const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]); 190 const CameraParameters* cam = static_cast<const CameraParameters *>(parameter(0)); 191 _error = measurement() - cam->stereocam_uvu_map(v1->estimate().map(v2->estimate())); 192 } 193 // virtual void linearizeOplus(); 194 CameraParameters * _cam; 195 }; 196 197 // Projection using focal_length in x and y directions 198 class G2O_TYPES_SBA_API EdgeSE3ProjectXYZ : public BaseBinaryEdge<2, Vector2, VertexSBAPointXYZ, VertexSE3Expmap> { 199 public: 200 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 201 202 EdgeSE3ProjectXYZ(); 203 204 bool read(std::istream &is); 205 206 bool write(std::ostream &os) const; 207 computeError()208 void computeError() { 209 const VertexSE3Expmap *v1 = static_cast<const VertexSE3Expmap *>(_vertices[1]); 210 const VertexSBAPointXYZ *v2 = static_cast<const VertexSBAPointXYZ *>(_vertices[0]); 211 Vector2 obs(_measurement); 212 _error = obs - cam_project(v1->estimate().map(v2->estimate())); 213 } 214 isDepthPositive()215 bool isDepthPositive() { 216 const VertexSE3Expmap *v1 = static_cast<const VertexSE3Expmap *>(_vertices[1]); 217 const VertexSBAPointXYZ *v2 = static_cast<const VertexSBAPointXYZ *>(_vertices[0]); 218 return (v1->estimate().map(v2->estimate()))(2) > 0.0; 219 } 220 221 virtual void linearizeOplus(); 222 223 Vector2 cam_project(const Vector3 &trans_xyz) const; 224 225 number_t fx, fy, cx, cy; 226 }; 227 228 // Edge to optimize only the camera pose 229 class G2O_TYPES_SBA_API EdgeSE3ProjectXYZOnlyPose : public BaseUnaryEdge<2, Vector2, VertexSE3Expmap> { 230 public: 231 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 232 EdgeSE3ProjectXYZOnlyPose()233 EdgeSE3ProjectXYZOnlyPose() {} 234 235 bool read(std::istream &is); 236 237 bool write(std::ostream &os) const; 238 computeError()239 void computeError() { 240 const VertexSE3Expmap *v1 = static_cast<const VertexSE3Expmap *>(_vertices[0]); 241 Vector2 obs(_measurement); 242 _error = obs - cam_project(v1->estimate().map(Xw)); 243 } 244 isDepthPositive()245 bool isDepthPositive() { 246 const VertexSE3Expmap *v1 = static_cast<const VertexSE3Expmap *>(_vertices[0]); 247 return (v1->estimate().map(Xw))(2) > 0; 248 } 249 250 virtual void linearizeOplus(); 251 252 Vector2 cam_project(const Vector3 &trans_xyz) const; 253 254 Vector3 Xw; 255 number_t fx, fy, cx, cy; 256 }; 257 258 // Projection using focal_length in x and y directions stereo 259 class G2O_TYPES_SBA_API EdgeStereoSE3ProjectXYZ : public BaseBinaryEdge<3, Vector3, VertexSBAPointXYZ, VertexSE3Expmap> { 260 public: 261 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 262 263 EdgeStereoSE3ProjectXYZ(); 264 265 bool read(std::istream &is); 266 267 bool write(std::ostream &os) const; 268 computeError()269 void computeError() { 270 const VertexSE3Expmap *v1 = static_cast<const VertexSE3Expmap *>(_vertices[1]); 271 const VertexSBAPointXYZ *v2 = static_cast<const VertexSBAPointXYZ *>(_vertices[0]); 272 Vector3 obs(_measurement); 273 _error = obs - cam_project(v1->estimate().map(v2->estimate()), bf); 274 } 275 isDepthPositive()276 bool isDepthPositive() { 277 const VertexSE3Expmap *v1 = static_cast<const VertexSE3Expmap *>(_vertices[1]); 278 const VertexSBAPointXYZ *v2 = static_cast<const VertexSBAPointXYZ *>(_vertices[0]); 279 return (v1->estimate().map(v2->estimate()))(2) > 0; 280 } 281 282 virtual void linearizeOplus(); 283 284 Vector3 cam_project(const Vector3 &trans_xyz, const float &bf) const; 285 286 number_t fx, fy, cx, cy, bf; 287 }; 288 289 // Edge to optimize only the camera pose stereo 290 class G2O_TYPES_SBA_API EdgeStereoSE3ProjectXYZOnlyPose : public BaseUnaryEdge<3, Vector3, VertexSE3Expmap> { 291 public: 292 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 293 EdgeStereoSE3ProjectXYZOnlyPose()294 EdgeStereoSE3ProjectXYZOnlyPose() {} 295 296 bool read(std::istream &is); 297 298 bool write(std::ostream &os) const; 299 computeError()300 void computeError() { 301 const VertexSE3Expmap *v1 = static_cast<const VertexSE3Expmap *>(_vertices[0]); 302 Vector3 obs(_measurement); 303 _error = obs - cam_project(v1->estimate().map(Xw)); 304 } 305 isDepthPositive()306 bool isDepthPositive() { 307 const VertexSE3Expmap *v1 = static_cast<const VertexSE3Expmap *>(_vertices[0]); 308 return (v1->estimate().map(Xw))(2) > 0; 309 } 310 311 virtual void linearizeOplus(); 312 313 Vector3 cam_project(const Vector3 &trans_xyz) const; 314 315 Vector3 Xw; 316 number_t fx, fy, cx, cy, bf; 317 }; 318 319 } // end namespace 320 321 #endif 322 323