added tests on smartHessianFactor with Cal3Bundler
Luca Carlone
parent
38c91e1913
commit
92f0bb64b2
|
|
@ -20,61 +20,61 @@
|
|||
1 6 543.18011474609375 294.80999755859375
|
||||
2 6 -58.419979095458984375 110.8300018310546875
|
||||
|
||||
-0.016943983733654022217
|
||||
0.011171804741024972743
|
||||
0.0024643507786095142365
|
||||
0.73030996322631824835
|
||||
-0.26490819454193109683
|
||||
-1.712789297103882058
|
||||
0.29656188120312942935
|
||||
-0.035318354384285870207
|
||||
0.31252101755032046793
|
||||
0.47230274932665988752
|
||||
-0.3572340863744113415
|
||||
-2.0517704282499575896
|
||||
1430.031982421875
|
||||
-7.5572756941255647689e-08
|
||||
3.2377570134516087119e-14
|
||||
|
||||
0.015049725770950320852
|
||||
-0.18504564464092254639
|
||||
-0.29278403520584106445
|
||||
-1.0590475797653198242
|
||||
-0.036017861217260457862
|
||||
-1.572034001350402832
|
||||
0.28532097381985194184
|
||||
-0.27699838370789808817
|
||||
0.048601169984112867206
|
||||
-1.2598695987143850861
|
||||
-0.049063798188844320869
|
||||
-1.9586867140445654023
|
||||
1432.137451171875
|
||||
-7.3171918302250560373e-08
|
||||
3.1759419042137054801e-14
|
||||
|
||||
-0.30793598294258112125
|
||||
0.32077908515930175781
|
||||
0.2225398570299148282
|
||||
8.5034484863281267764
|
||||
6.7499604225158700288
|
||||
-3.6383814811706534087
|
||||
0.057491325683772541433
|
||||
0.34853090049579965592
|
||||
0.47985129303736057116
|
||||
8.1963904289063389541
|
||||
6.5146840788718787252
|
||||
-3.8392804395897406344
|
||||
1572.047119140625
|
||||
-1.5962623223231275915e-08
|
||||
-1.6507904730136101212e-14
|
||||
|
||||
-12.055994987487792969
|
||||
12.838775634765625
|
||||
-41.099369049072265625
|
||||
-11.317351620610928364
|
||||
3.3594874875767186673
|
||||
-42.755222607849105998
|
||||
|
||||
6.4168906211853027344
|
||||
0.38897031545639038086
|
||||
-23.586282730102539062
|
||||
4.2648515634753199066
|
||||
-8.4629358700849355301
|
||||
-22.252086323427270997
|
||||
|
||||
13.051100730895996094
|
||||
3.8387587070465087891
|
||||
-29.777933120727539062
|
||||
10.996977688149536689
|
||||
-9.2123370180278048025
|
||||
-29.206739014051372294
|
||||
|
||||
13.060946464538574219
|
||||
3.5910520553588867188
|
||||
-29.759080886840820312
|
||||
10.935342607054865383
|
||||
-9.4338917557810741954
|
||||
-29.112263909175499776
|
||||
|
||||
14.265764236450195312
|
||||
24.096216201782226562
|
||||
-54.823970794677734375
|
||||
15.714024935401759819
|
||||
1.3745079651566265433
|
||||
-59.286834979937104606
|
||||
|
||||
-0.25292283296585083008
|
||||
2.2166082859039306641
|
||||
-21.712127685546875
|
||||
-1.3624227800805182031
|
||||
-4.1979357415396094666
|
||||
-21.034430148188398846
|
||||
|
||||
7.6465740203857421875
|
||||
14.185332298278808594
|
||||
-52.070301055908203125
|
||||
6.7690173115899296974
|
||||
-4.7352452433700786827
|
||||
-53.605307875695892506
|
||||
|
||||
|
|
|
|||
|
|
@ -53,9 +53,10 @@ static bool isDebugTest = false;
|
|||
// make a realistic calibration matrix
|
||||
static double fov = 60; // degrees
|
||||
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 boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
|
||||
|
||||
static double rankTol = 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));
|
||||
|
||||
typedef SmartProjectionHessianFactor<Pose3,Point3,Cal3_S2> SmartFactor;
|
||||
typedef SmartProjectionHessianFactor<Pose3,Point3,Cal3Bundler> SmartFactorBundler;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionHessianFactor, Constructor) {
|
||||
|
|
@ -938,107 +940,203 @@ TEST( SmartProjectionHessianFactor, HessianWithRotationDegenerate ){
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//TEST( SmartProjectionHessianFactor, ConstructorWithCal3Bundler) {
|
||||
// SmartProjectionHessianFactor<Pose3,Point3,Cal3Bundler> factor1(rankTol, linThreshold);
|
||||
// boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
|
||||
// factor1.add(measurement1, poseKey1, model, Kbundler);
|
||||
//}
|
||||
TEST( SmartProjectionHessianFactor, ConstructorWithCal3Bundler) {
|
||||
SmartProjectionHessianFactor<Pose3,Point3,Cal3Bundler> factor1(rankTol, linThreshold);
|
||||
boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
|
||||
factor1.add(measurement1, poseKey1, model, Kbundler);
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
//TEST( SmartProjectionHessianFactor, Cal3Bundler ){
|
||||
// // 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);
|
||||
TEST( SmartProjectionHessianFactor, Cal3Bundler ){
|
||||
// cout << " ************************ SmartProjectionHessianFactor: Cal3Bundler **********************" << endl;
|
||||
|
||||
// SmartProjectionHessianFactor<Pose3, Point3, Cal3Bundler>::shared_ptr smartFactor1(new SmartFactor());
|
||||
// smartFactor1->add(measurements_cam1, views, model, &Kbundler);
|
||||
//
|
||||
// SmartProjectionHessianFactor<Pose3, Point3, Cal3Bundler>::shared_ptr smartFactor2(new SmartFactor());
|
||||
// smartFactor2->add(measurements_cam2, views, model, &Kbundler);
|
||||
//
|
||||
// SmartProjectionHessianFactor<Pose3, Point3, Cal3Bundler>::shared_ptr smartFactor3(new SmartFactor());
|
||||
// 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)));
|
||||
// if(isDebugTest) tictoc_print_();
|
||||
// }
|
||||
// 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 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_();
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue