12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added tests on smartHessianFactor with Cal3Bundler

release/4.3a0
Luca Carlone 2013-10-19 22:00:43 +00:00
parent 38c91e1913
commit 92f0bb64b2
2 changed files with 236 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
examples/Data/dubrovnik-3-7-pre-rewritten.txt
View File

@ -20,61 +20,61 @@
1 6 543.18011474609375 294.80999755859375 1 6 543.18011474609375 294.80999755859375
2 6 -58.419979095458984375 110.8300018310546875 2 6 -58.419979095458984375 110.8300018310546875
-0.016943983733654022217 0.29656188120312942935
0.011171804741024972743 -0.035318354384285870207
0.0024643507786095142365 0.31252101755032046793
0.73030996322631824835 0.47230274932665988752
-0.26490819454193109683 -0.3572340863744113415
-1.712789297103882058 -2.0517704282499575896
1430.031982421875 1430.031982421875
-7.5572756941255647689e-08 -7.5572756941255647689e-08
3.2377570134516087119e-14 3.2377570134516087119e-14
0.015049725770950320852 0.28532097381985194184
-0.18504564464092254639 -0.27699838370789808817
-0.29278403520584106445 0.048601169984112867206
-1.0590475797653198242 -1.2598695987143850861
-0.036017861217260457862 -0.049063798188844320869
-1.572034001350402832 -1.9586867140445654023
1432.137451171875 1432.137451171875
-7.3171918302250560373e-08 -7.3171918302250560373e-08
3.1759419042137054801e-14 3.1759419042137054801e-14
-0.30793598294258112125 0.057491325683772541433
0.32077908515930175781 0.34853090049579965592
0.2225398570299148282 0.47985129303736057116
8.5034484863281267764 8.1963904289063389541
6.7499604225158700288 6.5146840788718787252
-3.6383814811706534087 -3.8392804395897406344
1572.047119140625 1572.047119140625
-1.5962623223231275915e-08 -1.5962623223231275915e-08
-1.6507904730136101212e-14 -1.6507904730136101212e-14
-12.055994987487792969 -11.317351620610928364
12.838775634765625 3.3594874875767186673
-41.099369049072265625 -42.755222607849105998
6.4168906211853027344 4.2648515634753199066
0.38897031545639038086 -8.4629358700849355301
-23.586282730102539062 -22.252086323427270997
13.051100730895996094 10.996977688149536689
3.8387587070465087891 -9.2123370180278048025
-29.777933120727539062 -29.206739014051372294
13.060946464538574219 10.935342607054865383
3.5910520553588867188 -9.4338917557810741954
-29.759080886840820312 -29.112263909175499776
14.265764236450195312 15.714024935401759819
24.096216201782226562 1.3745079651566265433
-54.823970794677734375 -59.286834979937104606
-0.25292283296585083008 -1.3624227800805182031
2.2166082859039306641 -4.1979357415396094666
-21.712127685546875 -21.034430148188398846
7.6465740203857421875 6.7690173115899296974
14.185332298278808594 -4.7352452433700786827
-52.070301055908203125 -53.605307875695892506

296
gtsam_unstable/slam/tests/testSmartProjectionHessianFactor.cpp
View File

@ -53,9 +53,10 @@ static bool isDebugTest = false;
// make a realistic calibration matrix // make a realistic calibration matrix
static double fov = 60; // degrees static double fov = 60; // degrees
static size_t w=640,h=480; static size_t w=640,h=480;
static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480)); static Cal3_S2::shared_ptr K2(new Cal3_S2(1500, 1200, 0, 640, 480));
static boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
static double rankTol = 1.0; static double rankTol = 1.0;
static double linThreshold = -1.0; static double linThreshold = -1.0;
@ -76,6 +77,7 @@ static Point2 measurement1(323.0, 240.0);
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
typedef SmartProjectionHessianFactor<Pose3,Point3,Cal3_S2> SmartFactor; typedef SmartProjectionHessianFactor<Pose3,Point3,Cal3_S2> SmartFactor;
typedef SmartProjectionHessianFactor<Pose3,Point3,Cal3Bundler> SmartFactorBundler;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionHessianFactor, Constructor) { TEST( SmartProjectionHessianFactor, Constructor) {
@ -938,107 +940,203 @@ TEST( SmartProjectionHessianFactor, HessianWithRotationDegenerate ){
} }
/* ************************************************************************* */ /* ************************************************************************* */
//TEST( SmartProjectionHessianFactor, ConstructorWithCal3Bundler) { TEST( SmartProjectionHessianFactor, ConstructorWithCal3Bundler) {
// SmartProjectionHessianFactor<Pose3,Point3,Cal3Bundler> factor1(rankTol, linThreshold); SmartProjectionHessianFactor<Pose3,Point3,Cal3Bundler> factor1(rankTol, linThreshold);
// boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
// factor1.add(measurement1, poseKey1, model, Kbundler); factor1.add(measurement1, poseKey1, model, Kbundler);
//} }
/* *************************************************************************/ /* *************************************************************************/
//TEST( SmartProjectionHessianFactor, Cal3Bundler ){ TEST( SmartProjectionHessianFactor, Cal3Bundler ){
// // cout << " ************************ SmartProjectionHessianFactor: Cal3Bundler **********************" << endl; // cout << " ************************ SmartProjectionHessianFactor: Cal3Bundler **********************" << endl;
//
// Cal3Bundler Kbundler(500, 1e-3, 1e-3, 1000, 2000);
//
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
// PinholeCamera<Cal3Bundler> cam1(pose1, Kbundler);
//
// // create second camera 1 meter to the right of first camera
// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
// PinholeCamera<Cal3Bundler> cam2(pose2, Kbundler);
//
// // create third camera 1 meter above the first camera
// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
// PinholeCamera<Cal3Bundler> cam3(pose3, Kbundler);
//
// // three landmarks ~5 meters infront of camera
// Point3 landmark1(5, 0.5, 1.2);
// Point3 landmark2(5, -0.5, 1.2);
// Point3 landmark3(3, 0, 3.0);
//
// vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
//
// // 1. Project three landmarks into three cameras and triangulate
// Point2 cam1_uv1 = cam1.project(landmark1);
// Point2 cam2_uv1 = cam2.project(landmark1);
// Point2 cam3_uv1 = cam3.project(landmark1);
// measurements_cam1.push_back(cam1_uv1);
// measurements_cam1.push_back(cam2_uv1);
// measurements_cam1.push_back(cam3_uv1);
//
// Point2 cam1_uv2 = cam1.project(landmark2);
// Point2 cam2_uv2 = cam2.project(landmark2);
// Point2 cam3_uv2 = cam3.project(landmark2);
// measurements_cam2.push_back(cam1_uv2);
// measurements_cam2.push_back(cam2_uv2);
// measurements_cam2.push_back(cam3_uv2);
//
// Point2 cam1_uv3 = cam1.project(landmark3);
// Point2 cam2_uv3 = cam2.project(landmark3);
// Point2 cam3_uv3 = cam3.project(landmark3);
// measurements_cam3.push_back(cam1_uv3);
// measurements_cam3.push_back(cam2_uv3);
// measurements_cam3.push_back(cam3_uv3);
//
// std::vector<Key> views;
// views.push_back(x1);
// views.push_back(x2);
// views.push_back(x3);
// SmartProjectionHessianFactor<Pose3, Point3, Cal3Bundler>::shared_ptr smartFactor1(new SmartFactor()); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
// smartFactor1->add(measurements_cam1, views, model, &Kbundler); Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
// PinholeCamera<Cal3Bundler> cam1(pose1, *Kbundler);
// SmartProjectionHessianFactor<Pose3, Point3, Cal3Bundler>::shared_ptr smartFactor2(new SmartFactor());
// smartFactor2->add(measurements_cam2, views, model, &Kbundler); // create second camera 1 meter to the right of first camera
// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
// SmartProjectionHessianFactor<Pose3, Point3, Cal3Bundler>::shared_ptr smartFactor3(new SmartFactor()); PinholeCamera<Cal3Bundler> cam2(pose2, *Kbundler);
// smartFactor3->add(measurements_cam3, views, model, &Kbundler);
// // create third camera 1 meter above the first camera
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
// PinholeCamera<Cal3Bundler> cam3(pose3, *Kbundler);
// NonlinearFactorGraph graph;
// graph.push_back(smartFactor1); // three landmarks ~5 meters infront of camera
// graph.push_back(smartFactor2); Point3 landmark1(5, 0.5, 1.2);
// graph.push_back(smartFactor3); Point3 landmark2(5, -0.5, 1.2);
// graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior)); Point3 landmark3(3, 0, 3.0);
// graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
// vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise // 1. Project three landmarks into three cameras and triangulate
// Values values; Point2 cam1_uv1 = cam1.project(landmark1);
// values.insert(x1, pose1); Point2 cam2_uv1 = cam2.project(landmark1);
// values.insert(x2, pose2); Point2 cam3_uv1 = cam3.project(landmark1);
// // initialize third pose with some noise, we expect it to move back to original pose3 measurements_cam1.push_back(cam1_uv1);
// values.insert(x3, pose3*noise_pose); measurements_cam1.push_back(cam2_uv1);
// if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: "); measurements_cam1.push_back(cam3_uv1);
//
// LevenbergMarquardtParams params; Point2 cam1_uv2 = cam1.project(landmark2);
// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; Point2 cam2_uv2 = cam2.project(landmark2);
// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; Point2 cam3_uv2 = cam3.project(landmark2);
// measurements_cam2.push_back(cam1_uv2);
// Values result; measurements_cam2.push_back(cam2_uv2);
// gttic_(SmartProjectionHessianFactor); measurements_cam2.push_back(cam3_uv2);
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
// result = optimizer.optimize(); Point2 cam1_uv3 = cam1.project(landmark3);
// gttoc_(SmartProjectionHessianFactor); Point2 cam2_uv3 = cam2.project(landmark3);
// tictoc_finishedIteration_(); Point2 cam3_uv3 = cam3.project(landmark3);
// measurements_cam3.push_back(cam1_uv3);
// // result.print("results of 3 camera, 3 landmark optimization \n"); measurements_cam3.push_back(cam2_uv3);
// if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: "); measurements_cam3.push_back(cam3_uv3);
// EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
// if(isDebugTest) tictoc_print_(); std::vector<Key> views;
// } views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
SmartProjectionHessianFactor<Pose3, Point3, Cal3Bundler>::shared_ptr smartFactor1(new SmartFactorBundler());
smartFactor1->add(measurements_cam1, views, model, Kbundler);
SmartProjectionHessianFactor<Pose3, Point3, Cal3Bundler>::shared_ptr smartFactor2(new SmartFactorBundler());
smartFactor2->add(measurements_cam2, views, model, Kbundler);
SmartProjectionHessianFactor<Pose3, Point3, Cal3Bundler>::shared_ptr smartFactor3(new SmartFactorBundler());
smartFactor3->add(measurements_cam3, views, model, Kbundler);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
Values values;
values.insert(x1, pose1);
values.insert(x2, pose2);
// initialize third pose with some noise, we expect it to move back to original pose3
values.insert(x3, pose3*noise_pose);
if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
LevenbergMarquardtParams params;
if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
gttic_(SmartProjectionHessianFactor);
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
gttoc_(SmartProjectionHessianFactor);
tictoc_finishedIteration_();
// result.print("results of 3 camera, 3 landmark optimization \n");
if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(pose3,result.at<Pose3>(x3), 1e-6));
if(isDebugTest) tictoc_print_();
}
/* *************************************************************************/
TEST( SmartProjectionHessianFactor, Cal3BundlerRotationOnly ){
std::vector<Key> views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
PinholeCamera<Cal3Bundler> cam1(pose1, *Kbundler);
// create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
PinholeCamera<Cal3Bundler> cam2(pose2, *Kbundler);
// create third camera 1 meter above the first camera
Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
PinholeCamera<Cal3Bundler> cam3(pose3, *Kbundler);
// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2);
Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(3, 0, 3.0);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
// 1. Project three landmarks into three cameras and triangulate
Point2 cam1_uv1 = cam1.project(landmark1);
Point2 cam2_uv1 = cam2.project(landmark1);
Point2 cam3_uv1 = cam3.project(landmark1);
measurements_cam1.push_back(cam1_uv1);
measurements_cam1.push_back(cam2_uv1);
measurements_cam1.push_back(cam3_uv1);
Point2 cam1_uv2 = cam1.project(landmark2);
Point2 cam2_uv2 = cam2.project(landmark2);
Point2 cam3_uv2 = cam3.project(landmark2);
measurements_cam2.push_back(cam1_uv2);
measurements_cam2.push_back(cam2_uv2);
measurements_cam2.push_back(cam3_uv2);
Point2 cam1_uv3 = cam1.project(landmark3);
Point2 cam2_uv3 = cam2.project(landmark3);
Point2 cam3_uv3 = cam3.project(landmark3);
measurements_cam3.push_back(cam1_uv3);
measurements_cam3.push_back(cam2_uv3);
measurements_cam3.push_back(cam3_uv3);
double rankTol = 10;
SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler(rankTol));
smartFactor1->add(measurements_cam1, views, model, Kbundler);
SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler(rankTol));
smartFactor2->add(measurements_cam2, views, model, Kbundler);
SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler(rankTol));
smartFactor3->add(measurements_cam3, views, model, Kbundler);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
Point3 positionPrior = gtsam::Point3(0,0,1);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
Values values;
values.insert(x1, pose1);
values.insert(x2, pose2);
// initialize third pose with some noise, we expect it to move back to original pose3
values.insert(x3, pose3*noise_pose);
if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
LevenbergMarquardtParams params;
if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
gttic_(SmartProjectionHessianFactor);
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
gttoc_(SmartProjectionHessianFactor);
tictoc_finishedIteration_();
// result.print("results of 3 camera, 3 landmark optimization \n");
if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
if(isDebugTest) tictoc_print_();
}
/* ************************************************************************* */ /* ************************************************************************* */