From e3906162e7604cb52f2fced9ed4bd2f57f17b942 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Sun, 15 Dec 2024 15:44:45 -0800 Subject: [PATCH 01/11] projection and SFM for 2d poses --- gtsam/slam/PlanarProjectionFactor.h | 174 +++++++++++++ gtsam/slam/PlanarSFMFactor.h | 195 ++++++++++++++ gtsam/slam/slam.i | 24 ++ .../slam/tests/testPlanarProjectionFactor.cpp | 156 +++++++++++ gtsam/slam/tests/testPlanarSFMFactor.cpp | 243 ++++++++++++++++++ 5 files changed, 792 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..9589c36d5 --- /dev/null +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -0,0 +1,174 @@ +/** + * 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); + // std::cout << "\nH0\n" << H0 << "\n"; + // this is z-forward y-down + gtsam::Matrix H00; + Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00); + // std::cout << "\nH00\n" << H00 << "\n"; + 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); + // std::cout << "\nDpose 1\n" << Dpose << "\n"; + Dpose = Dpose * H00 * H0; + // std::cout << "\nDpose 2\n" << Dpose << "\n"; + *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_, {}, {}, {}); + } + } + + Point2 h(const Pose2& pose) const { + // this is x-forward z-up + Pose3 offset_pose = Pose3(pose).compose(offset_); + // this is z-forward y-down + Pose3 camera_pose = offset_pose.compose(CAM_COORD); + PinholeCamera camera = PinholeCamera(camera_pose, calib_); + return camera.project2(landmark_); + } + + Vector evaluateError( + const Pose2& pose, + OptionalMatrixType H1 = OptionalNone) const override { + try { + return h2(pose, H1) - measured_; + + // Point2 result = h(pose) - measured_; + // if (H1) *H1 = numericalDerivative11( + // [&](const Pose2& p) {return h(p);}, + // pose); + // return result; + } + 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"; + // TODO: check the size here + 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..c39b40fef --- /dev/null +++ b/gtsam/slam/PlanarSFMFactor.h @@ -0,0 +1,195 @@ +/** + * 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 { + // std::cout << "POSE: " << pose << "\n"; + // std::cout << "OFFSET: " << offset << "\n"; + // std::cout << "CALIB: " << calib << "\n"; + // 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_, {}, {}, {}); + } + } + + Point2 h(const Pose2& pose, + const Pose3& offset, + const Cal3DS2& calib) const { + // std::cout << "POSE: " << pose << "\n"; + // std::cout << "OFFSET: " << offset << "\n"; + // std::cout << "CALIB: " << calib << "\n"; + // this is x-forward z-up + Pose3 offset_pose = Pose3(pose).compose(offset); + // this is z-forward y-down + Pose3 camera_pose = offset_pose.compose(CAM_COORD); + PinholeCamera camera = PinholeCamera(camera_pose, calib); + return camera.project2(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_; + + // Point2 result = h(pose, offset, calib) - measured_; + // if (H1) *H1 = numericalDerivative31( + // [&](const Pose2& p, const Pose3& o, const Cal3DS2& c) {return h(p, o, c);}, + // pose, offset, calib); + // if (H2) *H2 = numericalDerivative32( + // [&](const Pose2& p, const Pose3& o, const Cal3DS2& c) {return h(p, o, c);}, + // pose, offset, calib); + // if (H3) *H3 = numericalDerivative33( + // [&](const Pose2& p, const Pose3& o, const Cal3DS2& c) {return h(p, o, c);}, + // pose, offset, calib); + // return result; + } + 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"; + // TODO: what should these sizes be? + 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); +} +/* ************************************************************************* */ + From 06d45c416e2934e9c97ccf598df6820ffea7248a Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Sun, 15 Dec 2024 15:44:45 -0800 Subject: [PATCH 02/11] 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); +} +/* ************************************************************************* */ + From 14c79986d3416320e110f567b0def090b9ae9c1f Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Mon, 16 Dec 2024 18:04:40 -0800 Subject: [PATCH 03/11] consolidate factors in one file --- gtsam/slam/PlanarProjectionFactor.h | 360 +++++++++++++----- gtsam/slam/PlanarSFMFactor.h | 165 -------- gtsam/slam/slam.i | 36 +- .../slam/tests/testPlanarProjectionFactor.cpp | 224 ++++++++++- gtsam/slam/tests/testPlanarSFMFactor.cpp | 243 ------------ 5 files changed, 499 insertions(+), 529 deletions(-) delete mode 100644 gtsam/slam/PlanarSFMFactor.h delete mode 100644 gtsam/slam/tests/testPlanarSFMFactor.cpp diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h index 49d1f7cac..285b64900 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; - static const Pose3 CAM_COORD; - + class PlanarProjectionFactorBase { 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() {} + PlanarProjectionFactorBase() {} /** - * @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( + 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 Point2& measured, + const Pose2& pose, 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 { + 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 { - return h2(pose, H1) - measured_; + // 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"; - 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, 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()); + return Matrix::Constant(2, 1, 2.0 * calib.fx()); } } + + Point2 measured_; // pixel measurement + + private: + static const Pose3 CAM_COORD; }; // camera "zero" is facing +z; this turns it to face +x - const Pose3 PlanarProjectionFactor::CAM_COORD = Pose3( + const Pose3 PlanarProjectionFactorBase::CAM_COORD = Pose3( Rot3(0, 0, 1,// -1, 0, 0, // 0, -1, 0), Vector3(0, 0, 0) ); - template<> - struct traits : - public Testable { + /** + * @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 + Pose3 offset_; // camera offset to robot pose + Cal3DS2 calib_; // camera calibration }; -} // namespace gtsam + 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: + typedef NoiseModelFactorN Base; + using Base::evaluateError; + + 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 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 pose2 in the z=0 plane + * @param landmarkKey index of the landmark point3 + */ + PlanarProjectionFactor2( + const Point2& measured, + const Pose3& offset, + const Cal3DS2& calib, + const SharedNoiseModel& model, + Key landmarkKey, + Key poseKey) + : PlanarProjectionFactorBase(measured), + NoiseModelFactorN(model, landmarkKey, poseKey), + offset_(offset), + calib_(calib) { + assert(2 == model->dim()); + } + + /** + * @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 PlanarProjectionFactor3(*this))); + } + + /** + * @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, + 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_; + } + + private: + Point3 landmark_; // landmark + }; + + template<> + struct traits : + public Testable {}; + +} // 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 bdcbb6ee2..000000000 --- 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 d69b9890e..992c9aa6f 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 eafe2041f..21961cdad 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 22ec67561..000000000 --- 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); -} -/* ************************************************************************* */ - From ca958ccbc70eda61d5d56d101fa0101cac2e099c Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Mon, 16 Dec 2024 19:06:23 -0800 Subject: [PATCH 04/11] maybe fix clang? --- gtsam/slam/tests/testPlanarProjectionFactor.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp index 21961cdad..03a0b7bed 100644 --- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -141,7 +141,7 @@ TEST(PlanarProjectionFactor1, jacobian) { Matrix expectedH1 = numericalDerivative11( [&factor](const Pose2& p) { - return factor.evaluateError(p);}, + return factor.evaluateError(p, {});}, pose); CHECK(assert_equal(expectedH1, H1, 1e-6)); } @@ -338,15 +338,15 @@ TEST(PlanarProjectionFactor3, jacobian) { Matrix expectedH1 = numericalDerivative31( [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) { - return factor.evaluateError(p, o, 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);}, + 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);}, + return factor.evaluateError(p, o, c, {}, {}, {});}, pose, offset, calib); CHECK(assert_equal(expectedH1, H1, 1e-6)); CHECK(assert_equal(expectedH2, H2, 1e-6)); From 11f7eb642295d997012ca942695a5be7e1e79cb4 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Mon, 16 Dec 2024 19:47:42 -0800 Subject: [PATCH 05/11] fix typo, fix python CI? --- gtsam/slam/slam.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 992c9aa6f..e141cb50c 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -52,7 +52,7 @@ virtual class PlanarProjectionFactor3 : gtsam::NoiseModelFactor { const gtsam::noiseModel::Base* model, size_t poseKey, size_t offsetKey, - size_t calibKey)); + size_t calibKey); void serialize() const; }; From e4538d5b3e4ecddfabc8c8d81dc09553772f7147 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 17 Dec 2024 22:39:13 -0800 Subject: [PATCH 06/11] address some comments --- gtsam/geometry/Pose3.cpp | 11 ++ gtsam/geometry/Pose3.h | 2 + gtsam/slam/PlanarProjectionFactor.h | 168 +++++++++--------- gtsam/slam/slam.i | 36 ++-- .../slam/tests/testPlanarProjectionFactor.cpp | 24 ++- 5 files changed, 132 insertions(+), 109 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index bb1483432..5d7732947 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -42,6 +42,17 @@ Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR, return Pose3(R, t); } +Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) { + if (H) *H << (gtsam::Matrix(6, 3) << // + 0., 0., 0., // + 0., 0., 0.,// + 0., 0., 1.,// + 1., 0., 0.,// + 0., 1., 0.,// + 0., 0., 0.).finished(); + return Pose3(p); +} + /* ************************************************************************* */ Pose3 Pose3::inverse() const { Rot3 Rt = R_.inverse(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d7c280c80..baac21ccb 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -78,6 +78,8 @@ public: OptionalJacobian<6, 3> HR = {}, OptionalJacobian<6, 3> Ht = {}); + static Pose3 FromPose2(const Pose2& p, OptionalJacobian<6,3> H = {}); + /** * Create Pose3 by aligning two point pairs * A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h index 285b64900..21f24da93 100644 --- a/gtsam/slam/PlanarProjectionFactor.h +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -38,69 +38,67 @@ namespace gtsam { PlanarProjectionFactorBase() {} /** - * @param measured corresponding point in the camera frame - * @param poseKey index of the robot pose in the z=0 plane + * @param measured pixels in the camera frame */ 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 + * Predict the projection of the landmark in camera pixels. + * + * @param landmark point3 of the target + * @param wTb "world to body": planar pose2 of vehicle body frame in world frame + * @param bTc "body to camera": camera pose in body frame, oriented parallel to pose2 zero i.e. +x + * @param calib camera calibration with distortion + * @param Hlandmark jacobian + * @param HwTb jacobian + * @param HbTc jacobian + * @param Hcalib jacobian */ - Point2 h( + Point2 predict( const Point3& landmark, - const Pose2& pose, - const Pose3& offset, + const Pose2& wTb, + const Pose3& bTc, 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 + OptionalMatrixType Hlandmark = nullptr, // 2x3 (x, y, z) + OptionalMatrixType HwTb = nullptr, // 2x3 (x, y, theta) + OptionalMatrixType HbTc = nullptr, // 2x6 (rx, ry, rz, x, y, theta) + OptionalMatrixType Hcalib = nullptr // 2x9 ) const { +#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION try { - // this is x-forward z-up +#endif + gtsam::Matrix Hp; // 6x3 gtsam::Matrix H0; // 6x6 - Pose3 offset_pose = Pose3(pose).compose(offset, H0); + // this is x-forward z-up + Pose3 wTc = Pose3::FromPose2(wTb, Hp).compose(bTc, HwTb ? &H0 : nullptr); // this is z-forward y-down gtsam::Matrix H00; // 6x6 - Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00); + Pose3 camera_pose = wTc.compose(CAM_COORD, H00); PinholeCamera camera = PinholeCamera(camera_pose, calib); - if (H2 || H3) { + if (HwTb || HbTc) { // Dpose is for pose3, 2x6 (R,t) gtsam::Matrix Dpose; - Point2 result = camera.project(landmark, Dpose, H1, H4); + Point2 result = camera.project(landmark, Dpose, Hlandmark, Hcalib); 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 - } + if (HbTc) + *HbTc = DposeOffset; // with Eigen this is a deep copy (!) + if (HwTb) + *HwTb = DposeOffset * H0 * Hp; return result; } else { return camera.project(landmark, {}, {}, {}); } +#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION } 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); + if (Hlandmark) *Hlandmark = Matrix::Zero(2, 3); + if (HwTb) *HwTb = Matrix::Zero(2, 3); + if (HbTc) *HbTc = Matrix::Zero(2, 6); + if (Hcalib) *Hcalib = Matrix::Zero(2, 9); // return a large error return Matrix::Constant(2, 1, 2.0 * calib.fx()); } +#endif } Point2 measured_; // pixel measurement @@ -140,41 +138,42 @@ namespace gtsam { /** + * @brief constructor for known landmark, offset, and calibration + * @param poseKey index of the robot pose2 in the z=0 plane * @param landmark point3 in the world * @param measured corresponding point2 in the camera frame - * @param offset constant camera offset from pose + * @param bTc "body to camera": 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( + Key poseKey, const Point3& landmark, const Point2& measured, - const Pose3& offset, + const Pose3& bTc, const Cal3DS2& calib, - const SharedNoiseModel& model, - Key poseKey) + const SharedNoiseModel& model) : PlanarProjectionFactorBase(measured), NoiseModelFactorN(model, poseKey), landmark_(landmark), - offset_(offset), + bTc_(bTc), calib_(calib) { assert(2 == model->dim()); } /** - * @param pose estimated pose2 - * @param H1 pose jacobian + * @param wTb "world to body": estimated pose2 + * @param HwTb jacobian */ Vector evaluateError( - const Pose2& pose, - OptionalMatrixType H1 = OptionalNone) const override { - return h(landmark_, pose, offset_, calib_, {}, H1, {}, {}) - measured_; + const Pose2& wTb, + OptionalMatrixType HwTb = OptionalNone) const override { + return predict(landmark_, wTb, bTc_, calib_, {}, HwTb, {}, {}) - measured_; } private: Point3 landmark_; // landmark - Pose3 offset_; // camera offset to robot pose + Pose3 bTc_; // "body to camera": camera offset to robot pose Cal3DS2 calib_; // camera calibration }; @@ -188,9 +187,10 @@ namespace gtsam { * Camera offset and calibration are constant. * This is similar to GeneralSFMFactor, used for SLAM. */ - class PlanarProjectionFactor2 : public PlanarProjectionFactorBase, public NoiseModelFactorN { + class PlanarProjectionFactor2 + : public PlanarProjectionFactorBase, public NoiseModelFactorN { public: - typedef NoiseModelFactorN Base; + typedef NoiseModelFactorN Base; using Base::evaluateError; PlanarProjectionFactor2() {} @@ -204,43 +204,44 @@ namespace gtsam { } /** - * @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? + * @brief constructor for variable landmark, known offset and calibration * @param poseKey index of the robot pose2 in the z=0 plane * @param landmarkKey index of the landmark point3 + * @param measured corresponding point in the camera frame + * @param bTc "body to camera": constant camera offset from pose + * @param calib constant camera calibration + * @param model stddev of the measurements, ~one pixel? */ PlanarProjectionFactor2( - const Point2& measured, - const Pose3& offset, - const Cal3DS2& calib, - const SharedNoiseModel& model, + Key poseKey, Key landmarkKey, - Key poseKey) + const Point2& measured, + const Pose3& bTc, + const Cal3DS2& calib, + const SharedNoiseModel& model) : PlanarProjectionFactorBase(measured), NoiseModelFactorN(model, landmarkKey, poseKey), - offset_(offset), + bTc_(bTc), calib_(calib) { assert(2 == model->dim()); } /** + * @param wTb "world to body": estimated pose2 * @param landmark estimated landmark - * @param pose estimated pose2 - * @param H1 landmark jacobian - * @param H2 pose jacobian + * @param HwTb jacobian + * @param Hlandmark jacobian */ Vector evaluateError( + const Pose2& wTb, const Point3& landmark, - const Pose2& pose, - OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone) const override { - return h(landmark, pose, offset_, calib_, H1, H2, {}, {}) - measured_; + OptionalMatrixType HwTb = OptionalNone, + OptionalMatrixType Hlandmark = OptionalNone) const override { + return predict(landmark, wTb, bTc_, calib_, Hlandmark, HwTb, {}, {}) - measured_; } private: - Pose3 offset_; // camera offset to robot pose + Pose3 bTc_; // "body to camera": camera offset to robot pose Cal3DS2 calib_; // camera calibration }; @@ -270,6 +271,7 @@ namespace gtsam { } /** + * @brief constructor for variable pose, offset, and calibration, known landmark. * @param landmark point3 in the world * @param measured corresponding point2 in the camera frame * @param model stddev of the measurements, ~one pixel? @@ -277,12 +279,12 @@ namespace gtsam { * @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) + Key calibKey, + const Point3& landmark, + const Point2& measured, + const SharedNoiseModel& model) : PlanarProjectionFactorBase(measured), NoiseModelFactorN(model, poseKey, offsetKey, calibKey), landmark_(landmark) { @@ -290,21 +292,21 @@ namespace gtsam { } /** - * @param pose estimated pose2 - * @param offset pose3 offset from pose2 +x + * @param wTb "world to body": estimated pose2 + * @param bTc "body to camera": pose3 offset from pose2 +x * @param calib calibration - * @param H1 pose jacobian + * @param HwTb pose jacobian * @param H2 offset jacobian * @param H3 calibration jacobian */ Vector evaluateError( - const Pose2& pose, - const Pose3& offset, + const Pose2& wTb, + const Pose3& bTc, const Cal3DS2& calib, - OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone, - OptionalMatrixType H3 = OptionalNone) const override { - return h(landmark_, pose, offset, calib, {}, H1, H2, H3) - measured_; + OptionalMatrixType HwTb = OptionalNone, + OptionalMatrixType HbTc = OptionalNone, + OptionalMatrixType Hcalib = OptionalNone) const override { + return predict(landmark_, wTb, bTc, calib, {}, HwTb, HbTc, Hcalib) - measured_; } private: diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index e141cb50c..1761a6cc3 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -27,32 +27,32 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { #include virtual class PlanarProjectionFactor1 : gtsam::NoiseModelFactor { PlanarProjectionFactor1( - const gtsam::Point3& landmark, - const gtsam::Point2& measured, - const gtsam::Pose3& offset, - const gtsam::Cal3DS2& calib, - const gtsam::noiseModel::Base* model, - size_t poseKey); + size_t poseKey, + const gtsam::Point3& landmark, + const gtsam::Point2& measured, + const gtsam::Pose3& bTc, + const gtsam::Cal3DS2& calib, + const gtsam::noiseModel::Base* model); 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); + size_t poseKey, + size_t landmarkKey, + const gtsam::Point2& measured, + const gtsam::Pose3& bTc, + const gtsam::Cal3DS2& calib, + const gtsam::noiseModel::Base* model); 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); + size_t poseKey, + size_t offsetKey, + size_t calibKey, + const gtsam::Point3& landmark, + const gtsam::Point2& measured, + const gtsam::noiseModel::Base* model); void serialize() const; }; diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp index 03a0b7bed..67ebd5c25 100644 --- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -39,7 +39,8 @@ TEST(PlanarProjectionFactor1, error1) { Pose2 pose(0, 0, 0); values.insert(X(0), pose); - PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor( + X(0), landmark, measured, offset, calib, model); CHECK_EQUAL(2, factor.dim()); CHECK(factor.active(values)); @@ -65,7 +66,8 @@ TEST(PlanarProjectionFactor1, error2) { Pose3 offset; Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor( + X(0), landmark, measured, offset, calib, model); Values values; Pose2 pose(0, 0, 0); @@ -96,7 +98,8 @@ TEST(PlanarProjectionFactor1, error3) { // distortion Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor( + X(0), landmark, measured, offset, calib, model); Values values; Pose2 pose(0, 0, 0); @@ -131,7 +134,8 @@ TEST(PlanarProjectionFactor1, 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); - PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0)); + PlanarProjectionFactor1 factor( + X(0), landmark, measured, offset, calib, model); Pose2 pose(s(g), s(g), s(g)); @@ -164,7 +168,8 @@ TEST(PlanarProjectionFactor3, error1) { values.insert(C(0), offset); values.insert(K(0), calib); - PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + PlanarProjectionFactor3 factor( + X(0), C(0), K(0), landmark, measured, model); CHECK_EQUAL(2, factor.dim()); CHECK(factor.active(values)); @@ -213,7 +218,8 @@ TEST(PlanarProjectionFactor3, error2) { Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + PlanarProjectionFactor3 factor( + X(0), C(0), K(0), landmark, measured, model); Values values; Pose2 pose(0, 0, 0); @@ -267,7 +273,8 @@ TEST(PlanarProjectionFactor3, error3) { Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + PlanarProjectionFactor3 factor( + X(0), C(0), K(0), landmark, measured, model); Values values; Pose2 pose(0, 0, 0); @@ -328,7 +335,8 @@ TEST(PlanarProjectionFactor3, 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); - PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0)); + PlanarProjectionFactor3 factor( + X(0), C(0), K(0), landmark, measured, model); Pose2 pose(s(g), s(g), s(g)); From 0f8fe15e3118fd235ecddd10756a72410a0603bf Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 24 Dec 2024 09:06:25 -0800 Subject: [PATCH 07/11] remove "cam coord" idea --- gtsam/geometry/Pose3.cpp | 9 +- gtsam/geometry/Pose3.h | 3 +- gtsam/slam/PlanarProjectionFactor.h | 114 ++-- .../slam/tests/testPlanarProjectionFactor.cpp | 535 ++++++++++-------- 4 files changed, 355 insertions(+), 306 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 5d7732947..03464b0db 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -42,14 +42,17 @@ Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR, return Pose3(R, t); } -Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) { - if (H) *H << (gtsam::Matrix(6, 3) << // +// Pose2 constructor Jacobian is always the same. +static const Matrix63 Hpose2 = (Matrix63() << // 0., 0., 0., // 0., 0., 0.,// 0., 0., 1.,// 1., 0., 0.,// 0., 1., 0.,// 0., 0., 0.).finished(); + +Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) { + if (H) *H << Hpose2; return Pose3(p); } @@ -520,4 +523,4 @@ std::ostream &operator<<(std::ostream &os, const Pose3& pose) { return os; } -} // namespace gtsam +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index baac21ccb..ba00c1863 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -78,6 +78,7 @@ public: OptionalJacobian<6, 3> HR = {}, OptionalJacobian<6, 3> Ht = {}); + /** Construct from Pose2 in the xy plane, with derivative. */ static Pose3 FromPose2(const Pose2& p, OptionalJacobian<6,3> H = {}); /** @@ -460,4 +461,4 @@ struct Bearing : HasBearing {}; template struct Range : HasRange {}; -} // namespace gtsam +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h index 21f24da93..22cea2214 100644 --- a/gtsam/slam/PlanarProjectionFactor.h +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -59,31 +59,26 @@ namespace gtsam { const Pose2& wTb, const Pose3& bTc, const Cal3DS2& calib, - OptionalMatrixType Hlandmark = nullptr, // 2x3 (x, y, z) - OptionalMatrixType HwTb = nullptr, // 2x3 (x, y, theta) - OptionalMatrixType HbTc = nullptr, // 2x6 (rx, ry, rz, x, y, theta) - OptionalMatrixType Hcalib = nullptr // 2x9 + OptionalJacobian<2, 3> Hlandmark = {}, // (x, y, z) + OptionalJacobian<2, 3> HwTb = {}, // (x, y, theta) + OptionalJacobian<2, 6> HbTc = {}, // (rx, ry, rz, x, y, theta) + OptionalJacobian<2, 9> Hcalib = {} ) const { #ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION try { #endif - gtsam::Matrix Hp; // 6x3 - gtsam::Matrix H0; // 6x6 - // this is x-forward z-up - Pose3 wTc = Pose3::FromPose2(wTb, Hp).compose(bTc, HwTb ? &H0 : nullptr); - // this is z-forward y-down - gtsam::Matrix H00; // 6x6 - Pose3 camera_pose = wTc.compose(CAM_COORD, H00); - PinholeCamera camera = PinholeCamera(camera_pose, calib); + Matrix63 Hp; // 6x3 + Matrix66 H0; // 6x6 + Pose3 wTc = Pose3::FromPose2(wTb, HwTb ? &Hp : nullptr).compose(bTc, HwTb ? &H0 : nullptr); + PinholeCamera camera = PinholeCamera(wTc, calib); if (HwTb || HbTc) { - // Dpose is for pose3, 2x6 (R,t) - gtsam::Matrix Dpose; + // Dpose is for pose3 (R,t) + Matrix26 Dpose; Point2 result = camera.project(landmark, Dpose, Hlandmark, Hcalib); - gtsam::Matrix DposeOffset = Dpose * H00; // 2x6 if (HbTc) - *HbTc = DposeOffset; // with Eigen this is a deep copy (!) - if (HwTb) - *HwTb = DposeOffset * H0 * Hp; + *HbTc = Dpose; + if (HwTb) + *HwTb = Dpose * H0 * Hp; return result; } else { return camera.project(landmark, {}, {}, {}); @@ -91,10 +86,10 @@ namespace gtsam { #ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION } catch (CheiralityException& e) { std::cout << "****** CHIRALITY EXCEPTION ******\n"; - if (Hlandmark) *Hlandmark = Matrix::Zero(2, 3); - if (HwTb) *HwTb = Matrix::Zero(2, 3); - if (HbTc) *HbTc = Matrix::Zero(2, 6); - if (Hcalib) *Hcalib = Matrix::Zero(2, 9); + if (Hlandmark) Hlandmark->setZero(); + if (HwTb) HwTb->setZero(); + if (HbTc) HbTc->setZero(); + if (Hcalib) Hcalib->setZero(); // return a large error return Matrix::Constant(2, 1, 2.0 * calib.fx()); } @@ -102,18 +97,8 @@ namespace gtsam { } Point2 measured_; // pixel measurement - - private: - static const Pose3 CAM_COORD; }; - // 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 @@ -131,9 +116,9 @@ namespace gtsam { ~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))); + NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this))); } @@ -152,22 +137,18 @@ namespace gtsam { const Point2& measured, const Pose3& bTc, const Cal3DS2& calib, - const SharedNoiseModel& model) + const SharedNoiseModel& model = {}) : PlanarProjectionFactorBase(measured), NoiseModelFactorN(model, poseKey), landmark_(landmark), bTc_(bTc), - calib_(calib) { - assert(2 == model->dim()); - } + calib_(calib) {} /** * @param wTb "world to body": estimated pose2 * @param HwTb jacobian */ - Vector evaluateError( - const Pose2& wTb, - OptionalMatrixType HwTb = OptionalNone) const override { + Vector evaluateError(const Pose2& wTb, OptionalMatrixType HwTb) const override { return predict(landmark_, wTb, bTc_, calib_, {}, HwTb, {}, {}) - measured_; } @@ -187,8 +168,8 @@ namespace gtsam { * Camera offset and calibration are constant. * This is similar to GeneralSFMFactor, used for SLAM. */ - class PlanarProjectionFactor2 - : public PlanarProjectionFactorBase, public NoiseModelFactorN { + class PlanarProjectionFactor2 + : public PlanarProjectionFactorBase, public NoiseModelFactorN { public: typedef NoiseModelFactorN Base; using Base::evaluateError; @@ -198,9 +179,9 @@ namespace gtsam { ~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))); + NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this))); } /** @@ -218,13 +199,11 @@ namespace gtsam { const Point2& measured, const Pose3& bTc, const Cal3DS2& calib, - const SharedNoiseModel& model) + const SharedNoiseModel& model = {}) : PlanarProjectionFactorBase(measured), NoiseModelFactorN(model, landmarkKey, poseKey), bTc_(bTc), - calib_(calib) { - assert(2 == model->dim()); - } + calib_(calib) {} /** * @param wTb "world to body": estimated pose2 @@ -235,8 +214,8 @@ namespace gtsam { Vector evaluateError( const Pose2& wTb, const Point3& landmark, - OptionalMatrixType HwTb = OptionalNone, - OptionalMatrixType Hlandmark = OptionalNone) const override { + OptionalMatrixType HwTb, + OptionalMatrixType Hlandmark) const override { return predict(landmark, wTb, bTc_, calib_, Hlandmark, HwTb, {}, {}) - measured_; } @@ -265,47 +244,46 @@ namespace gtsam { ~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 PlanarProjectionFactor3(*this))); + NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + NonlinearFactor::shared_ptr(new PlanarProjectionFactor3(*this))); } /** * @brief constructor for variable pose, offset, and calibration, known landmark. + * @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 * @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( Key poseKey, Key offsetKey, Key calibKey, const Point3& landmark, const Point2& measured, - const SharedNoiseModel& model) + const SharedNoiseModel& model = {}) : PlanarProjectionFactorBase(measured), NoiseModelFactorN(model, poseKey, offsetKey, calibKey), - landmark_(landmark) { - assert(2 == model->dim()); - } + landmark_(landmark) {} /** * @param wTb "world to body": estimated pose2 * @param bTc "body to camera": pose3 offset from pose2 +x * @param calib calibration * @param HwTb pose jacobian - * @param H2 offset jacobian - * @param H3 calibration jacobian + * @param HbTc offset jacobian + * @param Hcalib calibration jacobian */ Vector evaluateError( const Pose2& wTb, const Pose3& bTc, const Cal3DS2& calib, - OptionalMatrixType HwTb = OptionalNone, - OptionalMatrixType HbTc = OptionalNone, - OptionalMatrixType Hcalib = OptionalNone) const override { + OptionalMatrixType HwTb, + OptionalMatrixType HbTc, + OptionalMatrixType Hcalib) const override { return predict(landmark_, wTb, bTc, calib, {}, HwTb, HbTc, Hcalib) - measured_; } diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp index 67ebd5c25..1cc002797 100644 --- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -16,6 +16,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -26,124 +30,101 @@ using namespace gtsam; using symbol_shorthand::X; using symbol_shorthand::C; using symbol_shorthand::K; +using symbol_shorthand::L; +/* ************************************************************************* */ TEST(PlanarProjectionFactor1, error1) { - // landmark is on the camera bore (which faces +x) + // Example: center projection and Jacobian Point3 landmark(1, 0, 0); - // so pixel measurement is (cx, cy) Point2 measured(200, 200); - Pose3 offset; + Pose3 offset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - Values values; + PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model); Pose2 pose(0, 0, 0); - values.insert(X(0), pose); - - PlanarProjectionFactor1 factor( - X(0), landmark, measured, offset, calib, model); - - 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)); + Matrix H; + CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6)); + CHECK(assert_equal((Matrix23() << // + 0, 200, 200, // + 0, 0, 0).finished(), H, 1e-6)); } +/* ************************************************************************* */ TEST(PlanarProjectionFactor1, error2) { - // landmark is in the upper left corner + // Example: upper left corner projection and Jacobian Point3 landmark(1, 1, 1); - // upper left corner in pixels Point2 measured(0, 0); - Pose3 offset; + Pose3 offset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor1 factor( - X(0), landmark, measured, offset, calib, model); - Values values; + PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model); 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() << // + Matrix H; + CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6)); + CHECK(assert_equal((Matrix23() << // -200, 200, 400, // - -200, 0, 200).finished(); - CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); + -200, 0, 200).finished(), H, 1e-6)); } +/* ************************************************************************* */ TEST(PlanarProjectionFactor1, error3) { - // landmark is in the upper left corner + // Example: upper left corner projection and Jacobian with distortion 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); + Pose3 offset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); + Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); // note distortion SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); - PlanarProjectionFactor1 factor( - X(0), landmark, measured, offset, calib, model); - Values values; + PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model); 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() << // + Matrix H; + CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6)); + CHECK(assert_equal((Matrix23() << // -360, 280, 640, // - -360, 80, 440).finished(); - CHECK(assert_equal(H1Expected, H1Actual, 1e-6)); + -360, 80, 440).finished(), H, 1e-6)); } +/* ************************************************************************* */ TEST(PlanarProjectionFactor1, jacobian) { - // test many jacobians with many randoms - - std::default_random_engine g; - std::uniform_real_distribution s(-0.3, 0.3); + // Verify Jacobians with numeric derivative + std::default_random_engine rng(42); + std::uniform_real_distribution dist(-0.3, 0.3); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + // center of the random camera poses + Pose3 centerOffset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); 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))); + Point3 landmark(2 + dist(rng), dist(rng), dist(rng)); + Point2 measured(200 + 100 * dist(rng), 200 + 100 * dist(rng)); + Pose3 offset = centerOffset.compose( + Pose3( + Rot3::Ypr(dist(rng), dist(rng), dist(rng)), + Point3(dist(rng), dist(rng), dist(rng)))); Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); - - PlanarProjectionFactor1 factor( - X(0), landmark, measured, offset, calib, model); - - Pose2 pose(s(g), s(g), s(g)); - - // actual H + PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model); + Pose2 pose(dist(rng), dist(rng), dist(rng)); Matrix H1; factor.evaluateError(pose, H1); - - Matrix expectedH1 = numericalDerivative11( + auto expectedH1 = numericalDerivative11( [&factor](const Pose2& p) { return factor.evaluateError(p, {});}, pose); @@ -151,194 +132,182 @@ TEST(PlanarProjectionFactor1, jacobian) { } } +/* ************************************************************************* */ +TEST(PlanarProjectionFactor1, solve) { + // Example localization + SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + // pose model is wide, so the solver finds the right answer. + SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(10, 10, 10)); -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; + // landmarks + Point3 l0(1, 0.1, 1); + Point3 l1(1, -0.1, 1); + + // camera pixels + Point2 p0(180, 0); + Point2 p1(220, 0); + + // body + Pose2 x0(0, 0, 0); + + // camera z looking at +x with (xy) antiparallel to (yz) + Pose3 c0( + Rot3(0, 0, 1, // + -1, 0, 0, // + 0, -1, 0), // + Vector3(0, 0, 0)); 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( - X(0), C(0), K(0), landmark, measured, model); + NonlinearFactorGraph graph; + graph.add(PlanarProjectionFactor1(X(0), l0, p0, c0, calib, pxModel)); + graph.add(PlanarProjectionFactor1(X(0), l1, p1, c0, calib, pxModel)); + graph.add(PriorFactor(X(0), x0, xNoise)); - CHECK_EQUAL(2, factor.dim()); - CHECK(factor.active(values)); - std::vector actualHs(3); + Values initialEstimate; + initialEstimate.insert(X(0), x0); - gtsam::Vector actual = factor.unwhitenedError(values, actualHs); - CHECK(assert_equal(Vector2(0, 0), actual)); + // run the optimizer + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); - const Matrix& H1Actual = actualHs.at(0); - const Matrix& H2Actual = actualHs.at(1); - const Matrix& H3Actual = actualHs.at(2); + // verify that the optimizer found the right pose. + CHECK(assert_equal(x0, result.at(X(0)), 2e-3)); - 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()); + // covariance + Marginals marginals(graph, result); + Matrix cov = marginals.marginalCovariance(X(0)); + CHECK(assert_equal((Matrix33() << // + 0.000012, 0.000000, 0.000000, // + 0.000000, 0.001287, -.001262, // + 0.000000, -.001262, 0.001250).finished(), cov, 3e-6)); - // 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); - + Pose3 offset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + Matrix H1; + Matrix H2; + Matrix H3; + gtsam::Vector actual = factor.evaluateError(pose, offset, calib, H1, H2, H3); 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); + std::default_random_engine rng(42); + std::uniform_real_distribution dist(-0.3, 0.3); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + // center of the random camera poses + Pose3 centerOffset( + Rot3(0, 0, 1,// + -1, 0, 0, // + 0, -1, 0), + Vector3(0, 0, 0) + ); 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))); + Point3 landmark(2 + dist(rng), dist(rng), dist(rng)); + Point2 measured(200 + 100 * dist(rng), 200 + 100 * dist(rng)); + Pose3 offset = centerOffset.compose( + Pose3( + Rot3::Ypr(dist(rng), dist(rng), dist(rng)), + Point3(dist(rng), dist(rng), dist(rng)))); Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); - PlanarProjectionFactor3 factor( - X(0), C(0), K(0), landmark, measured, model); + PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model); - Pose2 pose(s(g), s(g), s(g)); + Pose2 pose(dist(rng), dist(rng), dist(rng)); // actual H Matrix H1, H2, H3; @@ -362,6 +331,104 @@ TEST(PlanarProjectionFactor3, jacobian) { } } +/* ************************************************************************* */ +TEST(PlanarProjectionFactor3, solveOffset) { + // Example localization + SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01)); + // offset model is wide, so the solver finds the right answer. + SharedNoiseModel cNoise = noiseModel::Diagonal::Sigmas(Vector6(10, 10, 10, 10, 10, 10)); + SharedNoiseModel kNoise = noiseModel::Diagonal::Sigmas(Vector9(0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001)); + + // landmarks + Point3 l0(1, 0, 1); + Point3 l1(1, 0, 0); + Point3 l2(1, -1, 1); + Point3 l3(2, 2, 1); + + // camera pixels + Point2 p0(200, 200); + Point2 p1(200, 400); + Point2 p2(400, 200); + Point2 p3(0, 200); + + // body + Pose2 x0(0, 0, 0); + + // camera z looking at +x with (xy) antiparallel to (yz) + Pose3 c0( + Rot3(0, 0, 1, // + -1, 0, 0, // + 0, -1, 0), // + Vector3(0, 0, 1)); // note z offset + Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0); + + NonlinearFactorGraph graph; + graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l0, p0, pxModel)); + graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l1, p1, pxModel)); + graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l2, p2, pxModel)); + graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l3, p3, pxModel)); + graph.add(PriorFactor(X(0), x0, xNoise)); + graph.add(PriorFactor(C(0), c0, cNoise)); + graph.add(PriorFactor(K(0), calib, kNoise)); + + Values initialEstimate; + initialEstimate.insert(X(0), x0); + initialEstimate.insert(C(0), c0); + initialEstimate.insert(K(0), calib); + + // run the optimizer + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); + + // verify that the optimizer found the right pose. + CHECK(assert_equal(x0, result.at(X(0)), 2e-3)); + + // verify the camera is pointing at +x + Pose3 cc0 = result.at(C(0)); + CHECK(assert_equal(c0, cc0, 5e-3)); + + // verify the calibration + CHECK(assert_equal(calib, result.at(K(0)), 2e-3)); + + Marginals marginals(graph, result); + Matrix x0cov = marginals.marginalCovariance(X(0)); + + // narrow prior => ~zero cov + CHECK(assert_equal(Matrix33::Zero(), x0cov, 1e-4)); + + Matrix c0cov = marginals.marginalCovariance(C(0)); + + // invert the camera offset to get covariance in body coordinates + Matrix66 HcTb = cc0.inverse().AdjointMap().inverse(); + Matrix c0cov2 = HcTb * c0cov * HcTb.transpose(); + + // camera-frame stddev + Vector6 c0sigma = c0cov.diagonal().cwiseSqrt(); + CHECK(assert_equal((Vector6() << // + 0.009, + 0.011, + 0.004, + 0.012, + 0.012, + 0.011 + ).finished(), c0sigma, 1e-3)); + + // body frame stddev + Vector6 bTcSigma = c0cov2.diagonal().cwiseSqrt(); + CHECK(assert_equal((Vector6() << // + 0.004, + 0.009, + 0.011, + 0.012, + 0.012, + 0.012 + ).finished(), bTcSigma, 1e-3)); + + // narrow prior => ~zero cov + CHECK(assert_equal(Matrix99::Zero(), marginals.marginalCovariance(K(0)), 3e-3)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 69765496c5935edaa71791ac2a69b41a407b2acb Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 24 Dec 2024 14:27:11 -0800 Subject: [PATCH 08/11] fix windows test --- gtsam/slam/tests/testPlanarProjectionFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp index 1cc002797..603eb555a 100644 --- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -128,7 +128,7 @@ TEST(PlanarProjectionFactor1, jacobian) { [&factor](const Pose2& p) { return factor.evaluateError(p, {});}, pose); - CHECK(assert_equal(expectedH1, H1, 1e-6)); + CHECK(assert_equal(expectedH1, H1, 2e-6)); } } From 2827a1e04167707c74f0e51816d1e658b6e8eb58 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 24 Dec 2024 14:30:41 -0800 Subject: [PATCH 09/11] fix test names --- .../slam/tests/testPlanarProjectionFactor.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp index 603eb555a..acfa95618 100644 --- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -33,7 +33,7 @@ using symbol_shorthand::K; using symbol_shorthand::L; /* ************************************************************************* */ -TEST(PlanarProjectionFactor1, error1) { +TEST(PlanarProjectionFactor1, Error1) { // Example: center projection and Jacobian Point3 landmark(1, 0, 0); Point2 measured(200, 200); @@ -55,7 +55,7 @@ TEST(PlanarProjectionFactor1, error1) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor1, error2) { +TEST(PlanarProjectionFactor1, Error2) { // Example: upper left corner projection and Jacobian Point3 landmark(1, 1, 1); Point2 measured(0, 0); @@ -77,7 +77,7 @@ TEST(PlanarProjectionFactor1, error2) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor1, error3) { +TEST(PlanarProjectionFactor1, Error3) { // Example: upper left corner projection and Jacobian with distortion Point3 landmark(1, 1, 1); Point2 measured(0, 0); @@ -99,7 +99,7 @@ TEST(PlanarProjectionFactor1, error3) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor1, jacobian) { +TEST(PlanarProjectionFactor1, Jacobian) { // Verify Jacobians with numeric derivative std::default_random_engine rng(42); std::uniform_real_distribution dist(-0.3, 0.3); @@ -133,7 +133,7 @@ TEST(PlanarProjectionFactor1, jacobian) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor1, solve) { +TEST(PlanarProjectionFactor1, Solve) { // Example localization SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); @@ -193,7 +193,7 @@ TEST(PlanarProjectionFactor1, solve) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor3, error1) { +TEST(PlanarProjectionFactor3, Error1) { // Example: center projection and Jacobian Point3 landmark(1, 0, 0); Point2 measured(200, 200); @@ -223,7 +223,7 @@ TEST(PlanarProjectionFactor3, error1) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor3, error2) { +TEST(PlanarProjectionFactor3, Error2) { Point3 landmark(1, 1, 1); Point2 measured(0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); @@ -253,7 +253,7 @@ TEST(PlanarProjectionFactor3, error2) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor3, error3) { +TEST(PlanarProjectionFactor3, Error3) { Point3 landmark(1, 1, 1); Point2 measured(0, 0); SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); @@ -282,7 +282,7 @@ TEST(PlanarProjectionFactor3, error3) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor3, jacobian) { +TEST(PlanarProjectionFactor3, Jacobian) { // Verify Jacobians with numeric derivative std::default_random_engine rng(42); @@ -332,7 +332,7 @@ TEST(PlanarProjectionFactor3, jacobian) { } /* ************************************************************************* */ -TEST(PlanarProjectionFactor3, solveOffset) { +TEST(PlanarProjectionFactor3, SolveOffset) { // Example localization SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01)); From 1203f0c7024a197207c39f0995c61eaa860c06d5 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 24 Dec 2024 14:32:44 -0800 Subject: [PATCH 10/11] add newlines --- gtsam/geometry/Pose3.cpp | 2 +- gtsam/geometry/Pose3.h | 2 +- gtsam/slam/PlanarProjectionFactor.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 03464b0db..f3dacb163 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -523,4 +523,4 @@ std::ostream &operator<<(std::ostream &os, const Pose3& pose) { return os; } -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index ba00c1863..edb6094fa 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -461,4 +461,4 @@ struct Bearing : HasBearing {}; template struct Range : HasRange {}; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h index 22cea2214..d53cd0ae1 100644 --- a/gtsam/slam/PlanarProjectionFactor.h +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -295,4 +295,4 @@ namespace gtsam { struct traits : public Testable {}; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam From 993ac90e43012f8b2008ad5e74144a2d26622a91 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Tue, 24 Dec 2024 15:27:49 -0800 Subject: [PATCH 11/11] fix windows test --- gtsam/slam/tests/testPlanarProjectionFactor.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp index acfa95618..5f65c9b5e 100644 --- a/gtsam/slam/tests/testPlanarProjectionFactor.cpp +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -128,7 +128,7 @@ TEST(PlanarProjectionFactor1, Jacobian) { [&factor](const Pose2& p) { return factor.evaluateError(p, {});}, pose); - CHECK(assert_equal(expectedH1, H1, 2e-6)); + CHECK(assert_equal(expectedH1, H1, 5e-6)); } } @@ -325,9 +325,9 @@ TEST(PlanarProjectionFactor3, Jacobian) { [&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)); + CHECK(assert_equal(expectedH1, H1, 5e-6)); + CHECK(assert_equal(expectedH2, H2, 5e-6)); + CHECK(assert_equal(expectedH3, H3, 5e-6)); } }