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

Optimized CalibratedCamera.project derivative

release/4.3a0
Frank Dellaert 2012-01-08 04:06:29 +00:00
parent 5c18f57fe8
commit a032a93cf7
3 changed files with 50 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.cproject
View File

@ -1507,6 +1507,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="tests/timeCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/timeCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPoint2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPoint2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>

32
gtsam/geometry/CalibratedCamera.cpp
View File

@ -61,22 +61,42 @@ Point2 CalibratedCamera::project(const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
Point3 q = pose_.transform_to(point, H1, H2); Point3 q = pose_.transform_to(point, H1, H2);
#else
Point3 q = pose_.transform_to(point);
#endif
Point2 intrinsic = project_to_camera(q);
// Check if point is in front of camera // Check if point is in front of camera
if(q.z() <= 0) if(q.z() <= 0)
throw CheiralityException(); throw CheiralityException();
if (H1 || H2) { if (H1 || H2) {
Matrix H; #ifdef CALIBRATEDCAMERA_CHAIN_RULE
Point2 intrinsic = project_to_camera(q,H);
// just implement chain rule // just implement chain rule
Matrix H;
project_to_camera(q,H);
if (H1) *H1 = H * (*H1); if (H1) *H1 = H * (*H1);
if (H1) *H2 = H * (*H2); if (H2) *H2 = H * (*H2);
return intrinsic; #else
// optimized version, see CalibratedCamera.nb
const double z = q.z(), d = 1.0/z;
const double u = intrinsic.x(), v = intrinsic.y(), uv = u*v;
if (H1) *H1 = Matrix_(2,6,
uv,-(1.+u*u), v, -d , 0., d*u,
(1.+v*v), -uv,-u, 0.,-d , d*v
);
if (H2) {
const Matrix R(pose_.rotation().matrix());
*H2 = d * Matrix_(2,3,
R(0,0) - u*R(0,2), R(1,0) - u*R(1,2), R(2,0) - u*R(2,2),
R(0,1) - v*R(0,2), R(1,1) - v*R(1,2), R(2,1) - v*R(2,2)
);
} }
else #endif
return project_to_camera(q); }
return intrinsic;
} }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -78,10 +78,10 @@ TEST( CalibratedCamera, level2)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( CalibratedCamera, project) TEST( CalibratedCamera, project)
{ {
CHECK(assert_equal( camera.project(point1), Point2(-.16, .16) )); CHECK(assert_equal( Point2(-.16, .16), camera.project(point1) ));
CHECK(assert_equal( camera.project(point2), Point2(-.16, -.16) )); CHECK(assert_equal( Point2(-.16, -.16), camera.project(point2) ));
CHECK(assert_equal( camera.project(point3), Point2( .16, -.16) )); CHECK(assert_equal( Point2( .16, -.16), camera.project(point3) ));
CHECK(assert_equal( camera.project(point4), Point2( .16, .16) )); CHECK(assert_equal( Point2( .16, .16), camera.project(point4) ));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -105,9 +105,10 @@ TEST( CalibratedCamera, Dproject_point_pose)
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, pose1, point1);
Matrix numerical_point = numericalDerivative22(project2, pose1, point1); Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
CHECK(assert_equal(result, Point2(-.16, .16) )); CHECK(assert_equal(Point3(-0.08, 0.08, 0.5), camera.pose().transform_to(point1)));
CHECK(assert_equal(Dpose, numerical_pose, 1e-7)); CHECK(assert_equal(Point2(-.16, .16), result));
CHECK(assert_equal(Dpoint, numerical_point,1e-7)); CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
} }
/* ************************************************************************* */ /* ************************************************************************* */