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

fixed test

release/4.3a0
Luca 2014-10-17 15:48:20 -04:00
parent e96ceb2b4f
commit 5995b20ebe
1 changed files with 24 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -183,25 +183,30 @@ TEST( PinholeCamera, Dproject)
} }
/* ************************************************************************* */ /* ************************************************************************* */
//static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) { static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) {
// Point3 point(point2D.x(), point2D.y(), 1.0); return Camera(pose,cal).projectPointAtInfinity(point3D);
// return Camera(pose,cal).projectPointAtInfinity(point); }
//}
// TEST( PinholeCamera, Dproject_Infinity)
//TEST( PinholeCamera, Dproject_Infinity) {
//{ Matrix Dpose, Dpoint, Dcal;
// Matrix Dpose, Dpoint, Dcal; Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera
// Point2 point2D(-0.08,-0.08);
// Point3 point3D(point1.x(), point1.y(), 1.0); // test Projection
// Point2 result = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal);
// Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point2D, K); Point2 expected(-5.0, 5.0);
// Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point2D, K); CHECK(assert_equal(actual, expected, 1e-7));
// Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point2D, K);
// CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); // test Jacobians
// CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point3D, K);
// CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point3D, K);
//} Matrix numerical_point2x2 = numerical_point.block(0,0,2,2); // only the direction to the point matters
// Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point3D, K);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point2x2, Dpoint, 1e-7));
CHECK(assert_equal(numerical_cal, Dcal, 1e-7));
}
/* ************************************************************************* */ /* ************************************************************************* */
static Point2 project4(const Camera& camera, const Point3& point) { static Point2 project4(const Camera& camera, const Point3& point) {
return camera.project2(point); return camera.project2(point);