diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h index 49d1f7cace..285b649001 100644 --- a/gtsam/slam/PlanarProjectionFactor.h +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -1,24 +1,13 @@ /** - * Similar to GenericProjectionFactor, but: - * - * * 2d pose variable (robot on the floor) - * * constant landmarks - * * batched input - * * numeric differentiation + * ProjectionFactor, but with pose2 (robot on the floor) * * This factor is useful for high-school robotics competitions, * which run robots on the floor, with use fixed maps and fiducial - * markers. The camera offset and calibration are fixed, perhaps - * found using PlanarSFMFactor. - * - * The python version of this factor uses batches, to save on calls - * across the C++/python boundary, but here the only extra cost - * is instantiating the camera, so there's no batch. + * markers. * * @see https://www.firstinspires.org/ - * @see PlanarSFMFactor.h * - * @file PlanarSFMFactor.h + * @file PlanarProjectionFactor.h * @brief for planar smoothing * @date Dec 2, 2024 * @author joel@truher.org @@ -39,116 +28,291 @@ namespace gtsam { + /** - * @class PlanarProjectionFactor - * @brief Camera projection for robot on the floor. + * @class PlanarProjectionFactorBase + * @brief Camera projection for robot on the floor. Measurement is camera pixels. */ - class PlanarProjectionFactor : public NoiseModelFactorN { - typedef NoiseModelFactorN Base; + class PlanarProjectionFactorBase { + protected: + PlanarProjectionFactorBase() {} + + /** + * @param measured corresponding point in the camera frame + * @param poseKey index of the robot pose in the z=0 plane + */ + PlanarProjectionFactorBase(const Point2& measured) : measured_(measured) {} + + /** + * @param landmark point3 + * @param pose + * @param offset oriented parallel to pose2 zero i.e. +x + * @param calib + * @param H1 landmark jacobian + * @param H2 pose jacobian + * @param H3 offset jacobian + * @param H4 calib jacobian + */ + Point2 h( + const Point3& landmark, + const Pose2& pose, + const Pose3& offset, + const Cal3DS2& calib, + OptionalMatrixType H1, // 2x3 (x, y, z) + OptionalMatrixType H2, // 2x3 (x, y, theta) + OptionalMatrixType H3, // 2x6 (rx, ry, rz, x, y, theta) + OptionalMatrixType H4 // 2x9 + ) const { + try { + // this is x-forward z-up + gtsam::Matrix H0; // 6x6 + Pose3 offset_pose = Pose3(pose).compose(offset, H0); + // this is z-forward y-down + gtsam::Matrix H00; // 6x6 + Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00); + PinholeCamera camera = PinholeCamera(camera_pose, calib); + if (H2 || H3) { + // Dpose is for pose3, 2x6 (R,t) + gtsam::Matrix Dpose; + Point2 result = camera.project(landmark, Dpose, H1, H4); + gtsam::Matrix DposeOffset = Dpose * H00; // 2x6 + if (H3) + *H3 = DposeOffset; // with Eigen this is a deep copy (!) + if (H2) { + gtsam::Matrix DposeOffsetFwd = DposeOffset * H0; + *H2 = Matrix::Zero(2, 3); + (*H2)(0, 0) = DposeOffsetFwd(0, 3); // du/dx + (*H2)(1, 0) = DposeOffsetFwd(1, 3); // dv/dx + (*H2)(0, 1) = DposeOffsetFwd(0, 4); // du/dy + (*H2)(1, 1) = DposeOffsetFwd(1, 4); // dv/dy + (*H2)(0, 2) = DposeOffsetFwd(0, 2); // du/dyaw + (*H2)(1, 2) = DposeOffsetFwd(1, 2); // dv/dyaw + } + return result; + } else { + return camera.project(landmark, {}, {}, {}); + } + } catch (CheiralityException& e) { + std::cout << "****** CHIRALITY EXCEPTION ******\n"; + if (H1) *H1 = Matrix::Zero(2, 3); + if (H2) *H2 = Matrix::Zero(2, 3); + if (H3) *H3 = Matrix::Zero(2, 6); + if (H4) *H4 = Matrix::Zero(2, 9); + // return a large error + return Matrix::Constant(2, 1, 2.0 * calib.fx()); + } + } + + Point2 measured_; // pixel measurement + + private: static const Pose3 CAM_COORD; + }; - protected: + // camera "zero" is facing +z; this turns it to face +x + const Pose3 PlanarProjectionFactorBase::CAM_COORD = Pose3( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); + + /** + * @class PlanarProjectionFactor1 + * @brief One variable: the pose. + * Landmark, camera offset, camera calibration are constant. + * This is intended for localization within a known map. + */ + class PlanarProjectionFactor1 + : public PlanarProjectionFactorBase, public NoiseModelFactorN { + public: + typedef NoiseModelFactorN Base; + using Base::evaluateError; + PlanarProjectionFactor1() {} + + ~PlanarProjectionFactor1() override {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this))); + } + + + /** + * @param landmark point3 in the world + * @param measured corresponding point2 in the camera frame + * @param offset constant camera offset from pose + * @param calib constant camera calibration + * @param model stddev of the measurements, ~one pixel? + * @param poseKey index of the robot pose2 in the z=0 plane + */ + PlanarProjectionFactor1( + const Point3& landmark, + const Point2& measured, + const Pose3& offset, + const Cal3DS2& calib, + const SharedNoiseModel& model, + Key poseKey) + : PlanarProjectionFactorBase(measured), + NoiseModelFactorN(model, poseKey), + landmark_(landmark), + offset_(offset), + calib_(calib) { + assert(2 == model->dim()); + } + + /** + * @param pose estimated pose2 + * @param H1 pose jacobian + */ + Vector evaluateError( + const Pose2& pose, + OptionalMatrixType H1 = OptionalNone) const override { + return h(landmark_, pose, offset_, calib_, {}, H1, {}, {}) - measured_; + } + private: Point3 landmark_; // landmark - Point2 measured_; // pixel measurement Pose3 offset_; // camera offset to robot pose Cal3DS2 calib_; // camera calibration + }; + + template<> + struct traits : + public Testable {}; + /** + * @class PlanarProjectionFactor2 + * @brief Two unknowns: the pose and the landmark. + * Camera offset and calibration are constant. + * This is similar to GeneralSFMFactor, used for SLAM. + */ + class PlanarProjectionFactor2 : public PlanarProjectionFactorBase, public NoiseModelFactorN { public: - // Provide access to the Matrix& version of evaluateError: + typedef NoiseModelFactorN Base; using Base::evaluateError; - PlanarProjectionFactor() {} + PlanarProjectionFactor2() {} + + ~PlanarProjectionFactor2() override {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this))); + } /** - * @param landmarks point in the world * @param measured corresponding point in the camera frame * @param offset constant camera offset from pose * @param calib constant camera calibration * @param model stddev of the measurements, ~one pixel? - * @param poseKey index of the robot pose in the z=0 plane + * @param poseKey index of the robot pose2 in the z=0 plane + * @param landmarkKey index of the landmark point3 */ - PlanarProjectionFactor( - const Point3& landmark, + PlanarProjectionFactor2( const Point2& measured, const Pose3& offset, const Cal3DS2& calib, const SharedNoiseModel& model, + Key landmarkKey, Key poseKey) - : NoiseModelFactorN(model, poseKey), - landmark_(landmark), - measured_(measured), + : PlanarProjectionFactorBase(measured), + NoiseModelFactorN(model, landmarkKey, poseKey), offset_(offset), - calib_(calib) - { + calib_(calib) { assert(2 == model->dim()); } - ~PlanarProjectionFactor() override {} + /** + * @param landmark estimated landmark + * @param pose estimated pose2 + * @param H1 landmark jacobian + * @param H2 pose jacobian + */ + Vector evaluateError( + const Point3& landmark, + const Pose2& pose, + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const override { + return h(landmark, pose, offset_, calib_, H1, H2, {}, {}) - measured_; + } + + private: + Pose3 offset_; // camera offset to robot pose + Cal3DS2 calib_; // camera calibration + }; + + template<> + struct traits : + public Testable {}; + + /** + * @class PlanarProjectionFactor3 + * @brief Three unknowns: the pose, the camera offset, and the camera calibration. + * Landmark is constant. + * This is intended to be used for camera calibration. + */ + class PlanarProjectionFactor3 : public PlanarProjectionFactorBase, public NoiseModelFactorN { + public: + typedef NoiseModelFactorN Base; + using Base::evaluateError; + + PlanarProjectionFactor3() {} + + ~PlanarProjectionFactor3() override {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor(*this))); + gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor3(*this))); } - Point2 h2(const Pose2& pose, OptionalMatrixType H1) const { - // this is x-forward z-up - gtsam::Matrix H0; - Pose3 offset_pose = Pose3(pose).compose(offset_, H0); - // this is z-forward y-down - gtsam::Matrix H00; - Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00); - PinholeCamera camera = PinholeCamera(camera_pose, calib_); - if (H1) { - // Dpose is 2x6 (R,t) - // H1 is 2x3 (x, y, theta) - gtsam::Matrix Dpose; - Point2 result = camera.project(landmark_, Dpose); - Dpose = Dpose * H00 * H0; - *H1 = Matrix::Zero(2, 3); - (*H1)(0, 0) = Dpose(0, 3); // du/dx - (*H1)(1, 0) = Dpose(1, 3); // dv/dx - (*H1)(0, 1) = Dpose(0, 4); // du/dy - (*H1)(1, 1) = Dpose(1, 4); // dv/dy - (*H1)(0, 2) = Dpose(0, 2); // du/dyaw - (*H1)(1, 2) = Dpose(1, 2); // dv/dyaw - return result; - } - else { - return camera.project(landmark_, {}, {}, {}); - } + /** + * @param landmark point3 in the world + * @param measured corresponding point2 in the camera frame + * @param model stddev of the measurements, ~one pixel? + * @param poseKey index of the robot pose2 in the z=0 plane + * @param offsetKey index of camera offset from pose + * @param calibKey index of camera calibration */ + PlanarProjectionFactor3( + const Point3& landmark, + const Point2& measured, + const SharedNoiseModel& model, + Key poseKey, + Key offsetKey, + Key calibKey) + : PlanarProjectionFactorBase(measured), + NoiseModelFactorN(model, poseKey, offsetKey, calibKey), + landmark_(landmark) { + assert(2 == model->dim()); } + /** + * @param pose estimated pose2 + * @param offset pose3 offset from pose2 +x + * @param calib calibration + * @param H1 pose jacobian + * @param H2 offset jacobian + * @param H3 calibration jacobian + */ Vector evaluateError( const Pose2& pose, - OptionalMatrixType H1 = OptionalNone) const override { - try { - return h2(pose, H1) - measured_; - } catch (CheiralityException& e) { - std::cout << "****** CHIRALITY EXCEPTION ******\n"; - std::cout << "landmark " << landmark_ << "\n"; - std::cout << "pose " << pose << "\n"; - std::cout << "offset " << offset_ << "\n"; - std::cout << "calib " << calib_ << "\n"; - if (H1) *H1 = Matrix::Zero(2, 3); - // return a large error - return Matrix::Constant(2, 1, 2.0 * calib_.fx()); - } + const Pose3& offset, + const Cal3DS2& calib, + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone, + OptionalMatrixType H3 = OptionalNone) const override { + return h(landmark_, pose, offset, calib, {}, H1, H2, H3) - measured_; } - }; - // camera "zero" is facing +z; this turns it to face +x - const Pose3 PlanarProjectionFactor::CAM_COORD = Pose3( - Rot3(0, 0, 1,// - -1, 0, 0, // - 0, -1, 0), - Vector3(0, 0, 0) - ); + private: + Point3 landmark_; // landmark + }; template<> - struct traits : - public Testable { - }; + struct traits : + public Testable {}; -} // namespace gtsam +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/slam/PlanarSFMFactor.h b/gtsam/slam/PlanarSFMFactor.h deleted file mode 100644 index bdcbb6ee24..0000000000 --- a/gtsam/slam/PlanarSFMFactor.h +++ /dev/null @@ -1,165 +0,0 @@ -/** - * Similar to GeneralSFMFactor, but: - * - * * 2d pose variable (robot on the floor) - * * camera offset variable - * * constant landmarks - * * batched input - * * numeric differentiation - * - * This factor is useful to find camera calibration and placement, in - * a sort of "autocalibrate" mode. Once a satisfactory solution is - * found, the PlanarProjectionFactor should be used for localization. - * - * The python version of this factor uses batches, to save on calls - * across the C++/python boundary, but here the only extra cost - * is instantiating the camera, so there's no batch. - * - * @see https://www.firstinspires.org/ - * @see PlanarProjectionFactor.h - * - * @file PlanarSFMFactor.h - * @brief for planar smoothing with unknown calibration - * @date Dec 2, 2024 - * @author joel@truher.org - */ -#pragma once - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - - -namespace gtsam { - /** - * @class PlanarSFMFactor - * @brief Camera calibration for robot on the floor. - */ - class PlanarSFMFactor : public NoiseModelFactorN { - private: - typedef NoiseModelFactorN Base; - static const Pose3 CAM_COORD; - - protected: - - Point3 landmark_; // landmark - Point2 measured_; // pixel measurement - - public: - // Provide access to the Matrix& version of evaluateError: - using Base::evaluateError; - - PlanarSFMFactor() {} - /** - * @param landmarks point in the world - * @param measured corresponding point in the camera frame - * @param model stddev of the measurements, ~one pixel? - * @param poseKey index of the robot pose2 in the z=0 plane - * @param offsetKey index of the 3d camera offset from the robot pose - * @param calibKey index of the camera calibration - */ - PlanarSFMFactor( - const Point3& landmark, - const Point2& measured, - const SharedNoiseModel& model, - Key poseKey, - Key offsetKey, - Key calibKey) - : NoiseModelFactorN(model, poseKey, offsetKey, calibKey), - landmark_(landmark), measured_(measured) - { - assert(2 == model->dim()); - } - - ~PlanarSFMFactor() override {} - - /// @return a deep copy of this factor - gtsam::NonlinearFactor::shared_ptr clone() const override { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new PlanarSFMFactor(*this))); - } - - Point2 h2(const Pose2& pose, - const Pose3& offset, - const Cal3DS2& calib, - OptionalMatrixType H1, - OptionalMatrixType H2, - OptionalMatrixType H3 - ) const { - // this is x-forward z-up - gtsam::Matrix H0; - Pose3 offset_pose = Pose3(pose).compose(offset, H0); - // this is z-forward y-down - gtsam::Matrix H00; - Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00); - PinholeCamera camera = PinholeCamera(camera_pose, calib); - if (H1 || H2) { - gtsam::Matrix Dpose; - Point2 result = camera.project(landmark_, Dpose, {}, H3); - gtsam::Matrix DposeOffset = Dpose * H00; - if (H2) - *H2 = DposeOffset; // a deep copy - if (H1) { - gtsam::Matrix DposeOffsetFwd = DposeOffset * H0; - *H1 = Matrix::Zero(2,3); - (*H1)(0,0) = DposeOffsetFwd(0,3); // du/dx - (*H1)(1,0) = DposeOffsetFwd(1,3); // dv/dx - (*H1)(0,1) = DposeOffsetFwd(0,4); // du/dy - (*H1)(1,1) = DposeOffsetFwd(1,4); // dv/dy - (*H1)(0,2) = DposeOffsetFwd(0,2); // du/dyaw - (*H1)(1,2) = DposeOffsetFwd(1,2); // dv/dyaw - } - return result; - } else { - return camera.project(landmark_, {}, {}, {}); - } - } - - Vector evaluateError( - const Pose2& pose, - const Pose3& offset, - const Cal3DS2& calib, - OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone, - OptionalMatrixType H3 = OptionalNone - ) const override { - try { - return h2(pose,offset,calib,H1,H2,H3) - measured_; - } - catch (CheiralityException& e) { - std::cout << "****** CHIRALITY EXCEPTION ******\n"; - std::cout << "landmark " << landmark_ << "\n"; - std::cout << "pose " << pose << "\n"; - std::cout << "offset " << offset << "\n"; - std::cout << "calib " << calib << "\n"; - if (H1) *H1 = Matrix::Zero(2, 3); - if (H2) *H2 = Matrix::Zero(2, 6); - if (H3) *H3 = Matrix::Zero(2, 9); - // return a large error - return Matrix::Constant(2, 1, 2.0 * calib.fx()); - } - } - }; - - // camera "zero" is facing +z; this turns it to face +x - const Pose3 PlanarSFMFactor::CAM_COORD = Pose3( - Rot3(0, 0, 1,// - -1, 0, 0, // - 0, -1, 0), - Vector3(0, 0, 0) - ); - - template<> - struct traits : - public Testable { - }; - -} // namespace gtsam diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index d69b9890e8..992c9aa6fd 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -25,8 +25,8 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { }; #include -virtual class PlanarProjectionFactor : gtsam::NoiseModelFactor { - PlanarProjectionFactor( +virtual class PlanarProjectionFactor1 : gtsam::NoiseModelFactor { + PlanarProjectionFactor1( const gtsam::Point3& landmark, const gtsam::Point2& measured, const gtsam::Pose3& offset, @@ -35,6 +35,26 @@ virtual class PlanarProjectionFactor : gtsam::NoiseModelFactor { size_t poseKey); void serialize() const; }; +virtual class PlanarProjectionFactor2 : gtsam::NoiseModelFactor { + PlanarProjectionFactor2( + const gtsam::Point2& measured, + const gtsam::Pose3& offset, + const gtsam::Cal3DS2& calib, + const gtsam::noiseModel::Base* model, + size_t landmarkKey, + size_t poseKey); + void serialize() const; +}; +virtual class PlanarProjectionFactor3 : gtsam::NoiseModelFactor { + PlanarProjectionFactor3( + const gtsam::Point3& landmark, + const gtsam::Point2& measured, + const gtsam::noiseModel::Base* model, + size_t poseKey, + size_t offsetKey, + size_t calibKey)); + void serialize() const; +}; #include template @@ -79,18 +99,6 @@ typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3Unified; -#include -virtual class PlanarSFMFactor : gtsam::NoiseModelFactor { - PlanarSFMFactor( - const gtsam::Point3& landmark, - const gtsam::Point2& measured, - const gtsam::noiseModel::Base* model, - size_t poseKey, - size_t offsetKey, - size_t calibKey); - void serialize() const; -}; - #include template virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp index eafe2041fe..21961cdad0 100644 --- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -24,8 +24,10 @@ using namespace std; using namespace gtsam; using symbol_shorthand::X; +using symbol_shorthand::C; +using symbol_shorthand::K; -TEST(PlanarProjectionFactor, error1) { +TEST(PlanarProjectionFactor1, error1) { // landmark is on the camera bore (which faces +x) Point3 landmark(1, 0, 0); // so pixel measurement is (cx, cy) @@ -37,7 +39,7 @@ TEST(PlanarProjectionFactor, error1) { Pose2 pose(0, 0, 0); values.insert(X(0), pose); - PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); CHECK_EQUAL(2, factor.dim()); CHECK(factor.active(values)); @@ -55,7 +57,7 @@ TEST(PlanarProjectionFactor, error1) { CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); } -TEST(PlanarProjectionFactor, error2) { +TEST(PlanarProjectionFactor1, error2) { // landmark is in the upper left corner Point3 landmark(1, 1, 1); // upper left corner in pixels @@ -63,7 +65,7 @@ TEST(PlanarProjectionFactor, error2) { Pose3 offset; Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); Values values; Pose2 pose(0, 0, 0); @@ -85,7 +87,7 @@ TEST(PlanarProjectionFactor, error2) { CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); } -TEST(PlanarProjectionFactor, error3) { +TEST(PlanarProjectionFactor1, error3) { // landmark is in the upper left corner Point3 landmark(1, 1, 1); // upper left corner in pixels @@ -94,7 +96,7 @@ TEST(PlanarProjectionFactor, error3) { // distortion Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); Values values; Pose2 pose(0, 0, 0); @@ -116,7 +118,7 @@ TEST(PlanarProjectionFactor, error3) { CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); } -TEST(PlanarProjectionFactor, jacobian) { +TEST(PlanarProjectionFactor1, jacobian) { // test many jacobians with many randoms std::default_random_engine g; @@ -129,7 +131,7 @@ TEST(PlanarProjectionFactor, jacobian) { Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); - PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); Pose2 pose(s(g), s(g), s(g)); @@ -143,8 +145,213 @@ TEST(PlanarProjectionFactor, jacobian) { pose); CHECK(assert_equal(expectedH1, H1, 1e-6)); } +} + + + +TEST(PlanarProjectionFactor3, error1) { + // landmark is on the camera bore (facing +x) + Point3 landmark(1, 0, 0); + // so px is (cx, cy) + Point2 measured(200, 200); + // offset is identity + Pose3 offset; + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + Values values; + Pose2 pose(0, 0, 0); + values.insert(X(0), pose); + values.insert(C(0), offset); + values.insert(K(0), calib); + + PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(3); + + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + const Matrix& H2Actual = actualHs.at(1); + const Matrix& H3Actual = actualHs.at(2); + + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + CHECK_EQUAL(2, H2Actual.rows()); + CHECK_EQUAL(6, H2Actual.cols()); + CHECK_EQUAL(2, H3Actual.rows()); + CHECK_EQUAL(9, H3Actual.cols()); + + // du/dx etc for the pose2d + Matrix23 H1Expected = (Matrix23() <dim()); + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(3); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + const Matrix& H2Actual = actualHs.at(1); + const Matrix& H3Actual = actualHs.at(2); + + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + CHECK_EQUAL(2, H2Actual.rows()); + CHECK_EQUAL(6, H2Actual.cols()); + CHECK_EQUAL(2, H3Actual.rows()); + CHECK_EQUAL(9, H3Actual.cols()); + + Matrix23 H1Expected = (Matrix23() <dim()); + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(3); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + const Matrix& H2Actual = actualHs.at(1); + const Matrix& H3Actual = actualHs.at(2); + + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + CHECK_EQUAL(2, H2Actual.rows()); + CHECK_EQUAL(6, H2Actual.cols()); + CHECK_EQUAL(2, H3Actual.rows()); + CHECK_EQUAL(9, H3Actual.cols()); + + Matrix23 H1Expected = (Matrix23() < s(-0.3, 0.3); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + + for (int i = 0; i < 1000; ++i) { + Point3 landmark(2 + s(g), s(g), s(g)); + Point2 measured(200 + 100*s(g), 200 + 100*s(g)); + Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); + Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); + + PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + + Pose2 pose(s(g), s(g), s(g)); + + // actual H + Matrix H1, H2, H3; + factor.evaluateError(pose, offset, calib, H1, H2, H3); + + Matrix expectedH1 = numericalDerivative31( + [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { + return factor.evaluateError(p, o, c);}, + pose, offset, calib); + Matrix expectedH2 = numericalDerivative32( + [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { + return factor.evaluateError(p, o, c);}, + pose, offset, calib); + Matrix expectedH3 = numericalDerivative33( + [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { + return factor.evaluateError(p, o, c);}, + pose, offset, calib); + CHECK(assert_equal(expectedH1, H1, 1e-6)); + CHECK(assert_equal(expectedH2, H2, 1e-6)); + CHECK(assert_equal(expectedH3, H3, 1e-6)); + } } /* ************************************************************************* */ @@ -153,4 +360,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/slam/tests/testPlanarSFMFactor.cpp b/gtsam/slam/tests/testPlanarSFMFactor.cpp deleted file mode 100644 index 22ec675612..0000000000 --- a/gtsam/slam/tests/testPlanarSFMFactor.cpp +++ /dev/null @@ -1,243 +0,0 @@ -/** - * @file testPlanarSFMFactor.cpp - * @date Dec 3, 2024 - * @author joel@truher.org - * @brief unit tests for PlanarSFMFactor - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -using namespace std; -using namespace gtsam; -using symbol_shorthand::X; -using symbol_shorthand::C; -using symbol_shorthand::K; - -TEST(PlanarSFMFactor, error1) { - // landmark is on the camera bore (facing +x) - Point3 landmark(1, 0, 0); - // so px is (cx, cy) - Point2 measured(200, 200); - // offset is identity - Pose3 offset; - Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); - SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - Values values; - Pose2 pose(0, 0, 0); - values.insert(X(0), pose); - values.insert(C(0), offset); - values.insert(K(0), calib); - - PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0)); - - CHECK_EQUAL(2, factor.dim()); - CHECK(factor.active(values)); - std::vector actualHs(3); - - gtsam::Vector actual = factor.unwhitenedError(values, actualHs); - CHECK(assert_equal(Vector2(0, 0), actual)); - - const Matrix& H1Actual = actualHs.at(0); - const Matrix& H2Actual = actualHs.at(1); - const Matrix& H3Actual = actualHs.at(2); - - CHECK_EQUAL(2, H1Actual.rows()); - CHECK_EQUAL(3, H1Actual.cols()); - CHECK_EQUAL(2, H2Actual.rows()); - CHECK_EQUAL(6, H2Actual.cols()); - CHECK_EQUAL(2, H3Actual.rows()); - CHECK_EQUAL(9, H3Actual.cols()); - - // du/dx etc for the pose2d - Matrix23 H1Expected = (Matrix23() <dim()); - CHECK_EQUAL(2, factor.dim()); - CHECK(factor.active(values)); - std::vector actualHs(3); - gtsam::Vector actual = factor.unwhitenedError(values, actualHs); - - CHECK(assert_equal(Vector2(0, 0), actual)); - - const Matrix& H1Actual = actualHs.at(0); - const Matrix& H2Actual = actualHs.at(1); - const Matrix& H3Actual = actualHs.at(2); - - CHECK_EQUAL(2, H1Actual.rows()); - CHECK_EQUAL(3, H1Actual.cols()); - CHECK_EQUAL(2, H2Actual.rows()); - CHECK_EQUAL(6, H2Actual.cols()); - CHECK_EQUAL(2, H3Actual.rows()); - CHECK_EQUAL(9, H3Actual.cols()); - - Matrix23 H1Expected = (Matrix23() <dim()); - CHECK_EQUAL(2, factor.dim()); - CHECK(factor.active(values)); - std::vector actualHs(3); - gtsam::Vector actual = factor.unwhitenedError(values, actualHs); - - CHECK(assert_equal(Vector2(0, 0), actual)); - - const Matrix& H1Actual = actualHs.at(0); - const Matrix& H2Actual = actualHs.at(1); - const Matrix& H3Actual = actualHs.at(2); - - CHECK_EQUAL(2, H1Actual.rows()); - CHECK_EQUAL(3, H1Actual.cols()); - CHECK_EQUAL(2, H2Actual.rows()); - CHECK_EQUAL(6, H2Actual.cols()); - CHECK_EQUAL(2, H3Actual.rows()); - CHECK_EQUAL(9, H3Actual.cols()); - - Matrix23 H1Expected = (Matrix23() < s(-0.3, 0.3); - SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - - for (int i = 0; i < 1000; ++i) { - Point3 landmark(2 + s(g), s(g), s(g)); - Point2 measured(200 + 100*s(g), 200 + 100*s(g)); - Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g))); - Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); - - PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0)); - - Pose2 pose(s(g), s(g), s(g)); - - // actual H - Matrix H1, H2, H3; - factor.evaluateError(pose, offset, calib, H1, H2, H3); - - Matrix expectedH1 = numericalDerivative31( - [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { - return factor.evaluateError(p, o, c);}, - pose, offset, calib); - Matrix expectedH2 = numericalDerivative32( - [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { - return factor.evaluateError(p, o, c);}, - pose, offset, calib); - Matrix expectedH3 = numericalDerivative33( - [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { - return factor.evaluateError(p, o, c);}, - pose, offset, calib); - CHECK(assert_equal(expectedH1, H1, 1e-6)); - CHECK(assert_equal(expectedH2, H2, 1e-6)); - CHECK(assert_equal(expectedH3, H3, 1e-6)); - } -} - - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ -