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

Fixed cheirality exceptions in arbitrary rotation cases

release/4.3a0
dellaert 2015-02-21 17:39:50 +01:00
parent 4a0891f34c
commit 39d474ec68
3 changed files with 16 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -118,7 +118,7 @@ TEST( CalibratedCamera, Dproject_point_pose)
// Add a test with more arbitrary rotation // Add a test with more arbitrary rotation
TEST( CalibratedCamera, Dproject_point_pose2) TEST( CalibratedCamera, Dproject_point_pose2)
{ {
static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, 0.5)); static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const CalibratedCamera camera(pose1); static const CalibratedCamera camera(pose1);
Matrix Dpose, Dpoint; Matrix Dpose, Dpoint;
camera.project(point1, Dpose, Dpoint); camera.project(point1, Dpose, Dpoint);

14
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -238,6 +238,20 @@ TEST( PinholeCamera, Dproject2)
EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
} }
/* ************************************************************************* */
// Add a test with more arbitrary rotation
TEST( PinholeCamera, Dproject3)
{
static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const Camera camera(pose1);
Matrix Dpose, Dpoint;
camera.project2(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project4, camera, point1);
Matrix numerical_point = numericalDerivative22(project4, camera, point1);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */ /* ************************************************************************* */
static double range0(const Camera& camera, const Point3& point) { static double range0(const Camera& camera, const Point3& point) {
return camera.range(point); return camera.range(point);

2
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -172,7 +172,7 @@ TEST( PinholePose, Dproject2)
// Add a test with more arbitrary rotation // Add a test with more arbitrary rotation
TEST( CalibratedCamera, Dproject3) TEST( CalibratedCamera, Dproject3)
{ {
static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, 0.5)); static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const Camera camera(pose1); static const Camera camera(pose1);
Matrix Dpose, Dpoint; Matrix Dpose, Dpoint;
camera.project2(point1, Dpose, Dpoint); camera.project2(point1, Dpose, Dpoint);