00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "types_sba.h"
00018 #include <iostream>
00019
00020 #include "g2o/core/factory.h"
00021
00022 namespace g2o {
00023
00024 using namespace std;
00025 void __attribute__ ((constructor)) init_sba_types(void)
00026 {
00027
00028 Factory* factory = Factory::instance();
00029
00030 factory->registerType("VERTEX_CAM", new HyperGraphElementCreator<VertexCam>);
00031 factory->registerType("VERTEX_XYZ", new HyperGraphElementCreator<VertexPointXYZ>);
00032 factory->registerType("VERTEX_INTRINSICS", new HyperGraphElementCreator<VertexIntrinsics>);
00033
00034 factory->registerType("EDGE_PROJECT_P2MC", new HyperGraphElementCreator<EdgeProjectP2MC>);
00035 factory->registerType("EDGE_PROJECT_P2MC_INTRINSICS", new HyperGraphElementCreator<EdgeProjectP2MC_Intrinsics>);
00036 factory->registerType("EDGE_PROJECT_P2SC", new HyperGraphElementCreator<EdgeProjectP2SC>);
00037 factory->registerType("EDGE_CAM", new HyperGraphElementCreator<EdgeSBACam>);
00038 factory->registerType("EDGE_SCALE", new HyperGraphElementCreator<EdgeSBAScale>);
00039 }
00040
00041
00042 VertexIntrinsics::VertexIntrinsics()
00043 {
00044 estimate() << 1. , 1. , .5 , .5 , .1;
00045 }
00046
00047 bool VertexIntrinsics::read(std::istream& is)
00048 {
00049 for (int i=0; i<5; i++)
00050 is >> estimate()[i];
00051 return true;
00052 }
00053
00054 bool VertexIntrinsics::write(std::ostream& os) const
00055 {
00056 for (int i=0; i<5; i++)
00057 os << estimate()[i] << " ";
00058 return os.good();
00059 }
00060
00061
00062 VertexCam::VertexCam()
00063 {
00064 _intrinsics = 0;
00065 }
00066
00067 bool VertexCam::read(std::istream& is)
00068 {
00069
00070 Vector3d t;
00071 for (int i=0; i<3; i++){
00072 is >> t[i];
00073 }
00074 Vector4d rc;
00075 for (int i=0; i<4; i++) {
00076 is >> rc[i];
00077 }
00078 Quaterniond r;
00079 r.coeffs() = rc;
00080 r.normalize();
00081
00082
00083
00084 SBACam cam(r,t);
00085
00086
00087 double fx, fy, cx, cy, tx;
00088
00089
00090 is >> fx;
00091 if (is.good()) {
00092 is >> fy >> cx >> cy >> tx;
00093 cam.setKcam(fx,fy,cx,cy,tx);
00094 } else{
00095 is.clear();
00096 std::cerr << "cam not defined, using defaults" << std::endl;
00097 cam.setKcam(300,300,320,320,0.1);
00098 }
00099
00100
00101 estimate() = cam;
00102
00103
00104
00105 return true;
00106 }
00107
00108 bool VertexCam::write(std::ostream& os) const
00109 {
00110 const SBACam &cam = estimate();
00111
00112
00113 for (int i=0; i<3; i++)
00114 os << cam.translation()[i] << " ";
00115 for (int i=0; i<4; i++)
00116 os << cam.rotation().coeffs()[i] << " ";
00117
00118
00119 os << cam.Kcam(0,0) << " ";
00120 os << cam.Kcam(1,1) << " ";
00121 os << cam.Kcam(0,2) << " ";
00122 os << cam.Kcam(1,2) << " ";
00123 os << cam.baseline << " ";
00124 return os.good();
00125 }
00126
00127 EdgeSBACam::EdgeSBACam() :
00128 BaseBinaryEdge<6, SE3Quat, VertexCam, VertexCam>()
00129 {
00130 }
00131
00132 bool EdgeSBACam::read(std::istream& is)
00133 {
00134 for (int i=0; i<7; i++)
00135 is >> measurement()[i];
00136 measurement().rotation().normalize();
00137 inverseMeasurement() = measurement().inverse();
00138
00139 for (int i=0; i<6; i++)
00140 for (int j=i; j<6; j++) {
00141 is >> information()(i,j);
00142 if (i!=j)
00143 information()(j,i)=information()(i,j);
00144 }
00145 return true;
00146 }
00147
00148 bool EdgeSBACam::write(std::ostream& os) const
00149 {
00150 for (int i=0; i<7; i++)
00151 os << measurement()[i] << " ";
00152 for (int i=0; i<6; i++)
00153 for (int j=i; j<6; j++){
00154 os << " " << information()(i,j);
00155 }
00156 return os.good();
00157 }
00158
00159 void EdgeSBACam::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* )
00160 {
00161 VertexCam* from = static_cast<VertexCam*>(_vertices[0]);
00162 VertexCam* to = static_cast<VertexCam*>(_vertices[1]);
00163 if (from_.count(from) > 0)
00164 to->estimate() = ((SE3Quat) from->estimate()) * _measurement;
00165 else
00166 from->estimate() = ((SE3Quat) to->estimate() * _inverseMeasurement);
00167 }
00168
00169
00170 VertexPointXYZ::VertexPointXYZ() : BaseVertex<3, Vector3d>()
00171 {
00172 }
00173
00174 bool VertexPointXYZ::read(std::istream& is)
00175 {
00176 Vector3d lv;
00177 for (int i=0; i<3; i++)
00178 is >> lv[i];
00179 estimate() = lv;
00180 return true;
00181 }
00182
00183 bool VertexPointXYZ::write(std::ostream& os) const
00184 {
00185 Vector3d lv=estimate();
00186 for (int i=0; i<3; i++){
00187 os << lv[i] << " ";
00188 }
00189 return os.good();
00190 }
00191
00192
00193 EdgeProjectP2MC::EdgeProjectP2MC() :
00194 BaseBinaryEdge<2, Vector2d, VertexPointXYZ, VertexCam>()
00195 {
00196 information().setIdentity();
00197 }
00198
00199 bool EdgeProjectP2MC::read(std::istream& is)
00200 {
00201
00202 for (int i=0; i<2; i++)
00203 is >> measurement()[i];
00204
00205 inverseMeasurement() = -measurement();
00206
00207 information().setIdentity();
00208 return true;
00209 }
00210
00211 bool EdgeProjectP2MC::write(std::ostream& os) const
00212 {
00213 for (int i=0; i<2; i++)
00214 os << measurement()[i] << " ";
00215 return os.good();
00216 }
00217
00218
00219 EdgeProjectP2SC::EdgeProjectP2SC() :
00220 BaseBinaryEdge<3, Vector3d, VertexPointXYZ, VertexCam>()
00221 {
00222 }
00223
00224 bool EdgeProjectP2SC::read(std::istream& is)
00225 {
00226
00227 for (int i=0; i<3; i++)
00228 is >> measurement()[i];
00229
00230 inverseMeasurement() = -measurement();
00231
00232 information().setIdentity();
00233 return true;
00234 }
00235
00236 bool EdgeProjectP2SC::write(std::ostream& os) const
00237 {
00238 for (int i=0; i<3; i++)
00239 os << measurement()[i] << " ";
00240 return os.good();
00241 }
00242
00246 void EdgeProjectP2SC::linearizeOplus()
00247 {
00248 VertexCam *vc = static_cast<VertexCam *>(_vertices[1]);
00249 SBACam &cam = vc->estimate();
00250
00251 VertexPointXYZ *vp = static_cast<VertexPointXYZ *>(_vertices[0]);
00252 Vector4d pt, trans;
00253 pt.head<3>() = vp->estimate();
00254 pt(3) = 1.0;
00255 trans.head<3>() = cam.translation();
00256 trans(3) = 1.0;
00257
00258
00259 Eigen::Matrix<double,3,1> pc = cam.w2n * pt;
00260
00261
00262
00263 double px = pc(0);
00264 double py = pc(1);
00265 double pz = pc(2);
00266 double ipz2 = 1.0/(pz*pz);
00267 if (isnan(ipz2) )
00268 {
00269 std::cout << "[SetJac] infinite jac" << std::endl;
00270 *(int *)0x0 = 0;
00271 }
00272
00273 double ipz2fx = ipz2*cam.Kcam(0,0);
00274 double ipz2fy = ipz2*cam.Kcam(1,1);
00275 double b = cam.baseline;
00276
00277 Eigen::Matrix<double,3,1> pwt;
00278
00279
00280 pwt = (pt-trans).head<3>();
00281
00282
00283 Eigen::Matrix<double,3,1> dp = cam.dRdx * pwt;
00284 _jacobianOplusXj(0,3) = (pz*dp(0) - px*dp(2))*ipz2fx;
00285 _jacobianOplusXj(1,3) = (pz*dp(1) - py*dp(2))*ipz2fy;
00286 _jacobianOplusXj(2,3) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00287
00288 dp = cam.dRdy * pwt;
00289 _jacobianOplusXj(0,4) = (pz*dp(0) - px*dp(2))*ipz2fx;
00290 _jacobianOplusXj(1,4) = (pz*dp(1) - py*dp(2))*ipz2fy;
00291 _jacobianOplusXj(2,4) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00292
00293 dp = cam.dRdz * pwt;
00294 _jacobianOplusXj(0,5) = (pz*dp(0) - px*dp(2))*ipz2fx;
00295 _jacobianOplusXj(1,5) = (pz*dp(1) - py*dp(2))*ipz2fy;
00296 _jacobianOplusXj(2,5) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00297
00298
00299 dp = -cam.w2n.col(0);
00300 _jacobianOplusXj(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00301 _jacobianOplusXj(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00302 _jacobianOplusXj(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00303 dp = -cam.w2n.col(1);
00304 _jacobianOplusXj(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00305 _jacobianOplusXj(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00306 _jacobianOplusXj(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00307 dp = -cam.w2n.col(2);
00308 _jacobianOplusXj(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00309 _jacobianOplusXj(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00310 _jacobianOplusXj(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00311
00312
00313
00314 dp = cam.w2n.col(0);
00315 _jacobianOplusXi(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00316 _jacobianOplusXi(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00317 _jacobianOplusXi(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00318 dp = cam.w2n.col(1);
00319 _jacobianOplusXi(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00320 _jacobianOplusXi(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00321 _jacobianOplusXi(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00322 dp = cam.w2n.col(2);
00323 _jacobianOplusXi(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00324 _jacobianOplusXi(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00325 _jacobianOplusXi(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fx;
00326 }
00327
00328
00332 void EdgeProjectP2MC::linearizeOplus()
00333 {
00334 VertexCam *vc = static_cast<VertexCam *>(_vertices[1]);
00335 SBACam &cam = vc->estimate();
00336
00337 VertexPointXYZ *vp = static_cast<VertexPointXYZ *>(_vertices[0]);
00338 Vector4d pt, trans;
00339 pt.head<3>() = vp->estimate();
00340 pt(3) = 1.0;
00341 trans.head<3>() = cam.translation();
00342 trans(3) = 1.0;
00343
00344
00345 Eigen::Matrix<double,3,1> pc = cam.w2n * pt;
00346
00347
00348
00349 double px = pc(0);
00350 double py = pc(1);
00351 double pz = pc(2);
00352 double ipz2 = 1.0/(pz*pz);
00353 if (isnan(ipz2) )
00354 {
00355 std::cout << "[SetJac] infinite jac" << std::endl;
00356 *(int *)0x0 = 0;
00357 }
00358
00359 double ipz2fx = ipz2*cam.Kcam(0,0);
00360 double ipz2fy = ipz2*cam.Kcam(1,1);
00361
00362 Eigen::Matrix<double,3,1> pwt;
00363
00364
00365 pwt = (pt-trans).head<3>();
00366
00367
00368 Eigen::Matrix<double,3,1> dp = cam.dRdx * pwt;
00369 _jacobianOplusXj(0,3) = (pz*dp(0) - px*dp(2))*ipz2fx;
00370 _jacobianOplusXj(1,3) = (pz*dp(1) - py*dp(2))*ipz2fy;
00371
00372 dp = cam.dRdy * pwt;
00373 _jacobianOplusXj(0,4) = (pz*dp(0) - px*dp(2))*ipz2fx;
00374 _jacobianOplusXj(1,4) = (pz*dp(1) - py*dp(2))*ipz2fy;
00375
00376 dp = cam.dRdz * pwt;
00377 _jacobianOplusXj(0,5) = (pz*dp(0) - px*dp(2))*ipz2fx;
00378 _jacobianOplusXj(1,5) = (pz*dp(1) - py*dp(2))*ipz2fy;
00379
00380
00381 dp = -cam.w2n.col(0);
00382 _jacobianOplusXj(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00383 _jacobianOplusXj(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00384 dp = -cam.w2n.col(1);
00385 _jacobianOplusXj(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00386 _jacobianOplusXj(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00387 dp = -cam.w2n.col(2);
00388 _jacobianOplusXj(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00389 _jacobianOplusXj(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00390
00391
00392
00393 dp = cam.w2n.col(0);
00394 _jacobianOplusXi(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00395 _jacobianOplusXi(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00396 dp = cam.w2n.col(1);
00397 _jacobianOplusXi(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00398 _jacobianOplusXi(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00399 dp = cam.w2n.col(2);
00400 _jacobianOplusXi(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00401 _jacobianOplusXi(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00402 }
00403
00404
00405
00406
00407 EdgeProjectP2MC_Intrinsics::EdgeProjectP2MC_Intrinsics() :
00408 BaseMultiEdge<2, Vector2d>()
00409 {
00410 information().setIdentity();
00411 resize(3);
00412 _jacobianOplus[0].resize(2,3);
00413 _jacobianOplus[1].resize(2,6);
00414 _jacobianOplus[2].resize(2,4);
00415 }
00416
00420 void EdgeProjectP2MC_Intrinsics::linearizeOplus()
00421 {
00422 VertexCam *vc = static_cast<VertexCam *>(_vertices[1]);
00423 SBACam &cam = vc->estimate();
00424
00425 VertexPointXYZ *vp = static_cast<VertexPointXYZ *>(_vertices[0]);
00426
00427
00428
00429 Vector4d pt, trans;
00430 pt.head<3>() = vp->estimate();
00431 pt(3) = 1.0;
00432 trans.head<3>() = cam.translation();
00433 trans(3) = 1.0;
00434
00435
00436 Eigen::Matrix<double,3,1> pc = cam.w2n * pt;
00437
00438
00439
00440 double px = pc(0);
00441 double py = pc(1);
00442 double pz = pc(2);
00443 double ipz2 = 1.0/(pz*pz);
00444 if (isnan(ipz2) )
00445 {
00446 std::cout << "[SetJac] infinite jac" << std::endl;
00447 *(int *)0x0 = 0;
00448 }
00449
00450 double ipz2fx = ipz2*cam.Kcam(0,0);
00451 double ipz2fy = ipz2*cam.Kcam(1,1);
00452
00453 Eigen::Matrix<double,3,1> pwt;
00454
00455
00456 pwt = (pt-trans).head<3>();
00457
00458
00459 Eigen::Matrix<double,3,1> dp = cam.dRdx * pwt;
00460 _jacobianOplus[1](0,3) = (pz*dp(0) - px*dp(2))*ipz2fx;
00461 _jacobianOplus[1](1,3) = (pz*dp(1) - py*dp(2))*ipz2fy;
00462
00463 dp = cam.dRdy * pwt;
00464 _jacobianOplus[1](0,4) = (pz*dp(0) - px*dp(2))*ipz2fx;
00465 _jacobianOplus[1](1,4) = (pz*dp(1) - py*dp(2))*ipz2fy;
00466
00467 dp = cam.dRdz * pwt;
00468 _jacobianOplus[1](0,5) = (pz*dp(0) - px*dp(2))*ipz2fx;
00469 _jacobianOplus[1](1,5) = (pz*dp(1) - py*dp(2))*ipz2fy;
00470
00471
00472 dp = -cam.w2n.col(0);
00473 _jacobianOplus[1](0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00474 _jacobianOplus[1](1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00475 dp = -cam.w2n.col(1);
00476 _jacobianOplus[1](0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00477 _jacobianOplus[1](1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00478 dp = -cam.w2n.col(2);
00479 _jacobianOplus[1](0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00480 _jacobianOplus[1](1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00481
00482
00483
00484 dp = cam.w2n.col(0);
00485 _jacobianOplus[0](0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
00486 _jacobianOplus[0](1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
00487 dp = cam.w2n.col(1);
00488 _jacobianOplus[0](0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
00489 _jacobianOplus[0](1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
00490 dp = cam.w2n.col(2);
00491 _jacobianOplus[0](0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
00492 _jacobianOplus[0](1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
00493
00494
00495 _jacobianOplus[2].setZero();
00496 _jacobianOplus[2](0,0) = px/pz;
00497 _jacobianOplus[2](1,1) = py/pz;
00498 _jacobianOplus[2](0,2) = 1.;
00499 _jacobianOplus[2](1,3) = 1.;
00500 }
00501
00502 bool EdgeProjectP2MC_Intrinsics::read(std::istream& is)
00503 {
00504
00505 for (int i=0; i<2; i++)
00506 is >> measurement()[i];
00507
00508 inverseMeasurement() = -measurement();
00509
00510 information().setIdentity();
00511 return true;
00512 }
00513
00514 bool EdgeProjectP2MC_Intrinsics::write(std::ostream& os) const
00515 {
00516 for (int i=0; i<2; i++)
00517 os << measurement()[i] << " ";
00518 return os.good();
00519 }
00520
00521
00522
00523 EdgeSBAScale::EdgeSBAScale() :
00524 BaseBinaryEdge<1, double, VertexCam, VertexCam>()
00525 {
00526 }
00527
00528 bool EdgeSBAScale::read(std::istream& is)
00529 {
00530 is >> measurement();
00531 inverseMeasurement() = -measurement();
00532 information().setIdentity();
00533 is >> information()(0,0);
00534 return true;
00535 }
00536
00537 bool EdgeSBAScale::write(std::ostream& os) const
00538 {
00539 os << measurement() << " " << information()(0,0);
00540 return os.good();
00541 }
00542
00543 void EdgeSBAScale::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* )
00544 {
00545 VertexCam* v1 = dynamic_cast<VertexCam*>(_vertices[0]);
00546 VertexCam* v2 = dynamic_cast<VertexCam*>(_vertices[1]);
00547
00548 if (from_.count(v1) == 1){
00549 SE3Quat delta = (v1->estimate().inverse()*v2->estimate());
00550 double norm = delta.translation().norm();
00551 double alpha = _measurement/norm;
00552 delta.translation()*=alpha;
00553 v2->estimate()=v1->estimate()*delta;
00554 } else {
00555 SE3Quat delta = (v2->estimate().inverse()*v1->estimate());
00556 double norm = delta.translation().norm();
00557 double alpha = _measurement/norm;
00558 delta.translation()*=alpha;
00559 v1->estimate()=v2->estimate()*delta;
00560 }
00561 }
00562
00563 }