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

Derivative tests, and re-formatting (and getting rid of gtsam::/std::)

release/4.3a0
dellaert 2015-02-20 00:57:27 +01:00
parent ed267ed69d
commit 4badb4a10f
2 changed files with 726 additions and 472 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

763
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

File diff suppressed because it is too large Load Diff

411
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -32,22 +32,24 @@ using namespace std;
using namespace boost::assign; using namespace boost::assign;
using namespace gtsam; using namespace gtsam;
static bool isDebugTest = true; 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 double b = 1; static double b = 1;
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov,w,h,b)); static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));
static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,b)); static Cal3_S2Stereo::shared_ptr K2(
static boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b));
static boost::shared_ptr<Cal3Bundler> Kbundler(
new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
static double rankTol = 1.0; static double rankTol = 1.0;
static double linThreshold = -1.0; static double linThreshold = -1.0;
// static bool manageDegeneracy = true; // static bool manageDegeneracy = true;
// Create a noise model for the pixel error // Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(3)); static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
// Convenience for named keys // Convenience for named keys
using symbol_shorthand::X; using symbol_shorthand::X;
@ -60,14 +62,14 @@ static Symbol x3('X', 3);
static Key poseKey1(x1); static Key poseKey1(x1);
static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value?
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor; typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor;
typedef SmartStereoProjectionPoseFactor<Cal3Bundler> SmartFactorBundler; typedef SmartStereoProjectionPoseFactor<Cal3Bundler> SmartFactorBundler;
vector<StereoPoint2> stereo_projectToMultipleCameras( vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
const StereoCamera& cam1, const StereoCamera& cam2, const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
const StereoCamera& cam3, Point3 landmark){
vector<StereoPoint2> measurements_cam; vector<StereoPoint2> measurements_cam;
@ -83,7 +85,7 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor) { TEST( SmartStereoProjectionPoseFactor, Constructor) {
fprintf(stderr,"Test 1 Complete"); fprintf(stderr, "Test 1 Complete");
SmartFactor::shared_ptr factor1(new SmartFactor()); SmartFactor::shared_ptr factor1(new SmartFactor());
} }
@ -108,7 +110,8 @@ TEST( SmartStereoProjectionPoseFactor, Constructor4) {
TEST( SmartStereoProjectionPoseFactor, ConstructorWithTransform) { TEST( SmartStereoProjectionPoseFactor, ConstructorWithTransform) {
bool manageDegeneracy = true; bool manageDegeneracy = true;
bool enableEPI = false; bool enableEPI = false;
SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1); SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI,
body_P_sensor1);
factor1.add(measurement1, poseKey1, model, K); factor1.add(measurement1, poseKey1, model, K);
} }
@ -124,15 +127,16 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
// cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl;
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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), gtsam::Point3(0,0,1)); Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0, 0, 1));
StereoCamera level_camera(level_pose, K2); StereoCamera level_camera(level_pose, K2);
// create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
StereoCamera level_camera_right(level_pose_right, K2); StereoCamera level_camera_right(level_pose_right, K2);
// landmark ~5 meters infront of camera // landmark ~5 meters infront of camera
@ -164,75 +168,78 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, noisy ){ TEST( SmartStereoProjectionPoseFactor, noisy ) {
// cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl; // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl;
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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), gtsam::Point3(0,0,1)); Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0, 0, 1));
StereoCamera level_camera(level_pose, K2); StereoCamera level_camera(level_pose, K2);
// create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
StereoCamera level_camera_right(level_pose_right, K2); StereoCamera 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);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
StereoPoint2 pixelError(0.2,0.2,0); StereoPoint2 pixelError(0.2, 0.2, 0);
StereoPoint2 level_uv = level_camera.project(landmark) + pixelError; StereoPoint2 level_uv = level_camera.project(landmark) + pixelError;
StereoPoint2 level_uv_right = level_camera_right.project(landmark); StereoPoint2 level_uv_right = level_camera_right.project(landmark);
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, level_pose);
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
values.insert(x2, level_pose_right.compose(noise_pose)); values.insert(x2, level_pose_right.compose(noise_pose));
SmartFactor::shared_ptr factor1(new SmartFactor()); SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, x1, model, K); factor1->add(level_uv, x1, model, K);
factor1->add(level_uv_right, x2, model, K); factor1->add(level_uv_right, x2, model, K);
double actualError1= factor1->error(values); double actualError1 = factor1->error(values);
SmartFactor::shared_ptr factor2(new SmartFactor()); SmartFactor::shared_ptr factor2(new SmartFactor());
vector<StereoPoint2> measurements; vector<StereoPoint2> measurements;
measurements.push_back(level_uv); measurements.push_back(level_uv);
measurements.push_back(level_uv_right); measurements.push_back(level_uv_right);
std::vector< SharedNoiseModel > noises; vector<SharedNoiseModel> noises;
noises.push_back(model); noises.push_back(model);
noises.push_back(model); noises.push_back(model);
std::vector< boost::shared_ptr<Cal3_S2Stereo> > Ks; ///< shared pointer to calibration object (one for each camera) vector<boost::shared_ptr<Cal3_S2Stereo> > Ks; ///< shared pointer to calibration object (one for each camera)
Ks.push_back(K); Ks.push_back(K);
Ks.push_back(K); Ks.push_back(K);
std::vector<Key> views; vector<Key> views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
factor2->add(measurements, views, noises, Ks); factor2->add(measurements, views, noises, Ks);
double actualError2= factor2->error(values); double actualError2 = factor2->error(values);
DOUBLES_EQUAL(actualError1, actualError2, 1e-7); DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; cout
<< " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************"
<< endl;
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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)); Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K2); StereoCamera cam1(pose1, K2);
// create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
StereoCamera cam2(pose2, K2); StereoCamera cam2(pose2, K2);
// create third camera 1 meter above the first camera // create third camera 1 meter above the first camera
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
StereoCamera cam3(pose3, K2); StereoCamera cam3(pose3, K2);
// three landmarks ~5 meters infront of camera // three landmarks ~5 meters infront of camera
@ -241,11 +248,14 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){
Point3 landmark3(3, 0, 3.0); Point3 landmark3(3, 0, 3.0);
// 1. Project three landmarks into three cameras and triangulate // 1. Project three landmarks into three cameras and triangulate
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); cam2, cam3, landmark1);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark2);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
std::vector<Key> views; vector<Key> views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -268,21 +278,25 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior)); graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, 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/10, 0., -M_PI/10), 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 Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, pose1); values.insert(x1, pose1);
values.insert(x2, pose2); values.insert(x2, pose2);
// 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(x3, pose3*noise_pose); values.insert(x3, pose3 * noise_pose);
if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: "); if (isDebugTest)
values.at<Pose3>(x3).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_(SmartStereoProjectionPoseFactor); gttic_ (SmartStereoProjectionPoseFactor);
LevenbergMarquardtOptimizer optimizer(graph, values, params); LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize(); result = optimizer.optimize();
gttoc_(SmartStereoProjectionPoseFactor); gttoc_(SmartStereoProjectionPoseFactor);
@ -292,28 +306,29 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){
// VectorValues delta = GFG->optimize(); // VectorValues delta = GFG->optimize();
// result.print("results of 3 camera, 3 landmark optimization \n"); // result.print("results of 3 camera, 3 landmark optimization \n");
if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: "); if (isDebugTest)
EXPECT(assert_equal(pose3,result.at<Pose3>(x3))); result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
if(isDebugTest) tictoc_print_(); EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
if (isDebugTest)
tictoc_print_();
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
std::vector<Key> views; vector<Key> views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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)); Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K); StereoCamera cam1(pose1, K);
// create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
StereoCamera cam2(pose2, K); StereoCamera cam2(pose2, K);
// create third camera 1 meter above the first camera // create third camera 1 meter above the first camera
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
StereoCamera cam3(pose3, K); StereoCamera cam3(pose3, K);
// three landmarks ~5 meters infront of camera // three landmarks ~5 meters infront of camera
@ -322,17 +337,23 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){
Point3 landmark3(3, 0, 3.0); Point3 landmark3(3, 0, 3.0);
// 1. Project three landmarks into three cameras and triangulate // 1. Project three landmarks into three cameras and triangulate
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); cam2, cam3, landmark1);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark2);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
smartFactor1->add(measurements_cam1, views, model, K); smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
smartFactor2->add(measurements_cam2, views, model, K); smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
smartFactor3->add(measurements_cam3, views, model, K); smartFactor3->add(measurements_cam3, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -344,38 +365,39 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior)); graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, 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/10, 0., -M_PI/10), 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 Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, pose1); values.insert(x1, pose1);
values.insert(x2, pose2); values.insert(x2, pose2);
values.insert(x3, pose3*noise_pose); values.insert(x3, pose3 * noise_pose);
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
Values result; Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params); LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize(); result = optimizer.optimize();
EXPECT(assert_equal(pose3,result.at<Pose3>(x3))); EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
double excludeLandmarksFutherThanDist = 2; double excludeLandmarksFutherThanDist = 2;
std::vector<Key> views; vector<Key> views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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)); Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K); StereoCamera cam1(pose1, K);
// create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
StereoCamera cam2(pose2, K); StereoCamera cam2(pose2, K);
// create third camera 1 meter above the first camera // create third camera 1 meter above the first camera
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
StereoCamera cam3(pose3, K); StereoCamera cam3(pose3, K);
// three landmarks ~5 meters infront of camera // three landmarks ~5 meters infront of camera
@ -384,18 +406,26 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){
Point3 landmark3(3, 0, 3.0); Point3 landmark3(3, 0, 3.0);
// 1. Project three landmarks into three cameras and triangulate // 1. Project three landmarks into three cameras and triangulate
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); cam2, cam3, landmark1);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark2);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
SmartFactor::shared_ptr smartFactor1(
SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor1->add(measurements_cam1, views, model, K); smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor2->add(measurements_cam2, views, model, K); smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor3->add(measurements_cam3, views, model, K); smartFactor3->add(measurements_cam3, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -407,40 +437,41 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior)); graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, 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/10, 0., -M_PI/10), 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 Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, pose1); values.insert(x1, pose1);
values.insert(x2, pose2); values.insert(x2, pose2);
values.insert(x3, pose3*noise_pose); values.insert(x3, pose3 * noise_pose);
// All factors are disabled and pose should remain where it is // All factors are disabled and pose should remain where it is
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
Values result; Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params); LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize(); result = optimizer.optimize();
EXPECT(assert_equal(values.at<Pose3>(x3),result.at<Pose3>(x3))); EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
double excludeLandmarksFutherThanDist = 1e10; double excludeLandmarksFutherThanDist = 1e10;
double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
std::vector<Key> views; vector<Key> views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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)); Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K); StereoCamera cam1(pose1, K);
// create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
StereoCamera cam2(pose2, K); StereoCamera cam2(pose2, K);
// create third camera 1 meter above the first camera // create third camera 1 meter above the first camera
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
StereoCamera cam3(pose3, K); StereoCamera cam3(pose3, K);
// three landmarks ~5 meters infront of camera // three landmarks ~5 meters infront of camera
@ -450,27 +481,34 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
Point3 landmark4(5, -0.5, 1); Point3 landmark4(5, -0.5, 1);
// 1. Project four landmarks into three cameras and triangulate // 1. Project four landmarks into three cameras and triangulate
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); cam2, cam3, landmark1);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
vector<StereoPoint2> measurements_cam4 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark4); cam2, cam3, landmark2);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
vector<StereoPoint2> measurements_cam4 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark4);
measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier
measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10,10,1); // add outlier SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor1->add(measurements_cam1, views, model, K); smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor2->add(measurements_cam2, views, model, K); smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor3->add(measurements_cam3, views, model, K); smartFactor3->add(measurements_cam3, views, model, K);
SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, SmartFactor::shared_ptr smartFactor4(
new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor4->add(measurements_cam4, views, model, K); smartFactor4->add(measurements_cam4, views, model, K);
@ -484,7 +522,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior)); graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior)); graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, pose1); values.insert(x1, pose1);
values.insert(x2, pose2); values.insert(x2, pose2);
@ -495,19 +534,19 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
Values result; Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params); LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize(); result = optimizer.optimize();
EXPECT(assert_equal(pose3,result.at<Pose3>(x3))); EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
} }
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, jacobianQ ){ //TEST( SmartStereoProjectionPoseFactor, jacobianQ ){
// //
// std::vector<Key> views; // vector<Key> views;
// views.push_back(x1); // views.push_back(x1);
// views.push_back(x2); // views.push_back(x2);
// views.push_back(x3); // views.push_back(x3);
// //
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // // 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)); // Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
// StereoCamera cam1(pose1, K); // StereoCamera cam1(pose1, K);
// // create second camera 1 meter to the right of first camera // // create second camera 1 meter to the right of first camera
// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); // Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
@ -546,8 +585,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
// graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior)); // graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
// graph.push_back(PriorFactor<Pose3>(x2, pose2, 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/10, 0., -M_PI/10), 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 // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise
// Values values; // Values values;
// values.insert(x1, pose1); // values.insert(x1, pose1);
// values.insert(x2, pose2); // values.insert(x2, pose2);
@ -564,13 +603,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
//TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){ //TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){
// // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; // // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
// //
// std::vector<Key> views; // vector<Key> views;
// views.push_back(x1); // views.push_back(x1);
// views.push_back(x2); // views.push_back(x2);
// views.push_back(x3); // views.push_back(x3);
// //
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // // 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)); // Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
// StereoCamera cam1(pose1, K2); // StereoCamera cam1(pose1, K2);
// //
// // create second camera 1 meter to the right of first camera // // create second camera 1 meter to the right of first camera
@ -606,7 +645,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
// graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior)); // graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
// graph.push_back(PriorFactor<Pose3>(x2, pose2, 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)); // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
// Values values; // Values values;
// values.insert(x1, pose1); // values.insert(x1, pose1);
// values.insert(x2, pose2); // values.insert(x2, pose2);
@ -627,23 +666,23 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
//} //}
// //
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, CheckHessian){ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
std::vector<Key> views; vector<Key> views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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)); Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K); StereoCamera cam1(pose1, K);
// create second camera 1 meter to the right of first camera // create second camera
Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
StereoCamera cam2(pose2, K); StereoCamera cam2(pose2, K);
// create third camera 1 meter above the first camera // create third camera
Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
StereoCamera cam3(pose3, K); StereoCamera cam3(pose3, K);
// three landmarks ~5 meters infront of camera // three landmarks ~5 meters infront of camera
@ -651,12 +690,15 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
Point3 landmark2(5, -0.5, 1.2); Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(3, 0, 3.0); Point3 landmark3(3, 0, 3.0);
// 1. Project three landmarks into three cameras and triangulate // Project three landmarks into three cameras and triangulate
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); cam2, cam3, landmark1);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark2);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
// Create smartfactors
double rankTol = 10; double rankTol = 10;
SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol));
@ -668,38 +710,47 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol));
smartFactor3->add(measurements_cam3, views, model, K); smartFactor3->add(measurements_cam3, views, model, K);
// Create graph to optimize
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);
// 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 values;
values.insert(x1, pose1); values.insert(x1, pose1);
values.insert(x2, pose2); values.insert(x2, pose2);
// 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(x3, pose3*noise_pose); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: "); Point3(0.1, 0.1, 0.1)); // smaller noise
values.insert(x3, pose3 * noise_pose);
if (isDebugTest)
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
boost::shared_ptr<GaussianFactor> hessianFactor1 = smartFactor1->linearize(values); // TODO: next line throws Cheirality exception on Mac
boost::shared_ptr<GaussianFactor> hessianFactor2 = smartFactor2->linearize(values); boost::shared_ptr<GaussianFactor> hessianFactor1 = smartFactor1->linearize(
boost::shared_ptr<GaussianFactor> hessianFactor3 = smartFactor3->linearize(values); values);
boost::shared_ptr<GaussianFactor> hessianFactor2 = smartFactor2->linearize(
values);
boost::shared_ptr<GaussianFactor> hessianFactor3 = smartFactor3->linearize(
values);
Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); Matrix CumulativeInformation = hessianFactor1->information()
+ hessianFactor2->information() + hessianFactor3->information();
boost::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(values); boost::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(
values);
Matrix GraphInformation = GaussianGraph->hessian().first; Matrix GraphInformation = GaussianGraph->hessian().first;
// Check Hessian // Check Hessian
EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8));
Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + Matrix AugInformationMatrix = hessianFactor1->augmentedInformation()
hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); + hessianFactor2->augmentedInformation()
+ hessianFactor3->augmentedInformation();
// Check Information vector // Check Information vector
// cout << AugInformationMatrix.size() << endl; // cout << AugInformationMatrix.size() << endl;
Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector
// Check Hessian // Check Hessian
EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8));
@ -709,13 +760,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
//TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ //TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){
// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; // // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl;
// //
// std::vector<Key> views; // vector<Key> views;
// views.push_back(x1); // views.push_back(x1);
// views.push_back(x2); // views.push_back(x2);
// views.push_back(x3); // views.push_back(x3);
// //
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // // 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)); // Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
// StereoCamera cam1(pose1, K2); // StereoCamera cam1(pose1, K2);
// //
// // create second camera 1 meter to the right of first camera // // create second camera 1 meter to the right of first camera
@ -745,7 +796,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
// //
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
// const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); // const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
// Point3 positionPrior = gtsam::Point3(0,0,1); // Point3 positionPrior = Point3(0,0,1);
// //
// NonlinearFactorGraph graph; // NonlinearFactorGraph graph;
// graph.push_back(smartFactor1); // graph.push_back(smartFactor1);
@ -754,7 +805,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
// graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation)); // graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
// graph.push_back(PoseTranslationPrior<Pose3>(x3, 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.1,0.1,0.1)); // smaller noise // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise
// Values values; // Values values;
// values.insert(x1, pose1); // values.insert(x1, pose1);
// values.insert(x2, pose2*noise_pose); // values.insert(x2, pose2*noise_pose);
@ -775,7 +826,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
// //
// // result.print("results of 3 camera, 3 landmark optimization \n"); // // result.print("results of 3 camera, 3 landmark optimization \n");
// if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: "); // if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
// std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; // cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl;
// // EXPECT(assert_equal(pose3,result.at<Pose3>(x3))); // // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
// if(isDebugTest) tictoc_print_(); // if(isDebugTest) tictoc_print_();
//} //}
@ -784,13 +835,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
//TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ //TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){
// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; // // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl;
// //
// std::vector<Key> views; // vector<Key> views;
// views.push_back(x1); // views.push_back(x1);
// views.push_back(x2); // views.push_back(x2);
// views.push_back(x3); // views.push_back(x3);
// //
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // // 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)); // Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
// StereoCamera cam1(pose1, K); // StereoCamera cam1(pose1, K);
// //
// // create second camera 1 meter to the right of first camera // // create second camera 1 meter to the right of first camera
@ -826,7 +877,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
// //
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
// const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); // const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
// Point3 positionPrior = gtsam::Point3(0,0,1); // Point3 positionPrior = Point3(0,0,1);
// //
// NonlinearFactorGraph graph; // NonlinearFactorGraph graph;
// graph.push_back(smartFactor1); // graph.push_back(smartFactor1);
@ -836,8 +887,8 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
// graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation)); // graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
// graph.push_back(PoseTranslationPrior<Pose3>(x3, 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/10, 0., -M_PI/10), 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 // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise
// Values values; // Values values;
// values.insert(x1, pose1); // values.insert(x1, pose1);
// values.insert(x2, pose2); // values.insert(x2, pose2);
@ -858,7 +909,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
// //
// // result.print("results of 3 camera, 3 landmark optimization \n"); // // result.print("results of 3 camera, 3 landmark optimization \n");
// if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: "); // if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
// std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; // cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl;
// // EXPECT(assert_equal(pose3,result.at<Pose3>(x3))); // // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
// if(isDebugTest) tictoc_print_(); // if(isDebugTest) tictoc_print_();
//} //}
@ -867,12 +918,12 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
//TEST( SmartStereoProjectionPoseFactor, Hessian ){ //TEST( SmartStereoProjectionPoseFactor, Hessian ){
// // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl; // // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl;
// //
// std::vector<Key> views; // vector<Key> views;
// views.push_back(x1); // views.push_back(x1);
// views.push_back(x2); // views.push_back(x2);
// //
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // // 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)); // Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
// StereoCamera cam1(pose1, K2); // StereoCamera cam1(pose1, K2);
// //
// // create second camera 1 meter to the right of first camera // // create second camera 1 meter to the right of first camera
@ -892,7 +943,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
// SmartFactor::shared_ptr smartFactor1(new SmartFactor()); // SmartFactor::shared_ptr smartFactor1(new SmartFactor());
// smartFactor1->add(measurements_cam1,views, model, K2); // smartFactor1->add(measurements_cam1,views, model, K2);
// //
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
// Values values; // Values values;
// values.insert(x1, pose1); // values.insert(x1, pose1);
// values.insert(x2, pose2); // values.insert(x2, pose2);
@ -908,29 +959,30 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
// //
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
// cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl; // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl;
std::vector<Key> views; vector<Key> views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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)); Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K); StereoCamera cam1(pose1, K);
// create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
StereoCamera cam2(pose2, K); StereoCamera cam2(pose2, K);
// create third camera 1 meter above the first camera // create third camera 1 meter above the first camera
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
StereoCamera cam3(pose3, K); StereoCamera cam3(pose3, K);
Point3 landmark1(5, 0.5, 1.2); Point3 landmark1(5, 0.5, 1.2);
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); SmartFactor::shared_ptr smartFactorInstance(new SmartFactor());
smartFactorInstance->add(measurements_cam1, views, model, K); smartFactorInstance->add(measurements_cam1, views, model, K);
@ -940,46 +992,54 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){
values.insert(x2, pose2); values.insert(x2, pose2);
values.insert(x3, pose3); values.insert(x3, pose3);
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactorInstance->linearize(values); boost::shared_ptr<GaussianFactor> hessianFactor =
smartFactorInstance->linearize(values);
// hessianFactor->print("Hessian factor \n"); // hessianFactor->print("Hessian factor \n");
Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Values rotValues; Values rotValues;
rotValues.insert(x1, poseDrift.compose(pose1)); rotValues.insert(x1, poseDrift.compose(pose1));
rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x2, poseDrift.compose(pose2));
rotValues.insert(x3, poseDrift.compose(pose3)); rotValues.insert(x3, poseDrift.compose(pose3));
boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactorInstance->linearize(rotValues); boost::shared_ptr<GaussianFactor> hessianFactorRot =
smartFactorInstance->linearize(rotValues);
// hessianFactorRot->print("Hessian factor \n"); // hessianFactorRot->print("Hessian factor \n");
// Hessian is invariant to rotations in the nondegenerate case // Hessian is invariant to rotations in the nondegenerate case
EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); EXPECT(
assert_equal(hessianFactor->information(),
hessianFactorRot->information(), 1e-7));
Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
Point3(10, -4, 5));
Values tranValues; Values tranValues;
tranValues.insert(x1, poseDrift2.compose(pose1)); tranValues.insert(x1, poseDrift2.compose(pose1));
tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x2, poseDrift2.compose(pose2));
tranValues.insert(x3, poseDrift2.compose(pose3)); tranValues.insert(x3, poseDrift2.compose(pose3));
boost::shared_ptr<GaussianFactor> hessianFactorRotTran = smartFactorInstance->linearize(tranValues); boost::shared_ptr<GaussianFactor> hessianFactorRotTran =
smartFactorInstance->linearize(tranValues);
// Hessian is invariant to rotations and translations in the nondegenerate case // Hessian is invariant to rotations and translations in the nondegenerate case
EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); EXPECT(
assert_equal(hessianFactor->information(),
hessianFactorRotTran->information(), 1e-7));
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
// cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl;
std::vector<Key> views; vector<Key> views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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)); Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K2); StereoCamera cam1(pose1, K2);
// Second and third cameras in same place, which is a degenerate configuration // Second and third cameras in same place, which is a degenerate configuration
@ -990,49 +1050,60 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){
Point3 landmark1(5, 0.5, 1.2); Point3 landmark1(5, 0.5, 1.2);
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
SmartFactor::shared_ptr smartFactor(new SmartFactor()); SmartFactor::shared_ptr smartFactor(new SmartFactor());
smartFactor->add(measurements_cam1, views, model, K2); smartFactor->add(measurements_cam1, views, model, K2);
Values values; Values values;
values.insert(x1, pose1); values.insert(x1, pose1);
values.insert(x2, pose2); values.insert(x2, pose2);
values.insert(x3, pose3); values.insert(x3, pose3);
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(values); boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(
if(isDebugTest) hessianFactor->print("Hessian factor \n"); values);
if (isDebugTest)
hessianFactor->print("Hessian factor \n");
Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Values rotValues; Values rotValues;
rotValues.insert(x1, poseDrift.compose(pose1)); rotValues.insert(x1, poseDrift.compose(pose1));
rotValues.insert(x2, poseDrift.compose(pose2)); rotValues.insert(x2, poseDrift.compose(pose2));
rotValues.insert(x3, poseDrift.compose(pose3)); rotValues.insert(x3, poseDrift.compose(pose3));
boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(rotValues); boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(
if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); rotValues);
if (isDebugTest)
hessianFactorRot->print("Hessian factor \n");
// Hessian is invariant to rotations in the nondegenerate case // Hessian is invariant to rotations in the nondegenerate case
EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); EXPECT(
assert_equal(hessianFactor->information(),
hessianFactorRot->information(), 1e-8));
Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
Point3(10, -4, 5));
Values tranValues; Values tranValues;
tranValues.insert(x1, poseDrift2.compose(pose1)); tranValues.insert(x1, poseDrift2.compose(pose1));
tranValues.insert(x2, poseDrift2.compose(pose2)); tranValues.insert(x2, poseDrift2.compose(pose2));
tranValues.insert(x3, poseDrift2.compose(pose3)); tranValues.insert(x3, poseDrift2.compose(pose3));
boost::shared_ptr<GaussianFactor> hessianFactorRotTran = smartFactor->linearize(tranValues); boost::shared_ptr<GaussianFactor> hessianFactorRotTran =
smartFactor->linearize(tranValues);
// Hessian is invariant to rotations and translations in the nondegenerate case // Hessian is invariant to rotations and translations in the nondegenerate case
EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); EXPECT(
assert_equal(hessianFactor->information(),
hessianFactorRotTran->information(), 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */