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

Some more refactoring

release/4.3a0
dellaert 2014-03-02 13:49:42 -05:00
parent bf779af3d1
commit dfee108e53
1 changed files with 53 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

111
gtsam_unstable/geometry/tests/testTriangulation.cpp
View File

@ -33,21 +33,20 @@ static const boost::shared_ptr<Cal3_S2> sharedCal = //
boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480); boost::make_shared<Cal3_S2>(1500, 1200, 0, 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 Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2);
gtsam::Point3(0, 0, 1)); static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
PinholeCamera<Cal3_S2> level_camera(level_pose, *sharedCal); PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
// 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 level_pose_right = level_pose static const Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
* Pose3(Rot3(), Point3(1, 0, 0)); PinholeCamera<Cal3_S2> camera2(pose2, *sharedCal);
PinholeCamera<Cal3_S2> level_camera_right(level_pose_right, *sharedCal);
// landmark ~5 meters infront of camera // landmark ~5 meters infront of camera
static const Point3 landmark(5, 0.5, 1.2); static const Point3 landmark(5, 0.5, 1.2);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 level_uv = level_camera.project(landmark); Point2 z1 = camera1.project(landmark);
Point2 level_uv_right = level_camera_right.project(landmark); Point2 z2 = camera2.project(landmark);
/* ************************************************************************* */ /* ************************************************************************* */
@ -56,8 +55,8 @@ TEST( triangulation, twoPoses) {
vector<Pose3> poses; vector<Pose3> poses;
vector<Point2> measurements; vector<Point2> measurements;
poses += level_pose, level_pose_right; poses += pose1, pose2;
measurements += level_uv, level_uv_right; measurements += z1, z2;
bool optimize = true; bool optimize = true;
double rank_tol = 1e-9; double rank_tol = 1e-9;
@ -81,18 +80,18 @@ TEST( triangulation, twoPosesBundler) {
boost::shared_ptr<Cal3Bundler> bundlerCal = // boost::shared_ptr<Cal3Bundler> bundlerCal = //
boost::make_shared<Cal3Bundler>(1500, 0, 0, 640, 480); boost::make_shared<Cal3Bundler>(1500, 0, 0, 640, 480);
PinholeCamera<Cal3Bundler> level_camera(level_pose, *bundlerCal); PinholeCamera<Cal3Bundler> camera1(pose1, *bundlerCal);
PinholeCamera<Cal3Bundler> level_camera_right(level_pose_right, *bundlerCal); PinholeCamera<Cal3Bundler> camera2(pose2, *bundlerCal);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 level_uv = level_camera.project(landmark); Point2 z1 = camera1.project(landmark);
Point2 level_uv_right = level_camera_right.project(landmark); Point2 z2 = camera2.project(landmark);
vector<Pose3> poses; vector<Pose3> poses;
vector<Point2> measurements; vector<Point2> measurements;
poses += level_pose, level_pose_right; poses += pose1, pose2;
measurements += level_uv, level_uv_right; measurements += z1, z2;
bool optimize = true; bool optimize = true;
double rank_tol = 1e-9; double rank_tol = 1e-9;
@ -116,8 +115,8 @@ TEST( triangulation, fourPoses) {
vector<Pose3> poses; vector<Pose3> poses;
vector<Point2> measurements; vector<Point2> measurements;
poses += level_pose, level_pose_right; poses += pose1, pose2;
measurements += level_uv, level_uv_right; measurements += z1, z2;
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses, boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
sharedCal, measurements); sharedCal, measurements);
@ -127,21 +126,20 @@ TEST( triangulation, fourPoses) {
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> triangulated_landmark_noise = //
sharedCal, measurements); triangulatePoint3(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 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 pose_top = level_pose Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
* Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); SimpleCamera camera3(pose3, *sharedCal);
SimpleCamera camera_top(pose_top, *sharedCal); Point2 z3 = camera3.project(landmark);
Point2 top_uv = camera_top.project(landmark);
poses += pose_top; poses += pose3;
measurements += top_uv + Point2(0.1, -0.1); measurements += z3 + Point2(0.1, -0.1);
boost::optional<Point3> triangulated_3cameras = triangulatePoint3(poses, boost::optional<Point3> triangulated_3cameras = //
sharedCal, measurements); triangulatePoint3(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization // Again with nonlinear optimization
@ -150,14 +148,13 @@ TEST( triangulation, fourPoses) {
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); EXPECT(assert_equal(landmark, *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 level_pose180 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Point3(0, 0, 1)); SimpleCamera camera4(pose4, *sharedCal);
SimpleCamera camera_180(level_pose180, *sharedCal);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(camera_180.project(landmark);, CheiralityException); CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException);
poses += level_pose180; poses += pose4;
measurements += Point2(400, 400); measurements += Point2(400, 400);
CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
@ -170,24 +167,24 @@ TEST( triangulation, fourPoses) {
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)
SimpleCamera level_camera(level_pose, K1); SimpleCamera camera1(pose1, 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);
SimpleCamera level_camera_right(level_pose_right, K2); SimpleCamera camera2(pose2, K2);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 level_uv = level_camera.project(landmark); Point2 z1 = camera1.project(landmark);
Point2 level_uv_right = level_camera_right.project(landmark); Point2 z2 = camera2.project(landmark);
vector<SimpleCamera> cameras; vector<SimpleCamera> cameras;
vector<Point2> measurements; vector<Point2> measurements;
cameras += level_camera, level_camera_right; cameras += camera1, camera2;
measurements += level_uv, level_uv_right; measurements += z1, z2;
boost::optional<Point3> triangulated_landmark = triangulatePoint3(cameras, boost::optional<Point3> triangulated_landmark = //
measurements); triangulatePoint3(cameras, measurements);
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_landmark, 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)
@ -199,17 +196,16 @@ TEST( triangulation, fourPoses_distinct_Ks) {
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 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 pose_top = level_pose Pose3 pose3 = pose1 * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
* 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);
SimpleCamera camera_top(pose_top, K3); SimpleCamera camera3(pose3, K3);
Point2 top_uv = camera_top.project(landmark); Point2 z3 = camera3.project(landmark);
cameras += camera_top; cameras += camera3;
measurements += top_uv + Point2(0.1, -0.1); measurements += z3 + Point2(0.1, -0.1);
boost::optional<Point3> triangulated_3cameras = triangulatePoint3(cameras, boost::optional<Point3> triangulated_3cameras = //
measurements); triangulatePoint3(cameras, measurements);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization // Again with nonlinear optimization
@ -218,15 +214,14 @@ TEST( triangulation, fourPoses_distinct_Ks) {
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); EXPECT(assert_equal(landmark, *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 level_pose180 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Pose3 pose4 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
Point3(0, 0, 1));
Cal3_S2 K4(700, 500, 0, 640, 480); Cal3_S2 K4(700, 500, 0, 640, 480);
SimpleCamera camera_180(level_pose180, K4); SimpleCamera camera4(pose4, K4);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(camera_180.project(landmark);, CheiralityException); CHECK_EXCEPTION(camera4.project(landmark);, CheiralityException);
cameras += camera_180; cameras += camera4;
measurements += Point2(400, 400); measurements += Point2(400, 400);
CHECK_EXCEPTION(triangulatePoint3(cameras, measurements), CHECK_EXCEPTION(triangulatePoint3(cameras, measurements),
TriangulationCheiralityException); TriangulationCheiralityException);
@ -237,16 +232,16 @@ TEST( triangulation, fourPoses_distinct_Ks) {
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)
SimpleCamera level_camera(level_pose, *sharedCal); SimpleCamera camera1(pose1, *sharedCal);
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 level_uv = level_camera.project(landmark); Point2 z1 = camera1.project(landmark);
vector<Pose3> poses; vector<Pose3> poses;
vector<Point2> measurements; vector<Point2> measurements;
poses += level_pose, level_pose; poses += pose1, pose1;
measurements += level_uv, level_uv; measurements += z1, z1;
CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
TriangulationUnderconstrainedException); TriangulationUnderconstrainedException);