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

change global variable names in test

release/4.3a0
Akshay Krishnan 2022-07-12 18:45:46 -07:00
parent 7d9cf47579
commit 223ea468d6
1 changed files with 120 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

240
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -38,24 +38,24 @@ using namespace boost::assign;
// Some common constants // Some common constants
static const boost::shared_ptr<Cal3_S2> sharedCal = // static const boost::shared_ptr<Cal3_S2> kSharedCal = //
boost::make_shared<Cal3_S2>(1500, 1200, 0.1, 640, 480); boost::make_shared<Cal3_S2>(1500, 1200, 0.1, 640, 480);
// Looking along X-axis, 1 meter above ground plane (x-y) // Looking along X-axis, 1 meter above ground plane (x-y)
static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2); static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); static const Pose3 kPose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal); static const PinholeCamera<Cal3_S2> kCamera1(kPose1, *kSharedCal);
// create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
static const Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); static const Pose3 kPose2 = kPose1 * Pose3(Rot3(), Point3(1, 0, 0));
PinholeCamera<Cal3_S2> camera2(pose2, *sharedCal); static const PinholeCamera<Cal3_S2> kCamera2(kPose2, *kSharedCal);
// landmark ~5 meters infront of camera // landmark ~5 meters infront of camera
static const Point3 landmark(5, 0.5, 1.2); static const Point3 kLandmark(5, 0.5, 1.2);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark); static const Point2 kZ1 = kCamera1.project(kLandmark);
Point2 z2 = camera2.project(landmark); static const Point2 kZ2 = kCamera2.project(kLandmark);
//****************************************************************************** //******************************************************************************
// Simple test with a well-behaved two camera situation // Simple test with a well-behaved two camera situation
@ -63,22 +63,22 @@ TEST(triangulation, twoPoses) {
vector<Pose3> poses; vector<Pose3> poses;
Point2Vector measurements; Point2Vector measurements;
poses += pose1, pose2; poses += kPose1, kPose2;
measurements += z1, z2; measurements += kZ1, kZ2;
double rank_tol = 1e-9; double rank_tol = 1e-9;
// 1. Test simple DLT, perfect in no noise situation // 1. Test simple DLT, perfect in no noise situation
bool optimize = false; bool optimize = false;
boost::optional<Point3> actual1 = // boost::optional<Point3> actual1 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual1, 1e-7)); EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
// 2. test with optimization on, same answer // 2. test with optimization on, same answer
optimize = true; optimize = true;
boost::optional<Point3> actual2 = // boost::optional<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual2, 1e-7)); EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
// 3. Add some noise and try again: result should be ~ (4.995, // 3. Add some noise and try again: result should be ~ (4.995,
// 0.499167, 1.19814) // 0.499167, 1.19814)
@ -86,22 +86,22 @@ TEST(triangulation, twoPoses) {
measurements.at(1) += Point2(-0.2, 0.3); measurements.at(1) += Point2(-0.2, 0.3);
optimize = false; optimize = false;
boost::optional<Point3> actual3 = // boost::optional<Point3> actual3 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4)); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4));
// 4. Now with optimization on // 4. Now with optimization on
optimize = true; optimize = true;
boost::optional<Point3> actual4 = // boost::optional<Point3> actual4 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
} }
TEST(triangulation, twoCamerasUsingLOST) { TEST(triangulation, twoCamerasUsingLOST) {
CameraSet<PinholeCamera<Cal3_S2>> cameras; CameraSet<PinholeCamera<Cal3_S2>> cameras;
cameras.push_back(camera1); cameras.push_back(kCamera1);
cameras.push_back(camera2); cameras.push_back(kCamera2);
Point2Vector measurements = {z1, z2}; Point2Vector measurements = {kZ1, kZ2};
SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4); SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4);
double rank_tol = 1e-9; double rank_tol = 1e-9;
@ -111,7 +111,7 @@ TEST(triangulation, twoCamerasUsingLOST) {
cameras, measurements, rank_tol, cameras, measurements, rank_tol,
/*optimize=*/false, measurementNoise, /*optimize=*/false, measurementNoise,
/*use_lost_triangulation=*/true); /*use_lost_triangulation=*/true);
EXPECT(assert_equal(landmark, *actual1, 1e-12)); EXPECT(assert_equal(kLandmark, *actual1, 1e-12));
// 3. Add some noise and try again: result should be ~ (4.995, // 3. Add some noise and try again: result should be ~ (4.995,
// 0.499167, 1.19814) // 0.499167, 1.19814)
@ -167,18 +167,18 @@ TEST(triangulation, twoPosesCal3DS2) {
boost::make_shared<Cal3DS2>(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, boost::make_shared<Cal3DS2>(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001,
-0.0003); -0.0003);
PinholeCamera<Cal3DS2> camera1Distorted(pose1, *sharedDistortedCal); PinholeCamera<Cal3DS2> camera1Distorted(kPose1, *sharedDistortedCal);
PinholeCamera<Cal3DS2> camera2Distorted(pose2, *sharedDistortedCal); PinholeCamera<Cal3DS2> camera2Distorted(kPose2, *sharedDistortedCal);
// 0. Project two landmarks into two cameras and triangulate // 0. Project two landmarks into two cameras and triangulate
Point2 z1Distorted = camera1Distorted.project(landmark); Point2 z1Distorted = camera1Distorted.project(kLandmark);
Point2 z2Distorted = camera2Distorted.project(landmark); Point2 z2Distorted = camera2Distorted.project(kLandmark);
vector<Pose3> poses; vector<Pose3> poses;
Point2Vector measurements; Point2Vector measurements;
poses += pose1, pose2; poses += kPose1, kPose2;
measurements += z1Distorted, z2Distorted; measurements += z1Distorted, z2Distorted;
double rank_tol = 1e-9; double rank_tol = 1e-9;
@ -188,14 +188,14 @@ TEST(triangulation, twoPosesCal3DS2) {
boost::optional<Point3> actual1 = // boost::optional<Point3> actual1 = //
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements, triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
rank_tol, optimize); rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual1, 1e-7)); EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
// 2. test with optimization on, same answer // 2. test with optimization on, same answer
optimize = true; optimize = true;
boost::optional<Point3> actual2 = // boost::optional<Point3> actual2 = //
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements, triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
rank_tol, optimize); rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual2, 1e-7)); EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
// 3. Add some noise and try again: result should be ~ (4.995, // 3. Add some noise and try again: result should be ~ (4.995,
// 0.499167, 1.19814) // 0.499167, 1.19814)
@ -224,18 +224,18 @@ TEST(triangulation, twoPosesFisheye) {
boost::make_shared<Calibration>(1500, 1200, .1, 640, 480, -.3, 0.1, boost::make_shared<Calibration>(1500, 1200, .1, 640, 480, -.3, 0.1,
0.0001, -0.0003); 0.0001, -0.0003);
PinholeCamera<Calibration> camera1Distorted(pose1, *sharedDistortedCal); PinholeCamera<Calibration> camera1Distorted(kPose1, *sharedDistortedCal);
PinholeCamera<Calibration> camera2Distorted(pose2, *sharedDistortedCal); PinholeCamera<Calibration> camera2Distorted(kPose2, *sharedDistortedCal);
// 0. Project two landmarks into two cameras and triangulate // 0. Project two landmarks into two cameras and triangulate
Point2 z1Distorted = camera1Distorted.project(landmark); Point2 z1Distorted = camera1Distorted.project(kLandmark);
Point2 z2Distorted = camera2Distorted.project(landmark); Point2 z2Distorted = camera2Distorted.project(kLandmark);
vector<Pose3> poses; vector<Pose3> poses;
Point2Vector measurements; Point2Vector measurements;
poses += pose1, pose2; poses += kPose1, kPose2;
measurements += z1Distorted, z2Distorted; measurements += z1Distorted, z2Distorted;
double rank_tol = 1e-9; double rank_tol = 1e-9;
@ -245,14 +245,14 @@ TEST(triangulation, twoPosesFisheye) {
boost::optional<Point3> actual1 = // boost::optional<Point3> actual1 = //
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements, triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
rank_tol, optimize); rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual1, 1e-7)); EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
// 2. test with optimization on, same answer // 2. test with optimization on, same answer
optimize = true; optimize = true;
boost::optional<Point3> actual2 = // boost::optional<Point3> actual2 = //
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements, triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
rank_tol, optimize); rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual2, 1e-7)); EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
// 3. Add some noise and try again: result should be ~ (4.995, // 3. Add some noise and try again: result should be ~ (4.995,
// 0.499167, 1.19814) // 0.499167, 1.19814)
@ -277,17 +277,17 @@ TEST(triangulation, twoPosesFisheye) {
TEST(triangulation, twoPosesBundler) { TEST(triangulation, twoPosesBundler) {
boost::shared_ptr<Cal3Bundler> bundlerCal = // boost::shared_ptr<Cal3Bundler> bundlerCal = //
boost::make_shared<Cal3Bundler>(1500, 0.1, 0.2, 640, 480); boost::make_shared<Cal3Bundler>(1500, 0.1, 0.2, 640, 480);
PinholeCamera<Cal3Bundler> camera1(pose1, *bundlerCal); PinholeCamera<Cal3Bundler> camera1(kPose1, *bundlerCal);
PinholeCamera<Cal3Bundler> camera2(pose2, *bundlerCal); PinholeCamera<Cal3Bundler> camera2(kPose2, *bundlerCal);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(kLandmark);
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(kLandmark);
vector<Pose3> poses; vector<Pose3> poses;
Point2Vector measurements; Point2Vector measurements;
poses += pose1, pose2; poses += kPose1, kPose2;
measurements += z1, z2; measurements += z1, z2;
bool optimize = true; bool optimize = true;
@ -296,7 +296,7 @@ TEST(triangulation, twoPosesBundler) {
boost::optional<Point3> actual = // boost::optional<Point3> actual = //
triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol, triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol,
optimize); optimize);
EXPECT(assert_equal(landmark, *actual, 1e-7)); EXPECT(assert_equal(kLandmark, *actual, 1e-7));
// Add some noise and try again // Add some noise and try again
measurements.at(0) += Point2(0.1, 0.5); measurements.at(0) += Point2(0.1, 0.5);
@ -313,12 +313,12 @@ TEST(triangulation, fourPoses) {
vector<Pose3> poses; vector<Pose3> poses;
Point2Vector measurements; Point2Vector measurements;
poses += pose1, pose2; poses += kPose1, kPose2;
measurements += z1, z2; measurements += kZ1, kZ2;
boost::optional<Point3> actual = boost::optional<Point3> actual =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements); triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2)); EXPECT(assert_equal(kLandmark, *actual, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, // 2. Add some noise and try again: result should be ~ (4.995,
// 0.499167, 1.19814) // 0.499167, 1.19814)
@ -326,37 +326,37 @@ TEST(triangulation, fourPoses) {
measurements.at(1) += Point2(-0.2, 0.3); measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> actual2 = // boost::optional<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements); triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
EXPECT(assert_equal(landmark, *actual2, 1e-2)); EXPECT(assert_equal(kLandmark, *actual2, 1e-2));
// 3. Add a slightly rotated third camera above, again with measurement noise // 3. Add a slightly rotated third camera above, again with measurement noise
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal); PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
Point2 z3 = camera3.project(landmark); Point2 z3 = camera3.project(kLandmark);
poses += pose3; poses += pose3;
measurements += z3 + Point2(0.1, -0.1); measurements += z3 + Point2(0.1, -0.1);
boost::optional<Point3> triangulated_3cameras = // boost::optional<Point3> triangulated_3cameras = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements); triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization // Again with nonlinear optimization
boost::optional<Point3> triangulated_3cameras_opt = boost::optional<Point3> triangulated_3cameras_opt =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true); triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, 1e-9, true);
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2));
// 4. Test failure: Add a 4th camera facing the wrong way // 4. Test failure: Add a 4th camera facing the wrong way
Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
PinholeCamera<Cal3_S2> camera4(pose4, *sharedCal); PinholeCamera<Cal3_S2> camera4(pose4, *kSharedCal);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException);
poses += pose4; poses += pose4;
measurements += Point2(400, 400); measurements += Point2(400, 400);
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements), CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
TriangulationCheiralityException); TriangulationCheiralityException);
#endif #endif
} }
@ -364,33 +364,33 @@ TEST(triangulation, fourPoses) {
//****************************************************************************** //******************************************************************************
TEST(triangulation, threePoses_robustNoiseModel) { TEST(triangulation, threePoses_robustNoiseModel) {
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal); PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
Point2 z3 = camera3.project(landmark); Point2 z3 = camera3.project(kLandmark);
vector<Pose3> poses; vector<Pose3> poses;
Point2Vector measurements; Point2Vector measurements;
poses += pose1, pose2, pose3; poses += kPose1, kPose2, pose3;
measurements += z1, z2, z3; measurements += kZ1, kZ2, z3;
// noise free, so should give exactly the landmark // noise free, so should give exactly the landmark
boost::optional<Point3> actual = boost::optional<Point3> actual =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements); triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2)); EXPECT(assert_equal(kLandmark, *actual, 1e-2));
// Add outlier // Add outlier
measurements.at(0) += Point2(100, 120); // very large pixel noise! measurements.at(0) += Point2(100, 120); // very large pixel noise!
// now estimate does not match landmark // now estimate does not match landmark
boost::optional<Point3> actual2 = // boost::optional<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements); triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
// DLT is surprisingly robust, but still off (actual error is around 0.26m): // DLT is surprisingly robust, but still off (actual error is around 0.26m):
EXPECT( (landmark - *actual2).norm() >= 0.2); EXPECT( (kLandmark - *actual2).norm() >= 0.2);
EXPECT( (landmark - *actual2).norm() <= 0.5); EXPECT( (kLandmark - *actual2).norm() <= 0.5);
// Again with nonlinear optimization // Again with nonlinear optimization
boost::optional<Point3> actual3 = boost::optional<Point3> actual3 =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true); triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, 1e-9, true);
// result from nonlinear (but non-robust optimization) is close to DLT and still off // result from nonlinear (but non-robust optimization) is close to DLT and still off
EXPECT(assert_equal(*actual2, *actual3, 0.1)); EXPECT(assert_equal(*actual2, *actual3, 0.1));
@ -398,27 +398,27 @@ TEST(triangulation, threePoses_robustNoiseModel) {
auto model = noiseModel::Robust::Create( auto model = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2));
boost::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>( boost::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
poses, sharedCal, measurements, 1e-9, true, model); poses, kSharedCal, measurements, 1e-9, true, model);
// using the Huber loss we now have a quite small error!! nice! // using the Huber loss we now have a quite small error!! nice!
EXPECT(assert_equal(landmark, *actual4, 0.05)); EXPECT(assert_equal(kLandmark, *actual4, 0.05));
} }
//****************************************************************************** //******************************************************************************
TEST(triangulation, fourPoses_robustNoiseModel) { TEST(triangulation, fourPoses_robustNoiseModel) {
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
PinholeCamera<Cal3_S2> camera3(pose3, *sharedCal); PinholeCamera<Cal3_S2> camera3(pose3, *kSharedCal);
Point2 z3 = camera3.project(landmark); Point2 z3 = camera3.project(kLandmark);
vector<Pose3> poses; vector<Pose3> poses;
Point2Vector measurements; Point2Vector measurements;
poses += pose1, pose1, pose2, pose3; // 2 measurements from pose 1 poses += kPose1, kPose1, kPose2, pose3; // 2 measurements from pose 1
measurements += z1, z1, z2, z3; measurements += kZ1, kZ1, kZ2, z3;
// noise free, so should give exactly the landmark // noise free, so should give exactly the landmark
boost::optional<Point3> actual = boost::optional<Point3> actual =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements); triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2)); EXPECT(assert_equal(kLandmark, *actual, 1e-2));
// Add outlier // Add outlier
measurements.at(0) += Point2(100, 120); // very large pixel noise! measurements.at(0) += Point2(100, 120); // very large pixel noise!
@ -429,14 +429,14 @@ TEST(triangulation, fourPoses_robustNoiseModel) {
// now estimate does not match landmark // now estimate does not match landmark
boost::optional<Point3> actual2 = // boost::optional<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements); triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements);
// DLT is surprisingly robust, but still off (actual error is around 0.17m): // DLT is surprisingly robust, but still off (actual error is around 0.17m):
EXPECT( (landmark - *actual2).norm() >= 0.1); EXPECT( (kLandmark - *actual2).norm() >= 0.1);
EXPECT( (landmark - *actual2).norm() <= 0.5); EXPECT( (kLandmark - *actual2).norm() <= 0.5);
// Again with nonlinear optimization // Again with nonlinear optimization
boost::optional<Point3> actual3 = boost::optional<Point3> actual3 =
triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true); triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements, 1e-9, true);
// result from nonlinear (but non-robust optimization) is close to DLT and still off // result from nonlinear (but non-robust optimization) is close to DLT and still off
EXPECT(assert_equal(*actual2, *actual3, 0.1)); EXPECT(assert_equal(*actual2, *actual3, 0.1));
@ -444,24 +444,24 @@ TEST(triangulation, fourPoses_robustNoiseModel) {
auto model = noiseModel::Robust::Create( auto model = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2));
boost::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>( boost::optional<Point3> actual4 = triangulatePoint3<Cal3_S2>(
poses, sharedCal, measurements, 1e-9, true, model); poses, kSharedCal, measurements, 1e-9, true, model);
// using the Huber loss we now have a quite small error!! nice! // using the Huber loss we now have a quite small error!! nice!
EXPECT(assert_equal(landmark, *actual4, 0.05)); EXPECT(assert_equal(kLandmark, *actual4, 0.05));
} }
//****************************************************************************** //******************************************************************************
TEST(triangulation, fourPoses_distinct_Ks) { TEST(triangulation, fourPoses_distinct_Ks) {
Cal3_S2 K1(1500, 1200, 0, 640, 480); Cal3_S2 K1(1500, 1200, 0, 640, 480);
// 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)
PinholeCamera<Cal3_S2> camera1(pose1, K1); PinholeCamera<Cal3_S2> camera1(kPose1, K1);
// create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
Cal3_S2 K2(1600, 1300, 0, 650, 440); Cal3_S2 K2(1600, 1300, 0, 650, 440);
PinholeCamera<Cal3_S2> camera2(pose2, K2); PinholeCamera<Cal3_S2> camera2(kPose2, K2);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(kLandmark);
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(kLandmark);
CameraSet<PinholeCamera<Cal3_S2>> cameras; CameraSet<PinholeCamera<Cal3_S2>> cameras;
Point2Vector measurements; Point2Vector measurements;
@ -471,7 +471,7 @@ TEST(triangulation, fourPoses_distinct_Ks) {
boost::optional<Point3> actual = // boost::optional<Point3> actual = //
triangulatePoint3<Cal3_S2>(cameras, measurements); triangulatePoint3<Cal3_S2>(cameras, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2)); EXPECT(assert_equal(kLandmark, *actual, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, // 2. Add some noise and try again: result should be ~ (4.995,
// 0.499167, 1.19814) // 0.499167, 1.19814)
@ -480,25 +480,25 @@ TEST(triangulation, fourPoses_distinct_Ks) {
boost::optional<Point3> actual2 = // boost::optional<Point3> actual2 = //
triangulatePoint3<Cal3_S2>(cameras, measurements); triangulatePoint3<Cal3_S2>(cameras, measurements);
EXPECT(assert_equal(landmark, *actual2, 1e-2)); EXPECT(assert_equal(kLandmark, *actual2, 1e-2));
// 3. Add a slightly rotated third camera above, again with measurement noise // 3. Add a slightly rotated third camera above, again with measurement noise
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
Cal3_S2 K3(700, 500, 0, 640, 480); Cal3_S2 K3(700, 500, 0, 640, 480);
PinholeCamera<Cal3_S2> camera3(pose3, K3); PinholeCamera<Cal3_S2> camera3(pose3, K3);
Point2 z3 = camera3.project(landmark); Point2 z3 = camera3.project(kLandmark);
cameras += camera3; cameras += camera3;
measurements += z3 + Point2(0.1, -0.1); measurements += z3 + Point2(0.1, -0.1);
boost::optional<Point3> triangulated_3cameras = // boost::optional<Point3> triangulated_3cameras = //
triangulatePoint3<Cal3_S2>(cameras, measurements); triangulatePoint3<Cal3_S2>(cameras, measurements);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization // Again with nonlinear optimization
boost::optional<Point3> triangulated_3cameras_opt = boost::optional<Point3> triangulated_3cameras_opt =
triangulatePoint3<Cal3_S2>(cameras, measurements, 1e-9, true); triangulatePoint3<Cal3_S2>(cameras, measurements, 1e-9, true);
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2));
// 4. Test failure: Add a 4th camera facing the wrong way // 4. Test failure: Add a 4th camera facing the wrong way
Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); Pose3 pose4 = Pose3(Rot3::Ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
@ -506,7 +506,7 @@ TEST(triangulation, fourPoses_distinct_Ks) {
PinholeCamera<Cal3_S2> camera4(pose4, K4); PinholeCamera<Cal3_S2> camera4(pose4, K4);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(camera4.project(landmark), CheiralityException); CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException);
cameras += camera4; cameras += camera4;
measurements += Point2(400, 400); measurements += Point2(400, 400);
@ -519,15 +519,15 @@ TEST(triangulation, fourPoses_distinct_Ks) {
TEST(triangulation, fourPoses_distinct_Ks_distortion) { TEST(triangulation, fourPoses_distinct_Ks_distortion) {
Cal3DS2 K1(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); Cal3DS2 K1(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003);
// 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)
PinholeCamera<Cal3DS2> camera1(pose1, K1); PinholeCamera<Cal3DS2> camera1(kPose1, K1);
// create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
Cal3DS2 K2(1600, 1300, 0, 650, 440, -.2, 0.05, 0.0002, -0.0001); Cal3DS2 K2(1600, 1300, 0, 650, 440, -.2, 0.05, 0.0002, -0.0001);
PinholeCamera<Cal3DS2> camera2(pose2, K2); PinholeCamera<Cal3DS2> camera2(kPose2, K2);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(kLandmark);
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(kLandmark);
CameraSet<PinholeCamera<Cal3DS2>> cameras; CameraSet<PinholeCamera<Cal3DS2>> cameras;
Point2Vector measurements; Point2Vector measurements;
@ -537,22 +537,22 @@ TEST(triangulation, fourPoses_distinct_Ks_distortion) {
boost::optional<Point3> actual = // boost::optional<Point3> actual = //
triangulatePoint3<Cal3DS2>(cameras, measurements); triangulatePoint3<Cal3DS2>(cameras, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2)); EXPECT(assert_equal(kLandmark, *actual, 1e-2));
} }
//****************************************************************************** //******************************************************************************
TEST(triangulation, outliersAndFarLandmarks) { TEST(triangulation, outliersAndFarLandmarks) {
Cal3_S2 K1(1500, 1200, 0, 640, 480); Cal3_S2 K1(1500, 1200, 0, 640, 480);
// 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)
PinholeCamera<Cal3_S2> camera1(pose1, K1); PinholeCamera<Cal3_S2> camera1(kPose1, K1);
// create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
Cal3_S2 K2(1600, 1300, 0, 650, 440); Cal3_S2 K2(1600, 1300, 0, 650, 440);
PinholeCamera<Cal3_S2> camera2(pose2, K2); PinholeCamera<Cal3_S2> camera2(kPose2, K2);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(kLandmark);
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(kLandmark);
CameraSet<PinholeCamera<Cal3_S2>> cameras; CameraSet<PinholeCamera<Cal3_S2>> cameras;
Point2Vector measurements; Point2Vector measurements;
@ -565,7 +565,7 @@ TEST(triangulation, outliersAndFarLandmarks) {
1.0, false, landmarkDistanceThreshold); // all default except 1.0, false, landmarkDistanceThreshold); // all default except
// landmarkDistanceThreshold // landmarkDistanceThreshold
TriangulationResult actual = triangulateSafe(cameras, measurements, params); TriangulationResult actual = triangulateSafe(cameras, measurements, params);
EXPECT(assert_equal(landmark, *actual, 1e-2)); EXPECT(assert_equal(kLandmark, *actual, 1e-2));
EXPECT(actual.valid()); EXPECT(actual.valid());
landmarkDistanceThreshold = 4; // landmark is farther than that landmarkDistanceThreshold = 4; // landmark is farther than that
@ -577,10 +577,10 @@ TEST(triangulation, outliersAndFarLandmarks) {
// 3. Add a slightly rotated third camera above with a wrong measurement // 3. Add a slightly rotated third camera above with a wrong measurement
// (OUTLIER) // (OUTLIER)
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Pose3 pose3 = kPose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
Cal3_S2 K3(700, 500, 0, 640, 480); Cal3_S2 K3(700, 500, 0, 640, 480);
PinholeCamera<Cal3_S2> camera3(pose3, K3); PinholeCamera<Cal3_S2> camera3(pose3, K3);
Point2 z3 = camera3.project(landmark); Point2 z3 = camera3.project(kLandmark);
cameras += camera3; cameras += camera3;
measurements += z3 + Point2(10, -10); measurements += z3 + Point2(10, -10);
@ -603,18 +603,18 @@ TEST(triangulation, outliersAndFarLandmarks) {
//****************************************************************************** //******************************************************************************
TEST(triangulation, twoIdenticalPoses) { TEST(triangulation, twoIdenticalPoses) {
// 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)
PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal); PinholeCamera<Cal3_S2> camera1(kPose1, *kSharedCal);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(kLandmark);
vector<Pose3> poses; vector<Pose3> poses;
Point2Vector measurements; Point2Vector measurements;
poses += pose1, pose1; poses += kPose1, kPose1;
measurements += z1, z1; measurements += z1, z1;
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements), CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
TriangulationUnderconstrainedException); TriangulationUnderconstrainedException);
} }
@ -629,7 +629,7 @@ TEST(triangulation, onePose) {
poses += Pose3(); poses += Pose3();
measurements += Point2(0, 0); measurements += Point2(0, 0);
CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements), CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, kSharedCal, measurements),
TriangulationUnderconstrainedException); TriangulationUnderconstrainedException);
} }
@ -745,12 +745,12 @@ TEST(triangulation, twoPoses_sphericalCamera) {
std::vector<Unit3> measurements; std::vector<Unit3> measurements;
// Project landmark into two cameras and triangulate // Project landmark into two cameras and triangulate
SphericalCamera cam1(pose1); SphericalCamera cam1(kPose1);
SphericalCamera cam2(pose2); SphericalCamera cam2(kPose2);
Unit3 u1 = cam1.project(landmark); Unit3 u1 = cam1.project(kLandmark);
Unit3 u2 = cam2.project(landmark); Unit3 u2 = cam2.project(kLandmark);
poses += pose1, pose2; poses += kPose1, kPose2;
measurements += u1, u2; measurements += u1, u2;
CameraSet<SphericalCamera> cameras; CameraSet<SphericalCamera> cameras;
@ -762,25 +762,25 @@ TEST(triangulation, twoPoses_sphericalCamera) {
// 1. Test linear triangulation via DLT // 1. Test linear triangulation via DLT
auto projection_matrices = projectionMatricesFromCameras(cameras); auto projection_matrices = projectionMatricesFromCameras(cameras);
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
EXPECT(assert_equal(landmark, point, 1e-7)); EXPECT(assert_equal(kLandmark, point, 1e-7));
// 2. Test nonlinear triangulation // 2. Test nonlinear triangulation
point = triangulateNonlinear<SphericalCamera>(cameras, measurements, point); point = triangulateNonlinear<SphericalCamera>(cameras, measurements, point);
EXPECT(assert_equal(landmark, point, 1e-7)); EXPECT(assert_equal(kLandmark, point, 1e-7));
// 3. Test simple DLT, now within triangulatePoint3 // 3. Test simple DLT, now within triangulatePoint3
bool optimize = false; bool optimize = false;
boost::optional<Point3> actual1 = // boost::optional<Point3> actual1 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize); optimize);
EXPECT(assert_equal(landmark, *actual1, 1e-7)); EXPECT(assert_equal(kLandmark, *actual1, 1e-7));
// 4. test with optimization on, same answer // 4. test with optimization on, same answer
optimize = true; optimize = true;
boost::optional<Point3> actual2 = // boost::optional<Point3> actual2 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize); optimize);
EXPECT(assert_equal(landmark, *actual2, 1e-7)); EXPECT(assert_equal(kLandmark, *actual2, 1e-7));
// 5. Add some noise and try again: result should be ~ (4.995, // 5. Add some noise and try again: result should be ~ (4.995,
// 0.499167, 1.19814) // 0.499167, 1.19814)
@ -825,7 +825,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) {
EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, -1.0)), u2, EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, -1.0)), u2,
1e-7)); // behind and to the right of PoseB 1e-7)); // behind and to the right of PoseB
poses += pose1, pose2; poses += kPose1, kPose2;
measurements += u1, u2; measurements += u1, u2;
CameraSet<SphericalCamera> cameras; CameraSet<SphericalCamera> cameras;