diff --git a/.cproject b/.cproject index 229ede4df..c459bcfa2 100644 --- a/.cproject +++ b/.cproject @@ -300,7 +300,6 @@ make - install true true @@ -308,7 +307,6 @@ make - check true true @@ -316,6 +314,7 @@ make + check true true @@ -323,7 +322,6 @@ make - testSimpleCamera.run true true @@ -339,6 +337,7 @@ make + testVSLAMFactor.run true true @@ -346,7 +345,6 @@ make - testCalibratedCamera.run true true @@ -354,6 +352,7 @@ make + testConditionalGaussian.run true true @@ -361,7 +360,6 @@ make - testPose2.run true true @@ -377,7 +375,6 @@ make - testRot3.run true true @@ -385,6 +382,7 @@ make + testNonlinearOptimizer.run true true @@ -392,7 +390,6 @@ make - testLinearFactor.run true true @@ -400,7 +397,6 @@ make - testConstrainedNonlinearFactorGraph.run true true @@ -408,7 +404,6 @@ make - testLinearFactorGraph.run true true @@ -416,6 +411,7 @@ make + testNonlinearFactorGraph.run true true @@ -423,7 +419,6 @@ make - testPose3.run true true @@ -431,13 +426,31 @@ make + testConstrainedLinearFactorGraph.run true true true + +make + +testVectorConfig.run +true +true +true + + +make + +testPoint2.run +true +true +true + make + install true true @@ -445,6 +458,7 @@ make + clean true true @@ -452,6 +466,7 @@ make + check true true diff --git a/cpp/CalibratedCamera.cpp b/cpp/CalibratedCamera.cpp index baaeab1cd..7aa8fafbf 100644 --- a/cpp/CalibratedCamera.cpp +++ b/cpp/CalibratedCamera.cpp @@ -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; } /* ************************************************************************* */ diff --git a/cpp/CalibratedCamera.h b/cpp/CalibratedCamera.h index 0f67f1d4d..57f0052b0 100644 --- a/cpp/CalibratedCamera.h +++ b/cpp/CalibratedCamera.h @@ -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_ */ diff --git a/cpp/SimpleCamera.cpp b/cpp/SimpleCamera.cpp index 751d25390..d9cf0f338 100644 --- a/cpp/SimpleCamera.cpp +++ b/cpp/SimpleCamera.cpp @@ -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; } /* ************************************************************************* */ diff --git a/cpp/SimpleCamera.h b/cpp/SimpleCamera.h index 5bcf55741..89530afaa 100644 --- a/cpp/SimpleCamera.h +++ b/cpp/SimpleCamera.h @@ -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 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); } diff --git a/cpp/testCalibratedCamera.cpp b/cpp/testCalibratedCamera.cpp index f9151e2fc..450b5513f 100644 --- a/cpp/testCalibratedCamera.cpp +++ b/cpp/testCalibratedCamera.cpp @@ -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) )); diff --git a/cpp/testSimpleCamera.cpp b/cpp/testSimpleCamera.cpp index 7eb54e2ca..cb580f458 100644 --- a/cpp/testSimpleCamera.cpp +++ b/cpp/testSimpleCamera.cpp @@ -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) ));