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

another unit test for distinct calibrations, more documentation

release/4.3a0
Chris Beall 2013-10-22 15:52:10 +00:00
parent e98c80aad0
commit 6d288a193d
2 changed files with 89 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -32,7 +32,7 @@ using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* */
TEST( triangulation, twoPoses) {
TEST( triangulation, fourPoses) {
Cal3_S2 K(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
@ -95,6 +95,79 @@ TEST( triangulation, twoPoses) {
CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), TriangulationCheiralityException);
}
/* ************************************************************************* */
TEST( triangulation, fourPoses_distinct_Ks) {
Cal3_S2 K1(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera level_camera(level_pose, K1);
// create second camera 1 meter to the right of first camera
Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
Cal3_S2 K2(1600, 1300, 0, 650, 440);
SimpleCamera level_camera_right(level_pose_right, K2);
// landmark ~5 meters infront of camera
Point3 landmark(5, 0.5, 1.2);
// 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;
poses += level_pose, level_pose_right;
measurements += level_uv, level_uv_right;
std::vector<boost::shared_ptr<Cal3_S2> > Ks;
Ks.push_back(boost::make_shared<Cal3_S2>(K1));
Ks.push_back(boost::make_shared<Cal3_S2>(K2));
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses, measurements, Ks);
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, measurements, Ks);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
// 3. Add a slightly rotated third camera above, again with measurement noise
Pose3 pose_top = level_pose * Pose3(Rot3::ypr(0.1,0.2,0.1), Point3(0.1,-2,-.1));
Cal3_S2 K3(700, 500, 0, 640, 480);
SimpleCamera camera_top(pose_top, K3);
Point2 top_uv = camera_top.project(landmark);
poses += pose_top;
measurements += top_uv + Point2(0.1, -0.1);
Ks.push_back(boost::make_shared<Cal3_S2>(K3));
boost::optional<Point3> triangulated_3cameras = triangulatePoint3(poses, measurements, Ks);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization
boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses, measurements, Ks, 1e-9, true);
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
// 4. Test failure: Add a 4th camera facing the wrong way
Pose3 level_pose180 = Pose3(Rot3::ypr(M_PI/2, 0., -M_PI/2), Point3(0,0,1));
Cal3_S2 K4(700, 500, 0, 640, 480);
SimpleCamera camera_180(level_pose180, K4);
CHECK_EXCEPTION(camera_180.project(landmark);, CheiralityException);
poses += level_pose180;
measurements += Point2(400,400);
Ks.push_back(boost::make_shared<Cal3_S2>(K4));
CHECK_EXCEPTION(triangulatePoint3(poses, measurements, Ks), TriangulationCheiralityException);
}
/* ************************************************************************* */
TEST( triangulation, twoIdenticalPoses) {
Cal3_S2 K(1500, 1200, 0, 640, 480);

17
gtsam_unstable/geometry/triangulation.h
View File

@ -56,9 +56,22 @@ public:
/* ************************************************************************* */
// See Hartley and Zisserman, 2nd Ed., page 312
/**
*
* @param poses Camera poses
* @param projection_matrices Projection matrices (K*P^-1)
* @param measurements 2D measurements
* @param Ks vector of calibrations
* @param rank_tol SVD rank tolerance
* @param Flag to turn on nonlinear refinement of triangulation
* @return Triangulated Point3
*/
template<class CALIBRATION>
Point3 triangulateDLT(const std::vector<Pose3>& poses, const std::vector<Matrix>& projection_matrices,
const std::vector<Point2>& measurements, const std::vector<boost::shared_ptr<CALIBRATION> >& Ks, double rank_tol, bool optimize) {
Point3 triangulateDLT(const std::vector<Pose3>& poses,
const std::vector<Matrix>& projection_matrices,
const std::vector<Point2>& measurements,
const std::vector<boost::shared_ptr<CALIBRATION> >& Ks, double rank_tol,
bool optimize) {
Matrix A = zeros(projection_matrices.size() *2, 4);