diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 3f586a67f..277c78ee0 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -46,7 +46,7 @@ public: /// evaluate the error virtual Vector evaluateError(const Pose3& pose, boost::optional H = boost::none) const { - SimpleCamera camera(*K_, pose); + SimpleCamera camera(pose, *K_); Point2 reprojectionError(camera.project(P_, H) - p_); return reprojectionError.vector(); } diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 139523802..2754bf7ef 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -56,9 +56,6 @@ namespace gtsam { /** constructor with pose and calibration */ PinholeCamera(const Pose3& pose, const Calibration& K):pose_(pose),K_(K) {} - /** alternative constructor with pose and calibration */ - PinholeCamera(const Calibration& K, const Pose3& pose):pose_(pose),K_(K) {} - /// @} /// @name Named Constructors /// @{ @@ -290,11 +287,6 @@ namespace gtsam { return pose_.transform_from(pc); } - /// backproject a 2-dimensional point to a 3-dimensional point at given depth - inline Point3 backproject_from_camera(const Point2& p, double depth) const { - return backproject(p, depth); - } - /** * Calculate range to a landmark * @param point 3D location of landmark diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 71f36998c..e46a247a0 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -35,7 +35,7 @@ const Pose3 pose1(Matrix_(3,3, ), Point3(0,0,0.5)); -const SimpleCamera camera(K, pose1); +const SimpleCamera camera(pose1, K); const Point3 point1(-0.08,-0.08, 0.0); const Point3 point2(-0.08, 0.08, 0.0); @@ -106,7 +106,7 @@ TEST( SimpleCamera, backproject2) { Point3 origin; Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down - SimpleCamera camera(K, Pose3(rot, origin)); + SimpleCamera camera(Pose3(rot, origin), K); Point3 actual = camera.backproject(Point2(), 1.); Point3 expected(0., 1., 0.); @@ -119,7 +119,7 @@ TEST( SimpleCamera, backproject2) /* ************************************************************************* */ Point2 project2(const Pose3& pose, const Point3& point) { - return SimpleCamera(K,pose).project(point); + return SimpleCamera(pose,K).project(point); } TEST( SimpleCamera, Dproject_point_pose) diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index e1ea38b27..1b1aeb57b 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -93,7 +93,7 @@ namespace gtsam { Vector evaluateError(const Pose3& pose, const Point3& point, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { try { - PinholeCamera camera(*K_, pose); + PinholeCamera camera(pose, *K_); Point2 reprojectionError(camera.project(point, H1, H2) - measured_); return reprojectionError.vector(); } catch( CheiralityException& e) { diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 9a139e581..4679dd740 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -524,9 +524,9 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { 0.0, 0.0, 1.0, 0.0, -1.0, 0.0)); Pose3 pose1(faceDownY, Point3()); // origin, left camera - SimpleCamera camera1(K, pose1); + SimpleCamera camera1(pose1, K); Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left - SimpleCamera camera2(K, pose2); + SimpleCamera camera2(pose2, K); Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away // keys