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

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

File diff suppressed because it is too large Load Diff

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

@ -32,7 +32,7 @@ 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
@ -40,14 +40,16 @@ 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;
@ -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);
} }
@ -128,7 +131,8 @@ 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
@ -168,7 +172,8 @@ 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
@ -185,7 +190,8 @@ TEST( SmartStereoProjectionPoseFactor, noisy ){
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());
@ -199,15 +205,15 @@ TEST( SmartStereoProjectionPoseFactor, noisy ){
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);
@ -218,13 +224,14 @@ TEST( SmartStereoProjectionPoseFactor, noisy ){
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
@ -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,18 +278,22 @@ 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);
@ -292,22 +306,23 @@ 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)
result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(pose3, result.at<Pose3>(x3))); EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
if(isDebugTest) tictoc_print_(); 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));
@ -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,8 +365,9 @@ 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);
@ -363,13 +385,13 @@ 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));
@ -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,8 +437,9 @@ 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);
@ -428,13 +459,13 @@ 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));
@ -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, SmartFactor::shared_ptr smartFactor1(
JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); new SmartFactor(1, -1, false, false, boost::none, 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);
@ -501,13 +540,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
///* *************************************************************************/ ///* *************************************************************************/
//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);
@ -629,20 +668,20 @@ 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);
@ -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,34 +710,43 @@ 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
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
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: ");
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;
@ -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);
@ -911,13 +962,13 @@ 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
@ -930,7 +981,8 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){
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);
}
/* ************************************************************************* */ /* ************************************************************************* */