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

Revived sole camera test

release/4.3a0
dellaert 2015-03-15 07:09:55 -04:00
parent a920caf2ec
commit a3b6a47b2e
1 changed files with 37 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -63,32 +63,33 @@ TEST( triangulation, twoPoses) {
// 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 = triangulatePoint3(poses, sharedCal, boost::optional<Point3> actual1 = //
measurements, rank_tol, optimize); triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual1, 1e-7)); EXPECT(assert_equal(landmark, *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 = triangulatePoint3(poses, sharedCal, boost::optional<Point3> actual2 = //
measurements, rank_tol, optimize); triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual2, 1e-7)); EXPECT(assert_equal(landmark, *actual2, 1e-7));
// 3. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) // 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; optimize = false;
boost::optional<Point3> actual3 = triangulatePoint3(poses, sharedCal, boost::optional<Point3> actual3 = //
measurements, rank_tol, optimize); triangulatePoint3(poses, sharedCal, 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));
// 3. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) // 4. Now with optimization on
optimize = true; optimize = true;
boost::optional<Point3> actual4 = triangulatePoint3(poses, sharedCal, boost::optional<Point3> actual4 = //
measurements, rank_tol, optimize); triangulatePoint3(poses, sharedCal, 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));
} }
//****************************************************************************** //******************************************************************************
// Similar, but now with Bundler calibration
TEST( triangulation, twoPosesBundler) { TEST( triangulation, twoPosesBundler) {
boost::shared_ptr<Cal3Bundler> bundlerCal = // boost::shared_ptr<Cal3Bundler> bundlerCal = //
@ -109,17 +110,17 @@ TEST( triangulation, twoPosesBundler) {
bool optimize = true; bool optimize = true;
double rank_tol = 1e-9; double rank_tol = 1e-9;
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses, boost::optional<Point3> actual = //
bundlerCal, measurements, rank_tol, optimize); triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); EXPECT(assert_equal(landmark, *actual, 1e-7));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) // Add some noise and try again
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);
boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses, boost::optional<Point3> actual2 = //
bundlerCal, measurements, rank_tol, optimize); triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4));
} }
//****************************************************************************** //******************************************************************************
@ -130,17 +131,17 @@ TEST( triangulation, fourPoses) {
poses += pose1, pose2; poses += pose1, pose2;
measurements += z1, z2; measurements += z1, z2;
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses, boost::optional<Point3> actual = triangulatePoint3(poses, sharedCal,
sharedCal, measurements); measurements);
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); EXPECT(assert_equal(landmark, *actual, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) // 2. 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);
boost::optional<Point3> triangulated_landmark_noise = // boost::optional<Point3> actual2 = //
triangulatePoint3(poses, sharedCal, measurements); triangulatePoint3(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); EXPECT(assert_equal(landmark, *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 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
@ -164,8 +165,7 @@ 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) CHECK_EXCEPTION(camera4.project(landmark), CheiralityException);
;, CheiralityException);
poses += pose4; poses += pose4;
measurements += Point2(400, 400); measurements += Point2(400, 400);
@ -195,17 +195,17 @@ TEST( triangulation, fourPoses_distinct_Ks) {
cameras += camera1, camera2; cameras += camera1, camera2;
measurements += z1, z2; measurements += z1, z2;
boost::optional<Point3> triangulated_landmark = // boost::optional<Point3> actual = //
triangulatePoint3(cameras, measurements); triangulatePoint3(cameras, measurements);
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); EXPECT(assert_equal(landmark, *actual, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) // 2. 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);
boost::optional<Point3> triangulated_landmark_noise = // boost::optional<Point3> actual2 = //
triangulatePoint3(cameras, measurements); triangulatePoint3(cameras, measurements);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); EXPECT(assert_equal(landmark, *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 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
@ -260,23 +260,19 @@ TEST( triangulation, twoIdenticalPoses) {
} }
//****************************************************************************** //******************************************************************************
/*
TEST( triangulation, onePose) { TEST( triangulation, onePose) {
// we expect this test to fail with a TriangulationUnderconstrainedException // we expect this test to fail with a TriangulationUnderconstrainedException
// because there's only one camera observation // because there's only one camera observation
Cal3_S2 *sharedCal(1500, 1200, 0, 640, 480);
vector<Pose3> poses; vector<Pose3> poses;
vector<Point2> measurements; vector<Point2> measurements;
poses += Pose3(); poses += Pose3();
measurements += Point2(); measurements += Point2();
CHECK_EXCEPTION(triangulatePoint3(poses, measurements, *sharedCal), CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
TriangulationUnderconstrainedException); TriangulationUnderconstrainedException);
} }
*/
//****************************************************************************** //******************************************************************************
int main() { int main() {