From a60e13dd094c8ed8fa0c94d4a5739c0f74e07a98 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 23 Feb 2015 20:43:31 +0100 Subject: [PATCH] Remove a whole lot of copy/paste --- .../tests/testSmartProjectionCameraFactor.cpp | 540 +++++++++--------- 1 file changed, 256 insertions(+), 284 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index b9634025b..993cefea8 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -18,10 +18,7 @@ * @date Sept 2013 */ -//#include "BundlerDefinitions.h" #include -//#include "../SmartFactorsTestProblems.h" -//#include "../LevenbergMarquardtOptimizerCERES.h" #include #include @@ -39,17 +36,11 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -typedef PinholeCamera Camera; - 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 K2(new Cal3_S2(1500, 1200, 0, 640, 480)); -static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 0, 0)); +static size_t w = 640, h = 480; static double rankTol = 1.0; static double linThreshold = -1.0; @@ -61,20 +52,20 @@ static SharedNoiseModel model(noiseModel::Unit::Create(2)); using symbol_shorthand::X; using symbol_shorthand::L; -// tests data -Key x1 = 1; - +static Key x1(1); Symbol l1('l', 1), l2('l', 2), l3('l', 3); Key c1 = 1, c2 = 2, c3 = 3; -static Key poseKey1(x1); -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)); +// First camera pose, looking along X-axis, 1 meter above ground plane (x-y) +Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); +// Second camera 1 meter to the right of first camera +Pose3 pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); +// Third camera 1 meter above the first camera +Pose3 pose_up = level_pose * Pose3(Rot3(), Point3(0, -1, 0)); -typedef PinholeCamera CameraCal3_S2; -typedef SmartProjectionCameraFactor SmartFactor; -typedef SmartProjectionCameraFactor SmartFactorBundler; -typedef GeneralSFMFactor SFMFactor; +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)); template PinholeCamera perturbCameraPose( @@ -95,7 +86,8 @@ PinholeCamera perturbCameraPoseAndCalibration( Pose3 cameraPose = camera.pose(); Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); typename gtsam::traits::TangentVector d; - d.setRandom(); d*=0.1; + d.setRandom(); + d *= 0.1; CALIBRATION perturbedCalibration = camera.calibration().retract(d); return PinholeCamera(perturbedCameraPose, perturbedCalibration); } @@ -114,70 +106,83 @@ void projectToMultipleCameras(const PinholeCamera& cam1, } /* ************************************************************************* */ -TEST( SmartProjectionCameraFactor, perturbCameraPose) { - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); - CameraCal3_S2 level_camera(level_pose, *K2); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); - Pose3 perturbed_level_pose = level_pose.compose(noise_pose); - CameraCal3_S2 actualCamera(perturbed_level_pose, *K2); +// default Cal3_S2 cameras +namespace vanilla { +typedef PinholeCamera Camera; +static Cal3_S2 K(fov, w, h); +static Cal3_S2 K2(1500, 1200, 0, 640, 480); +Camera level_camera(level_pose, K2); +Camera level_camera_right(pose_right, K2); +Camera cam1(level_pose, K2); +Camera cam2(pose_right, K2); +Camera cam3(pose_up, K2); +typedef GeneralSFMFactor SFMFactor; +typedef SmartProjectionCameraFactor SmartFactor; +} - CameraCal3_S2 expectedCamera = perturbCameraPose(level_camera); +/* ************************************************************************* */ +TEST( SmartProjectionCameraFactor, perturbCameraPose) { + using namespace vanilla; + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Pose3 perturbed_level_pose = level_pose.compose(noise_pose); + Camera actualCamera(perturbed_level_pose, K2); + + Camera expectedCamera = perturbCameraPose(level_camera); CHECK(assert_equal(expectedCamera, actualCamera)); } /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor) { + using namespace vanilla; SmartFactor::shared_ptr factor1(new SmartFactor()); } /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor2) { + using namespace vanilla; SmartFactor factor1(rankTol, linThreshold); } /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor3) { + using namespace vanilla; SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, poseKey1, model); + factor1->add(measurement1, x1, model); } /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor4) { + using namespace vanilla; SmartFactor factor1(rankTol, linThreshold); - factor1.add(measurement1, poseKey1, model); + factor1.add(measurement1, x1, model); } /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, ConstructorWithTransform) { + using namespace vanilla; bool manageDegeneracy = true; bool isImplicit = false; bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, isImplicit, enableEPI, body_P_sensor1); - factor1.add(measurement1, poseKey1, model); + SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, isImplicit, + enableEPI, body_P_sensor1); + factor1.add(measurement1, x1, model); } /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Equals ) { + using namespace vanilla; SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, poseKey1, model); + factor1->add(measurement1, x1, model); SmartFactor::shared_ptr factor2(new SmartFactor()); - factor2->add(measurement1, poseKey1, model); - //CHECK(assert_equal(*factor1, *factor2)); + factor2->add(measurement1, x1, model); } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, noiseless ){ +TEST( SmartProjectionCameraFactor, noiseless ) { // cout << " ************************ SmartProjectionCameraFactor: noisy ****************************" << endl; - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); - CameraCal3_S2 level_camera(level_pose, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - CameraCal3_S2 level_camera_right(level_pose_right, *K2); + using namespace vanilla; // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); @@ -194,90 +199,41 @@ TEST( SmartProjectionCameraFactor, noiseless ){ factor1->add(level_uv, c1, model); factor1->add(level_uv_right, c2, model); - double actualError = factor1->error(values); - double expectedError = 0.0; - DOUBLES_EQUAL(expectedError, actualError, 1e-7); + DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7); + CHECK(assert_equal(zero(4), factor1->reprojectionError(values), 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, noiselessBundler ){ +TEST( SmartProjectionCameraFactor, noisy ) { - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); - Camera level_camera(level_pose, *Kbundler); - - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - Camera level_camera_right(level_pose_right, *Kbundler); + using namespace vanilla; // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate - Point2 level_uv = level_camera.project(landmark); - Point2 level_uv_right = level_camera_right.project(landmark); - - Values values; - values.insert(c1, level_camera); - values.insert(c2, level_camera_right); - - SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler()); - factor1->add(level_uv, c1, model); - factor1->add(level_uv_right, c2, model); - - double actualError = factor1->error(values); - - double expectedError = 0.0; - DOUBLES_EQUAL(expectedError, actualError, 1e-3); - - Point3 oldPoint; // this takes the point stored in the factor (we are not interested in this) - if(factor1->point()) - oldPoint = *(factor1->point()); - - Point3 expectedPoint; - if(factor1->point(values)) - expectedPoint = *(factor1->point(values)); - - EXPECT(assert_equal(expectedPoint,landmark, 1e-3)); -} - -/* *************************************************************************/ -TEST( SmartProjectionCameraFactor, noisy ){ - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); - CameraCal3_S2 level_camera(level_pose, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - CameraCal3_S2 level_camera_right(level_pose_right, *K2); - - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); - - // 1. Project two landmarks into two cameras and triangulate - Point2 pixelError(0.2,0.2); + Point2 pixelError(0.2, 0.2); Point2 level_uv = level_camera.project(landmark) + pixelError; Point2 level_uv_right = level_camera_right.project(landmark); Values values; values.insert(c1, level_camera); - CameraCal3_S2 perturbed_level_camera_right = perturbCameraPose(level_camera_right); + Camera perturbed_level_camera_right = perturbCameraPose(level_camera_right); values.insert(c2, perturbed_level_camera_right); SmartFactor::shared_ptr factor1(new SmartFactor()); factor1->add(level_uv, c1, model); factor1->add(level_uv_right, c2, model); - double actualError1= factor1->error(values); + double actualError1 = factor1->error(values); SmartFactor::shared_ptr factor2(new SmartFactor()); vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - vector< SharedNoiseModel > noises; + vector noises; noises.push_back(model); noises.push_back(model); @@ -287,26 +243,15 @@ TEST( SmartProjectionCameraFactor, noisy ){ factor2->add(measurements, views, noises); - double actualError2= factor2->error(values); + double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } - /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ){ +TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { - // 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), Point3(0,0,1)); - CameraCal3_S2 cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - CameraCal3_S2 cam2(pose2, *K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - CameraCal3_S2 cam3(pose3, *K2); + using namespace vanilla; // three landmarks ~5 meters infront of camera Point3 landmark1(5, 0.5, 1.2); @@ -334,24 +279,27 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ){ SmartFactor::shared_ptr smartFactor3(new SmartFactor()); smartFactor3->add(measurements_cam3, views, model); - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6+5, 1e-5); + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); Values values; values.insert(c1, cam1); values.insert(c2, cam2); values.insert(c3, perturbCameraPose(cam3)); - if(isDebugTest) values.at(c3).print("Smart: Pose3 before optimization: "); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; gttic_(SmartProjectionCameraFactor); @@ -363,28 +311,21 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ){ // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); // VectorValues delta = GFG->optimize(); - if(isDebugTest) result.at(c3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(cam1,result.at(c1))); - EXPECT(assert_equal(cam2,result.at(c2))); - EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-4)); - EXPECT(assert_equal(result.at(c3).calibration(), cam3.calibration(), 2)); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1))); + EXPECT(assert_equal(cam2, result.at(c2))); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-4)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 2)); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ){ +TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { - // 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), Point3(0,0,1)); - CameraCal3_S2 cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - CameraCal3_S2 cam2(pose2, *K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - CameraCal3_S2 cam3(pose3, *K2); + using namespace vanilla; // three landmarks ~5 meters infront of camera Point3 landmark1(5, 0.5, 1.2); @@ -403,9 +344,9 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ){ views.push_back(c3); SfM_Track track1; - for(size_t i=0;i<3;++i){ + for (size_t i = 0; i < 3; ++i) { SfM_Measurement measures; - measures.first = i+1;// cameras are from 1 to 3 + measures.first = i + 1; // cameras are from 1 to 3 measures.second = measurements_cam1.at(i); track1.measurements.push_back(measures); } @@ -413,9 +354,9 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ){ smartFactor1->add(track1, model); SfM_Track track2; - for(size_t i=0;i<3;++i){ + for (size_t i = 0; i < 3; ++i) { SfM_Measurement measures; - measures.first = i+1;// cameras are from 1 to 3 + measures.first = i + 1; // cameras are from 1 to 3 measures.second = measurements_cam2.at(i); track2.measurements.push_back(measures); } @@ -425,24 +366,27 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ){ SmartFactor::shared_ptr smartFactor3(new SmartFactor()); smartFactor3->add(measurements_cam3, views, model); - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6+5, 1e-5); + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); Values values; values.insert(c1, cam1); values.insert(c2, cam2); values.insert(c3, perturbCameraPose(cam3)); - if(isDebugTest) values.at(c3).print("Smart: Pose3 before optimization: "); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; gttic_(SmartProjectionCameraFactor); @@ -454,28 +398,21 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ){ // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); // VectorValues delta = GFG->optimize(); - if(isDebugTest) result.at(c3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(cam1,result.at(c1))); - EXPECT(assert_equal(cam2,result.at(c2))); - EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-4)); - EXPECT(assert_equal(result.at(c3).calibration(), cam3.calibration(), 2)); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1))); + EXPECT(assert_equal(cam2, result.at(c2))); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-4)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 2)); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ){ +TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { - // 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), Point3(0,0,1)); - CameraCal3_S2 cam1(pose1, *K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - CameraCal3_S2 cam2(pose2, *K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - CameraCal3_S2 cam3(pose3, *K2); + using namespace vanilla; // three landmarks ~5 meters infront of camera Point3 landmark1(5, 0.5, 1.2); @@ -485,7 +422,7 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ){ Point3 landmark5(10, -0.5, 1.2); vector measurements_cam1, measurements_cam2, measurements_cam3, - measurements_cam4, measurements_cam5; + measurements_cam4, measurements_cam5; // 1. Project three landmarks into three cameras and triangulate projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -514,7 +451,7 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ){ SmartFactor::shared_ptr smartFactor5(new SmartFactor()); smartFactor5->add(measurements_cam5, views, model); - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6+5, 1e-5); + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5); NonlinearFactorGraph graph; graph.push_back(smartFactor1); @@ -522,21 +459,24 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ){ graph.push_back(smartFactor3); graph.push_back(smartFactor4); graph.push_back(smartFactor5); - graph.push_back(PriorFactor(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.push_back(PriorFactor(c1, cam1, noisePrior)); + graph.push_back(PriorFactor(c2, cam2, noisePrior)); Values values; values.insert(c1, cam1); values.insert(c2, cam2); values.insert(c3, perturbCameraPoseAndCalibration(cam3)); - if(isDebugTest) values.at(c3).print("Smart: Pose3 before optimization: "); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; params.relativeErrorTol = 1e-8; params.absoluteErrorTol = 0; params.maxIterations = 20; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; gttic_(SmartProjectionCameraFactor); @@ -548,28 +488,36 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ){ // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); // VectorValues delta = GFG->optimize(); - if(isDebugTest) result.at(c3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(cam1,result.at(c1))); - EXPECT(assert_equal(cam2,result.at(c2))); - EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1)); - EXPECT(assert_equal(result.at(c3).calibration(), cam3.calibration(), 20)); - if(isDebugTest) tictoc_print_(); + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1))); + EXPECT(assert_equal(cam2, result.at(c2))); + EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1)); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 20)); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, Cal3Bundler ){ +// Cal3Bundler cameras +namespace bundler { +typedef PinholeCamera Camera; +static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0); +Camera level_camera(level_pose, K); +Camera level_camera_right(pose_right, K); +Pose3 pose1 = level_pose; +Camera cam1(level_pose, K); +Camera cam2(pose_right, K); +Camera cam3(pose_up, K); +typedef GeneralSFMFactor SFMFactor; +typedef SmartProjectionCameraFactor SmartFactorBundler; +} - // 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), Point3(0,0,1)); - Camera cam1(pose1, *Kbundler); +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, Cal3Bundler ) { - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - Camera cam2(pose2, *Kbundler); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - Camera cam3(pose3, *Kbundler); + using namespace bundler; // three landmarks ~5 meters infront of camera Point3 landmark1(5, 0.5, 1.2); @@ -579,7 +527,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ){ Point3 landmark5(10, -0.5, 1.2); vector measurements_cam1, measurements_cam2, measurements_cam3, - measurements_cam4, measurements_cam5; + measurements_cam4, measurements_cam5; // 1. Project three landmarks into three cameras and triangulate projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -622,14 +570,17 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ){ values.insert(c2, cam2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(c3, perturbCameraPose(cam3)); - if(isDebugTest) values.at(c3).print("Smart: Pose3 before optimization: "); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; params.relativeErrorTol = 1e-8; params.absoluteErrorTol = 0; params.maxIterations = 20; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; gttic_(SmartProjectionCameraFactor); @@ -638,28 +589,21 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ){ gttoc_(SmartProjectionCameraFactor); tictoc_finishedIteration_(); - if(isDebugTest) result.at(c3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(cam1,result.at(c1), 1e-4)); - EXPECT(assert_equal(cam2,result.at(c2), 1e-4)); + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1), 1e-4)); + EXPECT(assert_equal(cam2, result.at(c2), 1e-4)); EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1)); - EXPECT(assert_equal(result.at(c3).calibration(), cam3.calibration(), 1)); - if(isDebugTest) tictoc_print_(); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 1)); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, Cal3Bundler2 ){ +TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { - // 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), Point3(0,0,1)); - Camera cam1(pose1, *Kbundler); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - Camera cam2(pose2, *Kbundler); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - Camera cam3(pose3, *Kbundler); + using namespace bundler; // three landmarks ~5 meters infront of camera Point3 landmark1(5, 0.5, 1.2); @@ -669,7 +613,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ){ Point3 landmark5(10, -0.5, 1.2); vector measurements_cam1, measurements_cam2, measurements_cam3, - measurements_cam4, measurements_cam5; + measurements_cam4, measurements_cam5; // 1. Project three landmarks into three cameras and triangulate projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -712,14 +656,17 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ){ values.insert(c2, cam2); // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(c3, perturbCameraPoseAndCalibration(cam3)); - if(isDebugTest) values.at(c3).print("Smart: Pose3 before optimization: "); + if (isDebugTest) + values.at(c3).print("Smart: Pose3 before optimization: "); LevenbergMarquardtParams params; params.relativeErrorTol = 1e-8; params.absoluteErrorTol = 0; params.maxIterations = 20; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; + if (isDebugTest) + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + if (isDebugTest) + params.verbosity = NonlinearOptimizerParams::ERROR; Values result; gttic_(SmartProjectionCameraFactor); @@ -728,25 +675,56 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ){ gttoc_(SmartProjectionCameraFactor); tictoc_finishedIteration_(); - if(isDebugTest) result.at(c3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(cam1,result.at(c1), 1e-4)); - EXPECT(assert_equal(cam2,result.at(c2), 1e-4)); + if (isDebugTest) + result.at(c3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(cam1, result.at(c1), 1e-4)); + EXPECT(assert_equal(cam2, result.at(c2), 1e-4)); EXPECT(assert_equal(result.at(c3).pose(), cam3.pose(), 1e-1)); - EXPECT(assert_equal(result.at(c3).calibration(), cam3.calibration(), 2)); - if(isDebugTest) tictoc_print_(); + EXPECT( + assert_equal(result.at(c3).calibration(), cam3.calibration(), 2)); + if (isDebugTest) + tictoc_print_(); } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ){ +TEST( SmartProjectionCameraFactor, noiselessBundler ) { - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); - Camera level_camera(level_pose, *Kbundler); + using namespace bundler; + // landmark ~5 meters infront of camera + Point3 landmark(5, 0.5, 1.2); - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - Camera level_camera_right(level_pose_right, *Kbundler); + // 1. Project two landmarks into two cameras and triangulate + Point2 level_uv = level_camera.project(landmark); + Point2 level_uv_right = level_camera_right.project(landmark); + Values values; + values.insert(c1, level_camera); + values.insert(c2, level_camera_right); + + SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler()); + factor1->add(level_uv, c1, model); + factor1->add(level_uv_right, c2, model); + + double actualError = factor1->error(values); + + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-3); + + Point3 oldPoint; // this takes the point stored in the factor (we are not interested in this) + if (factor1->point()) + oldPoint = *(factor1->point()); + + Point3 expectedPoint; + if (factor1->point(values)) + expectedPoint = *(factor1->point(values)); + + EXPECT(assert_equal(expectedPoint, landmark, 1e-3)); +} + +/* *************************************************************************/ +TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) { + + using namespace bundler; // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); @@ -766,7 +744,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ){ double expectedError = factor1->error(values); double expectedErrorGraph = smartGraph.error(values); Point3 expectedPoint; - if(factor1->point()) + if (factor1->point()) expectedPoint = *(factor1->point()); // cout << "expectedPoint " << expectedPoint.vector() << endl; @@ -779,9 +757,10 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ){ generalGraph.push_back(sfm1); generalGraph.push_back(sfm2); values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation - Vector e1 = sfm1.evaluateError(values.at< Camera >(c1), values.at< Point3 >(l1)); - Vector e2 = sfm2.evaluateError(values.at< Camera >(c2), values.at< Point3 >(l1)); - double actualError = 0.5 * (norm_2(e1)*norm_2(e1) + norm_2(e2)*norm_2(e2)); + Vector e1 = sfm1.evaluateError(values.at(c1), values.at(l1)); + Vector e2 = sfm2.evaluateError(values.at(c2), values.at(l1)); + double actualError = 0.5 + * (norm_2(e1) * norm_2(e1) + norm_2(e2) * norm_2(e2)); double actualErrorGraph = generalGraph.error(values); DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7); @@ -791,14 +770,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ){ } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ){ +TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) { - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); - Camera level_camera(level_pose, *Kbundler); - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - Camera level_camera_right(level_pose_right, *Kbundler); + using namespace bundler; // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate @@ -817,7 +791,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ){ Matrix expectedHessian = smartGraph.linearize(values)->hessian().first; Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second; Point3 expectedPoint; - if(factor1->point()) + if (factor1->point()) expectedPoint = *(factor1->point()); // COMMENTS: @@ -831,25 +805,23 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ){ values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first; Matrix actualFullInfoVector = generalGraph.linearize(values)->hessian().second; - Matrix actualHessian = actualFullHessian.block(0,0,18,18) - - actualFullHessian.block(0,18,18,3) * (actualFullHessian.block(18,18,3,3)).inverse() * actualFullHessian.block(18,0,3,18); - Vector actualInfoVector = actualFullInfoVector.block(0,0,18,1) - - actualFullHessian.block(0,18,18,3) * (actualFullHessian.block(18,18,3,3)).inverse() * actualFullInfoVector.block(18,0,3,1); + Matrix actualHessian = actualFullHessian.block(0, 0, 18, 18) + - actualFullHessian.block(0, 18, 18, 3) + * (actualFullHessian.block(18, 18, 3, 3)).inverse() + * actualFullHessian.block(18, 0, 3, 18); + Vector actualInfoVector = actualFullInfoVector.block(0, 0, 18, 1) + - actualFullHessian.block(0, 18, 18, 3) + * (actualFullHessian.block(18, 18, 3, 3)).inverse() + * actualFullInfoVector.block(18, 0, 3, 1); - EXPECT(assert_equal(expectedHessian,actualHessian, 1e-7)); - EXPECT(assert_equal(expectedInfoVector,actualInfoVector, 1e-7)); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-7)); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-7)); } /* *************************************************************************/ // Have to think about how to compare multiplyHessianAdd in generalSfMFactor and smartFactors //TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor2 ){ // -// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) -// Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); -// cameraObjectBundler level_camera(level_pose, *Kbundler); -// // create second camera 1 meter to the right of first camera -// Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); -// cameraObjectBundler level_camera_right(level_pose_right, *Kbundler); // // landmark ~5 meters infront of camera // Point3 landmark(5, 0.5, 1.2); // // 1. Project two landmarks into two cameras @@ -901,16 +873,10 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ){ // // EXPECT(assert_equal(yActual,yExpected, 1e-7)); //} - /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, computeImplicitJacobian ){ +TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) { - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); - Camera level_camera(level_pose, *Kbundler); - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - Camera level_camera_right(level_pose_right, *Kbundler); + using namespace bundler; // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate @@ -927,13 +893,13 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ){ Matrix expectedF, expectedE; Vector expectedb; - CameraSet< Camera > cameras; + CameraSet cameras; cameras.push_back(level_camera); cameras.push_back(level_camera_right); factor1->error(values); // this is important to have a triangulation of the point Point3 expectedPoint; - if(factor1->point()) + if (factor1->point()) expectedPoint = *(factor1->point()); factor1->computeJacobians(expectedF, expectedE, expectedb, cameras); @@ -944,21 +910,17 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ){ generalGraph.push_back(sfm2); values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first; - Matrix actualVinv = (actualFullHessian.block(18,18,3,3)).inverse(); + Matrix actualVinv = (actualFullHessian.block(18, 18, 3, 3)).inverse(); Matrix3 expectedVinv = factor1->PointCov(expectedE); - EXPECT(assert_equal(expectedVinv,actualVinv, 1e-7)); + EXPECT(assert_equal(expectedVinv, actualVinv, 1e-7)); } /* *************************************************************************/ -TEST( SmartProjectionCameraFactor, implicitJacobianFactor ){ +TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { + + using namespace bundler; - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1)); - Camera level_camera(level_pose, *Kbundler); - // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - Camera level_camera_right(level_pose_right, *Kbundler); // landmark ~5 meters infront of camera Point3 landmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate @@ -975,35 +937,45 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ){ bool isImplicit = false; // Hessian version - SmartFactorBundler::shared_ptr explicitFactor(new SmartFactorBundler(rankTol,linThreshold, manageDegeneracy, useEPI, isImplicit)); + SmartFactorBundler::shared_ptr explicitFactor( + new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy, useEPI, + isImplicit)); explicitFactor->add(level_uv, c1, model); explicitFactor->add(level_uv_right, c2, model); - GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize(values); - HessianFactor& hessianFactor = dynamic_cast(*gaussianHessianFactor); + GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize( + values); + HessianFactor& hessianFactor = + dynamic_cast(*gaussianHessianFactor); // Implicit Schur version isImplicit = true; - SmartFactorBundler::shared_ptr implicitFactor(new SmartFactorBundler(rankTol,linThreshold, manageDegeneracy, useEPI, isImplicit)); + SmartFactorBundler::shared_ptr implicitFactor( + new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy, useEPI, + isImplicit)); implicitFactor->add(level_uv, c1, model); implicitFactor->add(level_uv_right, c2, model); - GaussianFactor::shared_ptr gaussianImplicitSchurFactor = implicitFactor->linearize(values); + GaussianFactor::shared_ptr gaussianImplicitSchurFactor = + implicitFactor->linearize(values); typedef RegularImplicitSchurFactor<9> Implicit9; - Implicit9& implicitSchurFactor = dynamic_cast(*gaussianImplicitSchurFactor); + Implicit9& implicitSchurFactor = + dynamic_cast(*gaussianImplicitSchurFactor); - VectorValues x = map_list_of - (c1, (Vector(9) << 1,2,3,4,5,6,7,8,9).finished()) - (c2, (Vector(9) << 11,12,13,14,15,16,17,18,19).finished()); + VectorValues x = map_list_of(c1, + (Vector(9) << 1, 2, 3, 4, 5, 6, 7, 8, 9).finished())(c2, + (Vector(9) << 11, 12, 13, 14, 15, 16, 17, 18, 19).finished()); VectorValues yExpected, yActual; double alpha = 1.0; hessianFactor.multiplyHessianAdd(alpha, x, yActual); implicitSchurFactor.multiplyHessianAdd(alpha, x, yExpected); - EXPECT(assert_equal(yActual,yExpected, 1e-7)); + EXPECT(assert_equal(yActual, yExpected, 1e-7)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ -