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

More deliberate testing of optimize on/off

release/4.3a0
dellaert 2015-03-14 20:26:50 -04:00
parent 3675754816
commit a920caf2ec
1 changed files with 26 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -50,6 +50,7 @@ Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(landmark);
//****************************************************************************** //******************************************************************************
// Simple test with a well-behaved two camera situation
TEST( triangulation, twoPoses) { TEST( triangulation, twoPoses) {
vector<Pose3> poses; vector<Pose3> poses;
@ -58,24 +59,36 @@ TEST( triangulation, twoPoses) {
poses += pose1, pose2; poses += pose1, pose2;
measurements += z1, z2; measurements += z1, z2;
bool optimize = true;
double rank_tol = 1e-9; double rank_tol = 1e-9;
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses, // 1. Test simple DLT, perfect in no noise situation
sharedCal, measurements, rank_tol, optimize); bool optimize = false;
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); boost::optional<Point3> actual1 = triangulatePoint3(poses, sharedCal,
measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual1, 1e-7));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) // 2. test with optimization on, same answer
optimize = true;
boost::optional<Point3> actual2 = triangulatePoint3(poses, sharedCal,
measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual2, 1e-7));
// 3. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements.at(0) += Point2(0.1, 0.5); measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3); measurements.at(1) += Point2(-0.2, 0.3);
optimize = false;
boost::optional<Point3> actual3 = triangulatePoint3(poses, sharedCal,
measurements, rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4));
boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses, // 3. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
sharedCal, measurements, rank_tol, optimize); optimize = true;
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); boost::optional<Point3> actual4 = triangulatePoint3(poses, sharedCal,
measurements, rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
} }
//****************************************************************************** //******************************************************************************
TEST( triangulation, twoPosesBundler) { TEST( triangulation, twoPosesBundler) {
boost::shared_ptr<Cal3Bundler> bundlerCal = // boost::shared_ptr<Cal3Bundler> bundlerCal = //
@ -151,7 +164,8 @@ TEST( triangulation, fourPoses) {
SimpleCamera camera4(pose4, *sharedCal); SimpleCamera camera4(pose4, *sharedCal);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException); CHECK_EXCEPTION(camera4.project(landmark)
;, CheiralityException);
poses += pose4; poses += pose4;
measurements += Point2(400, 400); measurements += Point2(400, 400);
@ -217,7 +231,8 @@ TEST( triangulation, fourPoses_distinct_Ks) {
SimpleCamera camera4(pose4, K4); SimpleCamera camera4(pose4, K4);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException); CHECK_EXCEPTION(camera4.project(landmark)
;, CheiralityException);
cameras += camera4; cameras += camera4;
measurements += Point2(400, 400); measurements += Point2(400, 400);