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

Combined evaluation/derivatives now return the function value instead of using a pointer

release/4.3a0
Frank Dellaert 2009-10-22 14:44:27 +00:00
parent 6c8531ed9c
commit 2aec16d3cf
7 changed files with 52 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
.cproject
View File

@ -300,7 +300,6 @@
<buildTargets>
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -308,7 +307,6 @@
</target>
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -316,6 +314,7 @@
</target>
<target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -323,7 +322,6 @@
</target>
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -339,6 +337,7 @@
</target>
<target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVSLAMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -346,7 +345,6 @@
</target>
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -354,6 +352,7 @@
</target>
<target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testConditionalGaussian.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -361,7 +360,6 @@
</target>
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -377,7 +375,6 @@
</target>
<target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -385,6 +382,7 @@
</target>
<target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -392,7 +390,6 @@
</target>
<target name="testLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testLinearFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -400,7 +397,6 @@
</target>
<target name="testConstrainedNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testConstrainedNonlinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -408,7 +404,6 @@
</target>
<target name="testLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testLinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -416,6 +411,7 @@
</target>
<target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -423,7 +419,6 @@
</target>
<target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -431,13 +426,31 @@
</target>
<target name="testConstrainedLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testConstrainedLinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVectorConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVectorConfig.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoint2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPoint2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -445,6 +458,7 @@
</target>
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -452,6 +466,7 @@
</target>
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>

8
cpp/CalibratedCamera.cpp
View File

@ -82,18 +82,20 @@ namespace gtsam {
}
/* ************************************************************************* */
void Dproject_pose_point(const CalibratedCamera& camera, const Point3& point,
Point2& intrinsic, Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point) {
Point2 Dproject_pose_point(const CalibratedCamera& camera, const Point3& point,
Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point) {
Point3 cameraPoint = transform_to(camera.pose(), point);
Matrix D_cameraPoint_pose = Dtransform_to1(camera.pose(), point); // 3*6
Matrix D_cameraPoint_point = Dtransform_to2(camera.pose()); // 3*3
intrinsic = project_to_camera(cameraPoint);
Point2 intrinsic = project_to_camera(cameraPoint);
Matrix D_intrinsic_cameraPoint = Dproject_to_camera1(cameraPoint);
D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose; // 2*6
D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point; // 2*3
return intrinsic;
}
/* ************************************************************************* */

13
cpp/CalibratedCamera.h
View File

@ -59,12 +59,15 @@ namespace gtsam {
/**
* This function receives the camera pose and the landmark location and
returns the location the point is supposed to appear in the image
* returns the location the point is supposed to appear in the image
* @param camera the CalibratedCamera
* @param point a 3D point to be projected
* @return the intrinsic coordinates of the projected point
*/
Point2 project(const CalibratedCamera& camera, const Point3& point);
/**
* Derivatives of project.
* Derivatives of project, same paramaters as project
*/
Matrix Dproject_pose(const CalibratedCamera& camera, const Point3& point);
Matrix Dproject_point(const CalibratedCamera& camera, const Point3& point);
@ -72,10 +75,10 @@ namespace gtsam {
/**
* super-duper combined evaluation + derivatives
* saves a lot of time because a lot of computation is shared
* @return project(camera,point)
*/
void
Dproject_pose_point(const CalibratedCamera& camera, const Point3& point,
Point2& intrinsic, Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point);
Point2 Dproject_pose_point(const CalibratedCamera& camera,
const Point3& point, Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point);
}
#endif /* CalibratedCAMERA_H_ */

8
cpp/SimpleCamera.cpp
View File

@ -73,18 +73,20 @@ namespace gtsam {
}
/* ************************************************************************* */
void Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
Point2& projection, Matrix& D_projection_pose, Matrix& D_projection_point) {
Point2 Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
Matrix& D_projection_pose, Matrix& D_projection_point) {
Point2 intrinsic = project(camera.calibrated_, point);
Matrix D_intrinsic_pose = Dproject_pose(camera.calibrated_, point);
Matrix D_intrinsic_point = Dproject_point(camera.calibrated_, point);
projection = uncalibrate(camera.K_, intrinsic);
Point2 projection = uncalibrate(camera.K_, intrinsic);
Matrix D_projection_intrinsic = Duncalibrate2(camera.K_, intrinsic);
D_projection_pose = D_projection_intrinsic * D_intrinsic_pose;
D_projection_point = D_projection_intrinsic * D_intrinsic_point;
return projection;
}
/* ************************************************************************* */

16
cpp/SimpleCamera.h
View File

@ -58,9 +58,8 @@ namespace gtsam {
// Friends
friend Matrix Dproject_pose(const SimpleCamera& camera, const Point3& point);
friend Matrix Dproject_point(const SimpleCamera& camera, const Point3& point);
friend void Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
Point2& projection, Matrix& D_projection_pose,
Matrix& D_projection_point);
friend Point2 Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
Matrix& D_projection_pose, Matrix& D_projection_point);
};
@ -70,13 +69,14 @@ namespace gtsam {
/**
* This function receives the camera pose and the landmark location and
returns the location the point is supposed to appear in the image as well as the sign of the depth
* returns the location the point is supposed to appear in the image
* as well as the sign of the depth.
*/
std::pair<Point2, bool> projectSafe(const SimpleCamera& camera, const Point3& point);
/**
* This function receives the camera pose and the landmark location and
returns the location the point is supposed to appear in the image
* returns the location the point is supposed to appear in the image
*/
Point2 project(const SimpleCamera& camera, const Point3& point);
@ -90,10 +90,8 @@ namespace gtsam {
* super-duper combined evaluation + derivatives
* saves a lot of time because a lot of computation is shared
*/
void
Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
Point2& projection, Matrix& D_projection_pose,
Matrix& D_projection_point);
Point2 Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
Matrix& D_projection_pose, Matrix& D_projection_point);
}

3
cpp/testCalibratedCamera.cpp
View File

@ -100,9 +100,8 @@ TEST( CalibratedCamera, Dproject_point)
TEST( CalibratedCamera, Dproject_point_pose)
{
Point2 result;
Matrix Dpose, Dpoint;
Dproject_pose_point(camera, point1, result, Dpose, Dpoint);
Point2 result = Dproject_pose_point(camera, point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project2, pose1, point1);
Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
CHECK(assert_equal(result, Point2(-.16, .16) ));

3
cpp/testSimpleCamera.cpp
View File

@ -81,9 +81,8 @@ TEST( SimpleCamera, Dproject_point)
TEST( SimpleCamera, Dproject_point_pose)
{
Point2 result;
Matrix Dpose, Dpoint;
Dproject_pose_point(camera, point1, result, Dpose, Dpoint);
Point2 result = Dproject_pose_point(camera, point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project2, pose1, point1);
Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
CHECK(assert_equal(result, Point2(-100, 100) ));