From 06d45c416e2934e9c97ccf598df6820ffea7248a Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Sun, 15 Dec 2024 15:44:45 -0800 Subject: [PATCH] projection and SFM for 2d poses --- gtsam/slam/PlanarProjectionFactor.h | 154 +++++++++++ gtsam/slam/PlanarSFMFactor.h | 165 ++++++++++++ gtsam/slam/slam.i | 24 ++ .../slam/tests/testPlanarProjectionFactor.cpp | 156 +++++++++++ gtsam/slam/tests/testPlanarSFMFactor.cpp | 243 ++++++++++++++++++ 5 files changed, 742 insertions(+) create mode 100644 gtsam/slam/PlanarProjectionFactor.h create mode 100644 gtsam/slam/PlanarSFMFactor.h create mode 100644 gtsam/slam/tests/testPlanarProjectionFactor.cpp create mode 100644 gtsam/slam/tests/testPlanarSFMFactor.cpp diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h new file mode 100644 index 000000000..49d1f7cac --- /dev/null +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -0,0 +1,154 @@ +/** + * Similar to GenericProjectionFactor, but: + * + * * 2d pose variable (robot on the floor) + * * constant landmarks + * * batched input + * * numeric differentiation + * + * 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. + * + * @see https://www.firstinspires.org/ + * @see PlanarSFMFactor.h + * + * @file PlanarSFMFactor.h + * @brief for planar smoothing + * @date Dec 2, 2024 + * @author joel@truher.org + */ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace gtsam { + /** + * @class PlanarProjectionFactor + * @brief Camera projection for robot on the floor. + */ + class PlanarProjectionFactor : public NoiseModelFactorN { + typedef NoiseModelFactorN Base; + static const Pose3 CAM_COORD; + + protected: + + Point3 landmark_; // landmark + Point2 measured_; // pixel measurement + Pose3 offset_; // camera offset to robot pose + Cal3DS2 calib_; // camera calibration + + public: + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + PlanarProjectionFactor() {} + + /** + * @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 + */ + PlanarProjectionFactor( + const Point3& landmark, + const Point2& measured, + const Pose3& offset, + const Cal3DS2& calib, + const SharedNoiseModel& model, + Key poseKey) + : NoiseModelFactorN(model, poseKey), + landmark_(landmark), + measured_(measured), + offset_(offset), + calib_(calib) + { + assert(2 == model->dim()); + } + + ~PlanarProjectionFactor() 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))); + } + + 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_, {}, {}, {}); + } + } + + 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()); + } + } + }; + + // 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) + ); + + template<> + struct traits : + public Testable { + }; + +} // namespace gtsam diff --git a/gtsam/slam/PlanarSFMFactor.h b/gtsam/slam/PlanarSFMFactor.h new file mode 100644 index 000000000..bdcbb6ee2 --- /dev/null +++ b/gtsam/slam/PlanarSFMFactor.h @@ -0,0 +1,165 @@ +/** + * 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 a226f06ec..d69b9890e 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -24,6 +24,18 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { void serialize() const; }; +#include +virtual class PlanarProjectionFactor : gtsam::NoiseModelFactor { + PlanarProjectionFactor( + const gtsam::Point3& landmark, + const gtsam::Point2& measured, + const gtsam::Pose3& offset, + const gtsam::Cal3DS2& calib, + const gtsam::noiseModel::Base* model, + size_t poseKey); + void serialize() const; +}; + #include template virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { @@ -67,6 +79,18 @@ 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 new file mode 100644 index 000000000..eafe2041f --- /dev/null +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -0,0 +1,156 @@ +/** + * @file testPlanarProjectionFactor.cpp + * @date Dec 3, 2024 + * @author joel@truher.org + * @brief unit tests for PlanarProjectionFactor + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::X; + +TEST(PlanarProjectionFactor, error1) { + // landmark is on the camera bore (which faces +x) + Point3 landmark(1, 0, 0); + // so pixel measurement is (cx, cy) + Point2 measured(200, 200); + 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); + + PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(1); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + const Matrix23 H1Expected = (Matrix23() << // + 0, 200, 200,// + 0, 0, 0).finished(); + CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); +} + +TEST(PlanarProjectionFactor, error2) { + // landmark is in the upper left corner + Point3 landmark(1, 1, 1); + // upper left corner in pixels + Point2 measured(0, 0); + 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)); + Values values; + Pose2 pose(0, 0, 0); + + values.insert(X(0), pose); + + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(1); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + Matrix23 H1Expected = (Matrix23() << // + -200, 200, 400, // + -200, 0, 200).finished(); + CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); +} + +TEST(PlanarProjectionFactor, error3) { + // landmark is in the upper left corner + Point3 landmark(1, 1, 1); + // upper left corner in pixels + Point2 measured(0, 0); + Pose3 offset; + // 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)); + Values values; + Pose2 pose(0, 0, 0); + + values.insert(X(0), pose); + + CHECK_EQUAL(2, factor.dim()); + CHECK(factor.active(values)); + std::vector actualHs(1); + gtsam::Vector actual = factor.unwhitenedError(values, actualHs); + + CHECK(assert_equal(Vector2(0, 0), actual)); + + const Matrix& H1Actual = actualHs.at(0); + CHECK_EQUAL(2, H1Actual.rows()); + CHECK_EQUAL(3, H1Actual.cols()); + Matrix23 H1Expected = (Matrix23() << // + -360, 280, 640, // + -360, 80, 440).finished(); + CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); +} + +TEST(PlanarProjectionFactor, jacobian) { + // test many jacobians with many randoms + + std::default_random_engine g; + std::uniform_real_distribution 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); + + PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0)); + + Pose2 pose(s(g), s(g), s(g)); + + // actual H + Matrix H1; + factor.evaluateError(pose, H1); + + Matrix expectedH1 = numericalDerivative11( + [&factor](const Pose2& p) { + return factor.evaluateError(p);}, + pose); + CHECK(assert_equal(expectedH1, H1, 1e-6)); + } + + +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/slam/tests/testPlanarSFMFactor.cpp b/gtsam/slam/tests/testPlanarSFMFactor.cpp new file mode 100644 index 000000000..22ec67561 --- /dev/null +++ b/gtsam/slam/tests/testPlanarSFMFactor.cpp @@ -0,0 +1,243 @@ +/** + * @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); +} +/* ************************************************************************* */ +