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

Added more diagnostic test for derivatives

release/4.3a0
dellaert 2015-02-21 11:00:37 +01:00
parent 2ee090ece5
commit 059ff82beb
2 changed files with 33 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -29,6 +29,7 @@ using namespace gtsam;
GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera) GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera)
// Camera situated at 0.5 meters high, looking down
static const Pose3 pose1((Matrix)(Matrix(3,3) << static const Pose3 pose1((Matrix)(Matrix(3,3) <<
1., 0., 0., 1., 0., 0.,
0.,-1., 0., 0.,-1., 0.,
@ -97,24 +98,37 @@ TEST( CalibratedCamera, Dproject_to_camera1) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Point2 project2(const Pose3& pose, const Point3& point) { static Point2 project2(const CalibratedCamera& camera, const Point3& point) {
return CalibratedCamera(pose).project(point); return camera.project(point);
} }
TEST( CalibratedCamera, Dproject_point_pose) TEST( CalibratedCamera, Dproject_point_pose)
{ {
Matrix Dpose, Dpoint; Matrix Dpose, Dpoint;
Point2 result = camera.project(point1, Dpose, Dpoint); Point2 result = camera.project(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project2, pose1, point1); Matrix numerical_pose = numericalDerivative21(project2, camera, point1);
Matrix numerical_point = numericalDerivative22(project2, pose1, point1); Matrix numerical_point = numericalDerivative22(project2, camera, point1);
CHECK(assert_equal(Point3(-0.08, 0.08, 0.5), camera.pose().transform_to(point1))); CHECK(assert_equal(Point3(-0.08, 0.08, 0.5), camera.pose().transform_to(point1)));
CHECK(assert_equal(Point2(-.16, .16), result)); CHECK(assert_equal(Point2(-.16, .16), result));
CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
} }
/* ************************************************************************* */
// Add a test with more arbitrary rotation
TEST( CalibratedCamera, Dproject_point_pose2)
{
static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, 0.5));
static const CalibratedCamera camera(pose1);
Matrix Dpose, Dpoint;
camera.project(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project2, camera, point1);
Matrix numerical_point = numericalDerivative22(project2, camera, point1);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -182,6 +182,20 @@ TEST( PinholePose, Dproject2)
EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
} }
/* ************************************************************************* */
// Add a test with more arbitrary rotation
TEST( CalibratedCamera, Dproject3)
{
static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, 0.5));
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);