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

Remove a whole lot of copy/paste

release/4.3a0
dellaert 2015-02-23 20:43:31 +01:00
parent f21ba05679
commit a60e13dd09
1 changed files with 256 additions and 284 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

540
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

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