1 // g2o - General Graph Optimization 2 // Copyright (C) 2011 Kurt Konolige 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 #include "types_sba.h" 28 #include <iostream> 29 30 #include "g2o/core/factory.h" 31 #include "g2o/stuff/macros.h" 32 33 namespace g2o { 34 35 using namespace std; 36 37 G2O_REGISTER_TYPE_GROUP(sba); 38 39 G2O_REGISTER_TYPE(VERTEX_CAM, VertexCam); 40 G2O_REGISTER_TYPE(VERTEX_XYZ, VertexSBAPointXYZ); 41 G2O_REGISTER_TYPE(VERTEX_INTRINSICS, VertexIntrinsics); 42 43 G2O_REGISTER_TYPE(EDGE_PROJECT_P2MC, EdgeProjectP2MC); 44 G2O_REGISTER_TYPE(EDGE_PROJECT_P2SC, EdgeProjectP2SC); 45 G2O_REGISTER_TYPE(EDGE_CAM, EdgeSBACam); 46 G2O_REGISTER_TYPE(EDGE_SCALE, EdgeSBAScale); 47 48 // constructor VertexIntrinsics()49 VertexIntrinsics::VertexIntrinsics() 50 { 51 _estimate << cst(1.), cst(1.), cst(.5), cst(.5), cst(.1); 52 } 53 read(std::istream & is)54 bool VertexIntrinsics::read(std::istream& is) { return internal::readVector(is, _estimate); } 55 write(std::ostream & os) const56 bool VertexIntrinsics::write(std::ostream& os) const { return internal::writeVector(os, estimate()); } 57 58 // constructor VertexCam()59 VertexCam::VertexCam() 60 { 61 } 62 read(std::istream & is)63 bool VertexCam::read(std::istream& is) 64 { 65 // first the position and orientation (vector3 and quaternion) 66 Vector3 t; 67 internal::readVector(is, t); 68 Quaternion r; 69 internal::readVector(is, r.coeffs()); 70 r.normalize(); // recover nummeric precision 71 72 // form the camera object 73 SBACam cam(r,t); 74 75 // now fx, fy, cx, cy, baseline 76 number_t fx, fy, cx, cy, tx; 77 78 // try to read one value 79 is >> fx; 80 if (is.good()) { 81 is >> fy >> cx >> cy >> tx; 82 cam.setKcam(fx,fy,cx,cy,tx); 83 } else{ 84 is.clear(); 85 std::cerr << "cam not defined, using defaults" << std::endl; 86 cam.setKcam(300,300,320,320,cst(0.1)); 87 } 88 89 setEstimate(cam); 90 return true; 91 } 92 write(std::ostream & os) const93 bool VertexCam::write(std::ostream& os) const 94 { 95 const SBACam &cam = estimate(); 96 97 // first the position and orientation (vector3 and quaternion) 98 internal::writeVector(os, cam.translation()); 99 internal::writeVector(os, cam.rotation().coeffs()); 100 101 // now fx, fy, cx, cy, baseline 102 os << cam.Kcam(0,0) << " "; 103 os << cam.Kcam(1,1) << " "; 104 os << cam.Kcam(0,2) << " "; 105 os << cam.Kcam(1,2) << " "; 106 os << cam.baseline << " "; 107 return os.good(); 108 } 109 EdgeSBACam()110 EdgeSBACam::EdgeSBACam() : 111 BaseBinaryEdge<6, SE3Quat, VertexCam, VertexCam>() 112 { 113 } 114 read(std::istream & is)115 bool EdgeSBACam::read(std::istream& is) 116 { 117 Vector7 meas; 118 internal::readVector(is, meas); 119 setMeasurement(SE3Quat(meas)); 120 return readInformationMatrix(is); 121 } 122 write(std::ostream & os) const123 bool EdgeSBACam::write(std::ostream& os) const 124 { 125 internal::writeVector(os, measurement().toVector()); 126 return writeInformationMatrix(os); 127 } 128 initialEstimate(const OptimizableGraph::VertexSet & from_,OptimizableGraph::Vertex *)129 void EdgeSBACam::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/) 130 { 131 VertexCam* from = static_cast<VertexCam*>(_vertices[0]); 132 VertexCam* to = static_cast<VertexCam*>(_vertices[1]); 133 if (from_.count(from) > 0) 134 to->setEstimate((SE3Quat) from->estimate() * _measurement); 135 else 136 from->setEstimate((SE3Quat) to->estimate() * _inverseMeasurement); 137 } 138 139 VertexSBAPointXYZ()140 VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3>() 141 { 142 } 143 read(std::istream & is)144 bool VertexSBAPointXYZ::read(std::istream& is) 145 { 146 return internal::readVector(is, _estimate); 147 } 148 write(std::ostream & os) const149 bool VertexSBAPointXYZ::write(std::ostream& os) const 150 { 151 return internal::writeVector(os, estimate()); 152 } 153 154 // point to camera projection, monocular EdgeProjectP2MC()155 EdgeProjectP2MC::EdgeProjectP2MC() : 156 BaseBinaryEdge<2, Vector2, VertexSBAPointXYZ, VertexCam>() 157 { 158 information().setIdentity(); 159 } 160 read(std::istream & is)161 bool EdgeProjectP2MC::read(std::istream& is) 162 { 163 // measured keypoint 164 internal::readVector(is, _measurement); 165 return readInformationMatrix(is); 166 } 167 write(std::ostream & os) const168 bool EdgeProjectP2MC::write(std::ostream& os) const 169 { 170 internal::writeVector(os, measurement()); 171 writeInformationMatrix(os); 172 return os.good(); 173 } 174 175 // point to camera projection, stereo EdgeProjectP2SC()176 EdgeProjectP2SC::EdgeProjectP2SC() : 177 BaseBinaryEdge<3, Vector3, VertexSBAPointXYZ, VertexCam>() 178 { 179 } 180 read(std::istream & is)181 bool EdgeProjectP2SC::read(std::istream& is) 182 { 183 internal::readVector(is, _measurement); 184 return readInformationMatrix(is); 185 } 186 write(std::ostream & os) const187 bool EdgeProjectP2SC::write(std::ostream& os) const 188 { 189 internal::writeVector(os, measurement()); 190 writeInformationMatrix(os); 191 return os.good(); 192 } 193 194 /** 195 * \brief Jacobian for stereo projection 196 */ linearizeOplus()197 void EdgeProjectP2SC::linearizeOplus() 198 { 199 VertexCam *vc = static_cast<VertexCam *>(_vertices[1]); 200 const SBACam &cam = vc->estimate(); 201 202 VertexSBAPointXYZ *vp = static_cast<VertexSBAPointXYZ *>(_vertices[0]); 203 Vector4 pt, trans; 204 pt.head<3>() = vp->estimate(); 205 pt(3) = 1.0; 206 trans.head<3>() = cam.translation(); 207 trans(3) = 1.0; 208 209 // first get the world point in camera coords 210 Vector3 pc = cam.w2n * pt; 211 212 // Jacobians wrt camera parameters 213 // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2 214 number_t px = pc(0); 215 number_t py = pc(1); 216 number_t pz = pc(2); 217 number_t ipz2 = 1.0/(pz*pz); 218 if (g2o_isnan(ipz2) ) { 219 std::cout << "[SetJac] infinite jac" << std::endl; 220 abort(); 221 } 222 223 number_t ipz2fx = ipz2*cam.Kcam(0,0); // Fx 224 number_t ipz2fy = ipz2*cam.Kcam(1,1); // Fy 225 number_t b = cam.baseline; // stereo baseline 226 227 // check for local vars 228 Vector3 pwt = (pt-trans).head<3>(); // transform translations, use differential rotation 229 230 // dx 231 Vector3 dp = cam.dRdx * pwt; // dR'/dq * [pw - t] 232 _jacobianOplusXj(0,3) = (pz*dp(0) - px*dp(2))*ipz2fx; 233 _jacobianOplusXj(1,3) = (pz*dp(1) - py*dp(2))*ipz2fy; 234 _jacobianOplusXj(2,3) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 235 // dy 236 dp = cam.dRdy * pwt; // dR'/dq * [pw - t] 237 _jacobianOplusXj(0,4) = (pz*dp(0) - px*dp(2))*ipz2fx; 238 _jacobianOplusXj(1,4) = (pz*dp(1) - py*dp(2))*ipz2fy; 239 _jacobianOplusXj(2,4) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 240 // dz 241 dp = cam.dRdz * pwt; // dR'/dq * [pw - t] 242 _jacobianOplusXj(0,5) = (pz*dp(0) - px*dp(2))*ipz2fx; 243 _jacobianOplusXj(1,5) = (pz*dp(1) - py*dp(2))*ipz2fy; 244 _jacobianOplusXj(2,5) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 245 246 // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2 247 dp = -cam.w2n.col(0); // dpc / dx 248 _jacobianOplusXj(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx; 249 _jacobianOplusXj(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy; 250 _jacobianOplusXj(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 251 dp = -cam.w2n.col(1); // dpc / dy 252 _jacobianOplusXj(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx; 253 _jacobianOplusXj(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy; 254 _jacobianOplusXj(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 255 dp = -cam.w2n.col(2); // dpc / dz 256 _jacobianOplusXj(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx; 257 _jacobianOplusXj(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy; 258 _jacobianOplusXj(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 259 260 // Jacobians wrt point parameters 261 // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2 262 dp = cam.w2n.col(0); // dpc / dx 263 _jacobianOplusXi(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx; 264 _jacobianOplusXi(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy; 265 _jacobianOplusXi(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 266 dp = cam.w2n.col(1); // dpc / dy 267 _jacobianOplusXi(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx; 268 _jacobianOplusXi(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy; 269 _jacobianOplusXi(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 270 dp = cam.w2n.col(2); // dpc / dz 271 _jacobianOplusXi(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx; 272 _jacobianOplusXi(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy; 273 _jacobianOplusXi(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx; // right image px 274 } 275 276 277 /** 278 * \brief Jacobian for monocular projection 279 */ linearizeOplus()280 void EdgeProjectP2MC::linearizeOplus() 281 { 282 VertexCam *vc = static_cast<VertexCam *>(_vertices[1]); 283 const SBACam &cam = vc->estimate(); 284 285 VertexSBAPointXYZ *vp = static_cast<VertexSBAPointXYZ *>(_vertices[0]); 286 Vector4 pt, trans; 287 pt.head<3>() = vp->estimate(); 288 pt(3) = 1.0; 289 trans.head<3>() = cam.translation(); 290 trans(3) = 1.0; 291 292 // first get the world point in camera coords 293 Vector3 pc = cam.w2n * pt; 294 295 // Jacobians wrt camera parameters 296 // set d(quat-x) values [ pz*dpx/dx - px*dpz/dx ] / pz^2 297 number_t px = pc(0); 298 number_t py = pc(1); 299 number_t pz = pc(2); 300 number_t ipz2 = 1.0/(pz*pz); 301 if (g2o_isnan(ipz2) ) { 302 std::cout << "[SetJac] infinite jac" << std::endl; 303 abort(); 304 } 305 306 number_t ipz2fx = ipz2*cam.Kcam(0,0); // Fx 307 number_t ipz2fy = ipz2*cam.Kcam(1,1); // Fy 308 309 // check for local vars 310 Vector3 pwt = (pt-trans).head<3>(); // transform translations, use differential rotation 311 312 // dx 313 Vector3 dp = cam.dRdx * pwt; // dR'/dq * [pw - t] 314 _jacobianOplusXj(0,3) = (pz*dp(0) - px*dp(2))*ipz2fx; 315 _jacobianOplusXj(1,3) = (pz*dp(1) - py*dp(2))*ipz2fy; 316 // dy 317 dp = cam.dRdy * pwt; // dR'/dq * [pw - t] 318 _jacobianOplusXj(0,4) = (pz*dp(0) - px*dp(2))*ipz2fx; 319 _jacobianOplusXj(1,4) = (pz*dp(1) - py*dp(2))*ipz2fy; 320 // dz 321 dp = cam.dRdz * pwt; // dR'/dq * [pw - t] 322 _jacobianOplusXj(0,5) = (pz*dp(0) - px*dp(2))*ipz2fx; 323 _jacobianOplusXj(1,5) = (pz*dp(1) - py*dp(2))*ipz2fy; 324 325 // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2 326 dp = -cam.w2n.col(0); // dpc / dx 327 _jacobianOplusXj(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx; 328 _jacobianOplusXj(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy; 329 dp = -cam.w2n.col(1); // dpc / dy 330 _jacobianOplusXj(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx; 331 _jacobianOplusXj(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy; 332 dp = -cam.w2n.col(2); // dpc / dz 333 _jacobianOplusXj(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx; 334 _jacobianOplusXj(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy; 335 336 // Jacobians wrt point parameters 337 // set d(t) values [ pz*dpx/dx - px*dpz/dx ] / pz^2 338 dp = cam.w2n.col(0); // dpc / dx 339 _jacobianOplusXi(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx; 340 _jacobianOplusXi(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy; 341 dp = cam.w2n.col(1); // dpc / dy 342 _jacobianOplusXi(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx; 343 _jacobianOplusXi(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy; 344 dp = cam.w2n.col(2); // dpc / dz 345 _jacobianOplusXi(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx; 346 _jacobianOplusXi(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy; 347 } 348 349 // point to camera projection, stereo EdgeSBAScale()350 EdgeSBAScale::EdgeSBAScale() : 351 BaseBinaryEdge<1, number_t, VertexCam, VertexCam>() 352 { 353 } 354 read(std::istream & is)355 bool EdgeSBAScale::read(std::istream& is) 356 { 357 number_t meas; 358 is >> meas; 359 setMeasurement(meas); 360 information().setIdentity(); 361 is >> information()(0,0); 362 return true; 363 } 364 write(std::ostream & os) const365 bool EdgeSBAScale::write(std::ostream& os) const 366 { 367 os << measurement() << " " << information()(0,0); 368 return os.good(); 369 } 370 initialEstimate(const OptimizableGraph::VertexSet & from_,OptimizableGraph::Vertex *)371 void EdgeSBAScale::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/) 372 { 373 VertexCam* v1 = dynamic_cast<VertexCam*>(_vertices[0]); 374 VertexCam* v2 = dynamic_cast<VertexCam*>(_vertices[1]); 375 //compute the translation vector of v1 w.r.t v2 376 if (from_.count(v1) == 1){ 377 SE3Quat delta = (v1->estimate().inverse()*v2->estimate()); 378 number_t norm = delta.translation().norm(); 379 number_t alpha = _measurement/norm; 380 delta.setTranslation(delta.translation()*alpha); 381 v2->setEstimate(v1->estimate()*delta); 382 } else { 383 SE3Quat delta = (v2->estimate().inverse()*v1->estimate()); 384 number_t norm = delta.translation().norm(); 385 number_t alpha = _measurement/norm; 386 delta.setTranslation(delta.translation()*alpha); 387 v1->setEstimate(v2->estimate()*delta); 388 } 389 } 390 setMeasurementFromState()391 bool EdgeSBACam::setMeasurementFromState() 392 { 393 const VertexCam* v1 = dynamic_cast<const VertexCam*>(_vertices[0]); 394 const VertexCam* v2 = dynamic_cast<const VertexCam*>(_vertices[1]); 395 _measurement = (v1->estimate().inverse()*v2->estimate()); 396 _inverseMeasurement = _measurement.inverse(); 397 return true; 398 } 399 400 } // end namespace 401