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

added twoPoses test

release/4.3a0
dellaert 2014-03-02 13:44:18 -05:00
parent 7b93cd207c
commit bf779af3d1
1 changed files with 41 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -29,19 +29,51 @@ using namespace boost::assign;
// Some common constants
static const boost::shared_ptr<Cal3_S2> sharedCal = //
boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
// 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),
gtsam::Point3(0, 0, 1));
PinholeCamera<Cal3_S2> level_camera(level_pose, *sharedCal);
// create second camera 1 meter to the right of first camera
static const Pose3 level_pose_right = level_pose
* Pose3(Rot3(), Point3(1, 0, 0));
PinholeCamera<Cal3_S2> level_camera_right(level_pose_right, *sharedCal);
// landmark ~5 meters infront of camera
static const Point3 landmark(5, 0.5, 1.2);
static const boost::shared_ptr<Cal3_S2> sharedCal = //
boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
// 1. Project two landmarks into two cameras and triangulate
Point2 level_uv = level_camera.project(landmark);
Point2 level_uv_right = level_camera_right.project(landmark);
/* ************************************************************************* */
TEST( triangulation, twoPoses) {
vector<Pose3> poses;
vector<Point2> measurements;
poses += level_pose, level_pose_right;
measurements += level_uv, level_uv_right;
bool optimize = true;
double rank_tol = 1e-9;
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
// 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(1) += Point2(-0.2, 0.3);
boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses,
sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
}
/* ************************************************************************* */
@ -81,16 +113,6 @@ TEST( triangulation, twoPosesBundler) {
/* ************************************************************************* */
TEST( triangulation, fourPoses) {
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
SimpleCamera level_camera(level_pose, *sharedCal);
// create second camera 1 meter to the right of first camera
SimpleCamera level_camera_right(level_pose_right, *sharedCal);
// 1. Project two landmarks into two cameras and triangulate
Point2 level_uv = level_camera.project(landmark);
Point2 level_uv_right = level_camera_right.project(landmark);
vector<Pose3> poses;
vector<Point2> measurements;