diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 7bc5949cc..bc5279c7b 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -144,8 +144,9 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) { // Check last diagonal element - Eigen does not check it if (nFrontal >= 2) { int exp2, exp1; - (void)frexp(R(topleft + nFrontal - 2, topleft + nFrontal - 2), &exp2); - (void)frexp(R(topleft + nFrontal - 1, topleft + nFrontal - 1), &exp1); + // NOTE(gareth): R is already the size of A, so we don't need to add topleft here. + (void)frexp(R(nFrontal - 2, nFrontal - 2), &exp2); + (void)frexp(R(nFrontal - 1, nFrontal - 1), &exp1); return (exp2 - exp1 < underconstrainedExponentDifference); } else if (nFrontal == 1) { int exp1; diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index dcb0425b7..2fa6e33c6 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -298,7 +298,7 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (* /** * Compute numerical derivative in argument 1 of 4-argument function - * @param h ternary function yielding m-vector + * @param h quartic function yielding m-vector * @param x1 n-dimensional first argument value * @param x2 second argument value * @param x3 third argument value @@ -317,9 +317,15 @@ typename internal::FixedSizeMatrix::type numericalDerivative41( return numericalDerivative11(boost::bind(h, _1, x2, x3, x4), x1, delta); } +template +inline typename internal::FixedSizeMatrix::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + return numericalDerivative41(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); +} + /** * Compute numerical derivative in argument 2 of 4-argument function - * @param h ternary function yielding m-vector + * @param h quartic function yielding m-vector * @param x1 first argument value * @param x2 n-dimensional second argument value * @param x3 third argument value @@ -338,9 +344,15 @@ typename internal::FixedSizeMatrix::type numericalDerivative42( return numericalDerivative11(boost::bind(h, x1, _1, x3, x4), x2, delta); } +template +inline typename internal::FixedSizeMatrix::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + return numericalDerivative42(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); +} + /** * Compute numerical derivative in argument 3 of 4-argument function - * @param h ternary function yielding m-vector + * @param h quartic function yielding m-vector * @param x1 first argument value * @param x2 second argument value * @param x3 n-dimensional third argument value @@ -359,9 +371,15 @@ typename internal::FixedSizeMatrix::type numericalDerivative43( return numericalDerivative11(boost::bind(h, x1, x2, _1, x4), x3, delta); } +template +inline typename internal::FixedSizeMatrix::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + return numericalDerivative43(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); +} + /** * Compute numerical derivative in argument 4 of 4-argument function - * @param h ternary function yielding m-vector + * @param h quartic function yielding m-vector * @param x1 first argument value * @param x2 second argument value * @param x3 third argument value @@ -380,6 +398,152 @@ typename internal::FixedSizeMatrix::type numericalDerivative44( return numericalDerivative11(boost::bind(h, x1, x2, x3, _1), x4, delta); } +template +inline typename internal::FixedSizeMatrix::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + return numericalDerivative44(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); +} + +/** + * Compute numerical derivative in argument 1 of 5-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative51( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, _1, x2, x3, x4, x5), x1, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + return numericalDerivative51(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); +} + +/** + * Compute numerical derivative in argument 2 of 5-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative52( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, _1, x3, x4, x5), x2, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + return numericalDerivative52(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); +} + +/** + * Compute numerical derivative in argument 3 of 5-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative53( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, x2, _1, x4, x5), x3, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + return numericalDerivative53(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); +} + +/** + * Compute numerical derivative in argument 4 of 5-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative54( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, x2, x3, _1, x5), x4, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + return numericalDerivative54(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); +} + +/** + * Compute numerical derivative in argument 5 of 5-argument function + * @param h quintic function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param x5 fifth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative55( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, x2, x3, x4, _1), x5, delta); +} + +template +inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), + const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { + return numericalDerivative55(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); +} + /** * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar * function. This is implemented simply as the derivative of the gradient. diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index c55e5067b..4ef09beb1 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -24,6 +24,9 @@ #include +#include + + // whether to print the serialized text to stdout const bool verbose = false; @@ -142,4 +145,3 @@ bool equalsDereferencedBinary(const T& input = T()) { } // \namespace serializationTestHelpers } // \namespace gtsam - diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp index 1db28bcc8..d863eaba3 100644 --- a/gtsam/base/tests/testSerializationBase.cpp +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,7 +11,7 @@ /** * @file testSerializationBase.cpp - * @brief + * @brief * @author Richard Roberts * @date Feb 7, 2012 */ diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 026becebe..3c9fb1563 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -142,8 +142,11 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, Matrix23 Dpc_rot; Matrix2 Dpc_point; const Unit3 pc = pose().rotation().unrotate(pw, Dpose ? &Dpc_rot : 0, - Dpose ? &Dpc_point : 0); - + Dpoint ? &Dpc_point : 0); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (pc.unitVector().z() <= 0) + throw CheiralityException(); +#endif // camera to normalized image coordinate Matrix2 Dpn_pc; const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0); @@ -161,8 +164,12 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, return pn; } /* ************************************************************************* */ -Point3 PinholeBase::backproject_from_camera(const Point2& p, - const double depth) { +Point3 PinholeBase::BackprojectFromCamera(const Point2& p, + const double depth, OptionalJacobian<3, 2> Dpoint, OptionalJacobian<3, 1> Ddepth) { + if (Dpoint) + *Dpoint << depth, 0, 0, depth, 0, 0; + if (Ddepth) + *Ddepth << p.x(), p.y(), 1; return Point3(p.x() * depth, p.y() * depth, depth); } diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index f1fa509c1..f2bed032f 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -201,7 +201,9 @@ public: OptionalJacobian<2, 2> Dpoint = boost::none) const; /// backproject a 2-dimensional point to a 3-dimensional point at given depth - static Point3 backproject_from_camera(const Point2& p, const double depth); + static Point3 BackprojectFromCamera(const Point2& p, const double depth, + OptionalJacobian<3, 2> Dpoint = boost::none, + OptionalJacobian<3, 1> Ddepth = boost::none); /// @} /// @name Advanced interface @@ -337,8 +339,28 @@ public: boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; /// backproject a 2-dimensional point to a 3-dimensional point at given depth - Point3 backproject(const Point2& pn, double depth) const { - return pose().transform_from(backproject_from_camera(pn, depth)); + Point3 backproject(const Point2& pn, double depth, + OptionalJacobian<3, 6> Dresult_dpose = boost::none, + OptionalJacobian<3, 2> Dresult_dp = boost::none, + OptionalJacobian<3, 1> Dresult_ddepth = boost::none) const { + + Matrix32 Dpoint_dpn; + Matrix31 Dpoint_ddepth; + const Point3 point = BackprojectFromCamera(pn, depth, + Dresult_dp ? &Dpoint_dpn : 0, + Dresult_ddepth ? &Dpoint_ddepth : 0); + + Matrix33 Dresult_dpoint; + const Point3 result = pose().transform_from(point, Dresult_dpose, + (Dresult_ddepth || + Dresult_dp) ? &Dresult_dpoint : 0); + + if (Dresult_dp) + *Dresult_dp = Dresult_dpoint * Dpoint_dpn; + if (Dresult_ddepth) + *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth; + + return result; } /** @@ -404,4 +426,3 @@ template struct Range : HasRange {}; } // namespace gtsam - diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 43ba78ea2..937eb0785 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -85,25 +85,9 @@ public: return pn; } - /** project a point from world coordinate to the image - * @param pw is a point in the world coordinates - */ - Point2 project(const Point3& pw) const { - const Point2 pn = PinholeBase::project2(pw); // project to normalized coordinates - return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates - } - - /** project a point from world coordinate to the image - * @param pw is a point at infinity in the world coordinates - */ - Point2 project(const Unit3& pw) const { - const Unit3 pc = pose().rotation().unrotate(pw); // convert to camera frame - const Point2 pn = PinholeBase::Project(pc); // project to normalized coordinates - return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates - } /** Templated projection of a point (possibly at infinity) from world coordinate to the image - * @param pw is a 3D point or aUnit3 (point at infinity) in world coordinates + * @param pw is a 3D point or a Unit3 (point at infinity) in world coordinates * @param Dpose is the Jacobian w.r.t. pose3 * @param Dpoint is the Jacobian w.r.t. point3 * @param Dcal is the Jacobian w.r.t. calibration @@ -131,25 +115,51 @@ public: } /// project a 3D point from world coordinates into the image - Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose, + Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none, OptionalJacobian<2, DimK> Dcal = boost::none) const { return _project(pw, Dpose, Dpoint, Dcal); } /// project a point at infinity from world coordinates into the image - Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose, + Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 2> Dpoint = boost::none, OptionalJacobian<2, DimK> Dcal = boost::none) const { return _project(pw, Dpose, Dpoint, Dcal); } /// backproject a 2-dimensional point to a 3-dimensional point at given depth - Point3 backproject(const Point2& p, double depth) const { - const Point2 pn = calibration().calibrate(p); - return pose().transform_from(backproject_from_camera(pn, depth)); + Point3 backproject(const Point2& p, double depth, + OptionalJacobian<3, 6> Dresult_dpose = boost::none, + OptionalJacobian<3, 2> Dresult_dp = boost::none, + OptionalJacobian<3, 1> Dresult_ddepth = boost::none, + OptionalJacobian<3, DimK> Dresult_dcal = boost::none) const { + typedef Eigen::Matrix Matrix2K; + Matrix2K Dpn_dcal; + Matrix22 Dpn_dp; + const Point2 pn = calibration().calibrate(p, Dresult_dcal ? &Dpn_dcal : 0, + Dresult_dp ? &Dpn_dp : 0); + Matrix32 Dpoint_dpn; + Matrix31 Dpoint_ddepth; + const Point3 point = BackprojectFromCamera(pn, depth, + (Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0, + Dresult_ddepth ? &Dpoint_ddepth : 0); + Matrix33 Dresult_dpoint; + const Point3 result = pose().transform_from(point, Dresult_dpose, + (Dresult_ddepth || + Dresult_dp || + Dresult_dcal) ? &Dresult_dpoint : 0); + if (Dresult_dcal) + *Dresult_dcal = Dresult_dpoint * Dpoint_dpn * Dpn_dcal; // (3x3)*(3x2)*(2xDimK) + if (Dresult_dp) + *Dresult_dp = Dresult_dpoint * Dpoint_dpn * Dpn_dp; // (3x3)*(3x2)*(2x2) + if (Dresult_ddepth) + *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth; // (3x3)*(3x1) + + return result; } + /// backproject a 2-dimensional point to a 3-dimensional point at infinity Unit3 backprojectPointAtInfinity(const Point2& p) const { const Point2 pn = calibration().calibrate(p); @@ -302,6 +312,11 @@ public: Base(v), K_(new CALIBRATION(K)) { } + // Init from Pose3 and calibration + PinholePose(const Pose3 &pose, const Vector &K) : + Base(pose), K_(new CALIBRATION(K)) { + } + /// @} /// @name Testable /// @{ diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index e629eb3c6..33a88db1f 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -113,7 +113,7 @@ static Point2 Project2(const Unit3& point) { return PinholeBase::Project(point); } -Unit3 pointAtInfinity(0, 0, 1000); +Unit3 pointAtInfinity(0, 0, -1000); TEST( CalibratedCamera, DProjectInfinity) { Matrix test1; CalibratedCamera::Project(pointAtInfinity, test1); @@ -127,6 +127,7 @@ static Point2 project2(const CalibratedCamera& camera, const Point3& point) { return camera.project(point); } + TEST( CalibratedCamera, Dproject_point_pose) { Matrix Dpose, Dpoint; @@ -173,7 +174,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity) // Add a test with more arbitrary rotation TEST( CalibratedCamera, Dproject_point_pose2_infinity) { - static const Pose3 pose(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); + static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10)); static const CalibratedCamera camera(pose); Matrix Dpose, Dpoint; camera.project2(pointAtInfinity, Dpose, Dpoint); @@ -183,7 +184,48 @@ TEST( CalibratedCamera, Dproject_point_pose2_infinity) CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); } +static Point3 BackprojectFromCamera(const CalibratedCamera& camera, const Point2& point, + const double& depth) { + return camera.BackprojectFromCamera(point, depth); +} +TEST( CalibratedCamera, DBackprojectFromCamera) +{ + static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10)); + static const CalibratedCamera camera(pose); + static const double depth = 5.4; + static const Point2 point(10.1, 50.3); + Matrix Dpoint, Ddepth; + camera.BackprojectFromCamera(point, depth, Dpoint, Ddepth); + Matrix numerical_point = numericalDerivative32(BackprojectFromCamera, camera, point, depth); + Matrix numerical_depth = numericalDerivative33(BackprojectFromCamera, camera, point, depth); + CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); + CHECK(assert_equal(numerical_depth, Ddepth, 1e-7)); +} + + +static Point3 backproject(const Pose3& pose, const Point2& point, const double& depth) { + return CalibratedCamera(pose).backproject(point, depth); +} +TEST( PinholePose, Dbackproject) +{ + Matrix36 Dpose; + Matrix31 Ddepth; + Matrix32 Dpoint; + const Point2 point(-100, 100); + const double depth(10); + static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10)); + static const CalibratedCamera camera(pose); + camera.backproject(point, depth, Dpose, Dpoint, Ddepth); + Matrix expectedDpose = numericalDerivative31(backproject, pose, point, depth); + Matrix expectedDpoint = numericalDerivative32(backproject, pose, point, depth); + Matrix expectedDdepth = numericalDerivative33(backproject, pose,point, depth); + + EXPECT(assert_equal(expectedDpose, Dpose, 1e-7)); + EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); + EXPECT(assert_equal(expectedDdepth, Ddepth, 1e-7)); +} + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index ecbb92061..05d48f4cc 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -191,7 +191,7 @@ static Point2 project(const Pose3& pose, const Unit3& pointAtInfinity, /* ************************************************************************* */ TEST( PinholePose, DprojectAtInfinity2) { - Unit3 pointAtInfinity(0,0,1000); + Unit3 pointAtInfinity(0,0,-1000); Matrix Dpose, Dpoint; Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint); Matrix expectedDcamera = numericalDerivative31(project, pose, pointAtInfinity, K); @@ -201,6 +201,32 @@ TEST( PinholePose, DprojectAtInfinity2) EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); } + +static Point3 backproject(const Pose3& pose, const Cal3_S2& cal, + const Point2& p, const double& depth) { + return Camera(pose, cal.vector()).backproject(p, depth); +} + +TEST( PinholePose, Dbackproject) +{ + Matrix36 Dpose; + Matrix31 Ddepth; + Matrix32 Dpoint; + Matrix Dcal; + const Point2 point(-100, 100); + const double depth(10); + camera.backproject(point, depth, Dpose, Dpoint, Ddepth, Dcal); + Matrix expectedDpose = numericalDerivative41(backproject, pose, *K, point, depth); + Matrix expectedDcal = numericalDerivative42(backproject, pose, *K, point, depth); + Matrix expectedDpoint = numericalDerivative43(backproject, pose, *K, point, depth); + Matrix expectedDdepth = numericalDerivative44(backproject, pose, *K, point, depth); + + EXPECT(assert_equal(expectedDpose, Dpose, 1e-7)); + EXPECT(assert_equal(expectedDcal, Dcal, 1e-7)); + EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); + EXPECT(assert_equal(expectedDdepth, Ddepth, 1e-7)); +} + /* ************************************************************************* */ static double range0(const Camera& camera, const Point3& point) { return camera.range(point); diff --git a/python/handwritten/geometry/PinholeBaseK.cpp b/python/handwritten/geometry/PinholeBaseK.cpp index da98783e2..4153ecf06 100644 --- a/python/handwritten/geometry/PinholeBaseK.cpp +++ b/python/handwritten/geometry/PinholeBaseK.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -36,31 +36,38 @@ struct PinholeBaseKCal3_S2Callback : PinholeBaseKCal3_S2, wrapper Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; -Point2 (PinholeBaseKCal3_S2::*project3) (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; +// Function pointers to disambiguate project() calls +Point2 (PinholeBaseKCal3_S2::*project1) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; +Point2 (PinholeBaseKCal3_S2::*project2) (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension::value > Dcal) const = &PinholeBaseKCal3_S2::project; -// function pointers to desambiguate range() calls +// function pointers to disambiguate range() calls double (PinholeBaseKCal3_S2::*range1) (const Point3 &point, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 3 > Dpoint) const = &PinholeBaseKCal3_S2::range; double (PinholeBaseKCal3_S2::*range2) (const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dpose) const = &PinholeBaseKCal3_S2::range; double (PinholeBaseKCal3_S2::*range3) (const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dother) const = &PinholeBaseKCal3_S2::range; -void exportPinholeBaseK(){ +// wrap projectSafe in a function that returns None or a tuple +// TODO(frank): find out how to return an ndarray instead +object project_safe(const PinholeBaseKCal3_S2& camera, const gtsam::Point3& p) { + auto result = camera.projectSafe(p); + if (result.second) + return make_tuple(result.first.x(), result.first.y()); + else + return object(); +} +void exportPinholeBaseK() { class_("PinholeBaseKCal3_S2", no_init) - .def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), return_value_policy()) - .def("project", project1) - .def("project", project2, project_overloads()) - .def("project", project3, project_overloads()) - .def("backproject", &PinholeBaseKCal3_S2::backproject) - .def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity) - .def("range", range1, range_overloads()) - .def("range", range2, range_overloads()) - .def("range", range3, range_overloads()) - ; - -} \ No newline at end of file + .def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), + return_value_policy()) + .def("project_safe", make_function(project_safe)) + .def("project", project1, project_overloads()) + .def("project", project2, project_overloads()) + .def("backproject", &PinholeBaseKCal3_S2::backproject) + .def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity) + .def("range", range1, range_overloads()) + .def("range", range2, range_overloads()) + .def("range", range3, range_overloads()); +}