diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index d0bf21047..169259a36 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -126,10 +126,10 @@ namespace gtsam { gttoc(product); // Max over all the potentials by pretending all keys are frontal: - auto normalization = product.max(product.size()); + auto normalizer = product.max(product.size()); // Normalize the product factor to prevent underflow. - product = product / (*normalization); + product = product / (*normalizer); return product; } @@ -207,8 +207,7 @@ namespace gtsam { return dag.argmax(); } - DiscreteValues DiscreteFactorGraph::optimize( - const Ordering& ordering) const { + DiscreteValues DiscreteFactorGraph::optimize(const Ordering& ordering) const { gttic(DiscreteFactorGraph_optimize); DiscreteLookupDAG dag = maxProduct(ordering); return dag.argmax(); diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 22548da07..bf9662e34 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -252,41 +252,12 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const { DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { DiscreteKeys dkeys = discreteKeys(); - // Record key assignment and value pairs in pair_table. - // The assignments are stored in descending order of keys so that the order of - // the values matches what is expected by a DecisionTree. - // This is why we reverse the keys and then - // query for the key value/assignment. - DiscreteKeys rdkeys(dkeys.rbegin(), dkeys.rend()); - std::vector> pair_table; + std::vector table; for (auto i = 0; i < sparse_table_.size(); i++) { - std::stringstream ss; - for (auto&& [key, _] : rdkeys) { - ss << keyValueForIndex(key, i); - } - // k will be in reverse key order already - uint64_t k; - ss >> k; - pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i))); + table.push_back(sparse_table_.coeff(i)); } - // Sort the pair_table (of assignment-value pairs) based on assignment so we - // get values in reverse key order. - std::sort( - pair_table.begin(), pair_table.end(), - [](const std::pair& a, - const std::pair& b) { return a.first < b.first; }); - - // Create the table vector by extracting the values from pair_table. - // The pair_table has already been sorted in the desired order, - // so the values will be in descending key order. - std::vector table; - std::for_each(pair_table.begin(), pair_table.end(), - [&table](const std::pair& pair) { - table.push_back(pair.second); - }); - - AlgebraicDecisionTree tree(rdkeys, table); + AlgebraicDecisionTree tree(dkeys, table); DecisionTreeFactor f(dkeys, tree); return f; } diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 0d71c12ba..cbcf5234e 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -117,9 +117,9 @@ TEST(DiscreteFactorGraph, test) { *std::dynamic_pointer_cast(newFactorPtr); // Normalize newFactor by max for comparison with expected - auto normalization = newFactor.max(newFactor.size()); + auto normalizer = newFactor.max(newFactor.size()); - newFactor = newFactor / *normalization; + newFactor = newFactor / *normalizer; // Check Conditional CHECK(conditional); @@ -131,9 +131,9 @@ TEST(DiscreteFactorGraph, test) { CHECK(&newFactor); DecisionTreeFactor expectedFactor(B & A, "10 6 6 10"); // Normalize by max. - normalization = expectedFactor.max(expectedFactor.size()); - // Ensure normalization is correct. - expectedFactor = expectedFactor / *normalization; + normalizer = expectedFactor.max(expectedFactor.size()); + // Ensure normalizer is correct. + expectedFactor = expectedFactor / *normalizer; EXPECT(assert_equal(expectedFactor, newFactor)); // Test using elimination tree diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 16dda3d1e..6d9256f93 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -43,6 +43,20 @@ Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR, return Pose3(R, t); } +// 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); +} + /* ************************************************************************* */ Pose3 Pose3::inverse() const { Rot3 Rt = R_.inverse(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 81a9848e2..f7a4ceecd 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -78,6 +78,9 @@ 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 = {}); + /** * 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/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 8832cbb34..b9051554a 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -282,7 +282,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors, } else if (auto hc = dynamic_pointer_cast(f)) { auto dc = hc->asDiscrete(); if (!dc) throwRuntimeError("discreteElimination", dc); - dfg.push_back(hc->asDiscrete()); + dfg.push_back(dc); } else { throwRuntimeError("discreteElimination", f); } diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h new file mode 100644 index 000000000..d53cd0ae1 --- /dev/null +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -0,0 +1,298 @@ +/** + * 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. + * + * @see https://www.firstinspires.org/ + * + * @file PlanarProjectionFactor.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 PlanarProjectionFactorBase + * @brief Camera projection for robot on the floor. Measurement is camera pixels. + */ + class PlanarProjectionFactorBase { + protected: + PlanarProjectionFactorBase() {} + + /** + * @param measured pixels in the camera frame + */ + PlanarProjectionFactorBase(const Point2& measured) : measured_(measured) {} + + /** + * 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 predict( + const Point3& landmark, + const Pose2& wTb, + const Pose3& bTc, + const Cal3DS2& calib, + 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 + 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 (R,t) + Matrix26 Dpose; + Point2 result = camera.project(landmark, Dpose, Hlandmark, Hcalib); + if (HbTc) + *HbTc = Dpose; + if (HwTb) + *HwTb = Dpose * H0 * Hp; + return result; + } else { + return camera.project(landmark, {}, {}, {}); + } +#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION + } catch (CheiralityException& e) { + std::cout << "****** CHIRALITY EXCEPTION ******\n"; + 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()); + } +#endif + } + + Point2 measured_; // pixel measurement + }; + + + /** + * @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 + NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this))); + } + + + /** + * @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 bTc "body to camera": constant camera offset from pose + * @param calib constant camera calibration + * @param model stddev of the measurements, ~one pixel? + */ + PlanarProjectionFactor1( + Key poseKey, + const Point3& landmark, + const Point2& measured, + const Pose3& bTc, + const Cal3DS2& calib, + const SharedNoiseModel& model = {}) + : PlanarProjectionFactorBase(measured), + NoiseModelFactorN(model, poseKey), + landmark_(landmark), + bTc_(bTc), + calib_(calib) {} + + /** + * @param wTb "world to body": estimated pose2 + * @param HwTb jacobian + */ + Vector evaluateError(const Pose2& wTb, OptionalMatrixType HwTb) const override { + return predict(landmark_, wTb, bTc_, calib_, {}, HwTb, {}, {}) - measured_; + } + + private: + Point3 landmark_; // landmark + Pose3 bTc_; // "body to camera": camera offset to robot pose + Cal3DS2 calib_; // camera calibration + }; + + template<> + struct traits : + public Testable {}; + + /** + * @class PlanarProjectionFactor2 + * @brief Two unknowns: the pose and the landmark. + * Camera offset and calibration are constant. + * This is similar to GeneralSFMFactor, used for SLAM. + */ + class PlanarProjectionFactor2 + : public PlanarProjectionFactorBase, public NoiseModelFactorN { + public: + typedef NoiseModelFactorN Base; + using Base::evaluateError; + + PlanarProjectionFactor2() {} + + ~PlanarProjectionFactor2() override {} + + /// @return a deep copy of this factor + NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this))); + } + + /** + * @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( + Key poseKey, + Key landmarkKey, + const Point2& measured, + const Pose3& bTc, + const Cal3DS2& calib, + const SharedNoiseModel& model = {}) + : PlanarProjectionFactorBase(measured), + NoiseModelFactorN(model, landmarkKey, poseKey), + bTc_(bTc), + calib_(calib) {} + + /** + * @param wTb "world to body": estimated pose2 + * @param landmark estimated landmark + * @param HwTb jacobian + * @param Hlandmark jacobian + */ + Vector evaluateError( + const Pose2& wTb, + const Point3& landmark, + OptionalMatrixType HwTb, + OptionalMatrixType Hlandmark) const override { + return predict(landmark, wTb, bTc_, calib_, Hlandmark, HwTb, {}, {}) - measured_; + } + + private: + Pose3 bTc_; // "body to camera": 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 + 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? + */ + PlanarProjectionFactor3( + Key poseKey, + Key offsetKey, + Key calibKey, + const Point3& landmark, + const Point2& measured, + const SharedNoiseModel& model = {}) + : PlanarProjectionFactorBase(measured), + NoiseModelFactorN(model, poseKey, offsetKey, calibKey), + 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 HbTc offset jacobian + * @param Hcalib calibration jacobian + */ + Vector evaluateError( + const Pose2& wTb, + const Pose3& bTc, + const Cal3DS2& calib, + OptionalMatrixType HwTb, + OptionalMatrixType HbTc, + OptionalMatrixType Hcalib) const override { + return predict(landmark_, wTb, bTc, calib, {}, HwTb, HbTc, Hcalib) - measured_; + } + + private: + Point3 landmark_; // landmark + }; + + template<> + struct traits : + public Testable {}; + +} // namespace gtsam diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index a226f06ec..1761a6cc3 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -24,6 +24,38 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor { void serialize() const; }; +#include +virtual class PlanarProjectionFactor1 : gtsam::NoiseModelFactor { + PlanarProjectionFactor1( + 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( + 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( + 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; +}; + #include template virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp new file mode 100644 index 000000000..5f65c9b5e --- /dev/null +++ b/gtsam/slam/tests/testPlanarProjectionFactor.cpp @@ -0,0 +1,437 @@ +/** + * @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 +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::X; +using symbol_shorthand::C; +using symbol_shorthand::K; +using symbol_shorthand::L; + +/* ************************************************************************* */ +TEST(PlanarProjectionFactor1, Error1) { + // Example: center projection and Jacobian + Point3 landmark(1, 0, 0); + Point2 measured(200, 200); + 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); + Pose2 pose(0, 0, 0); + 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) { + // Example: upper left corner projection and Jacobian + Point3 landmark(1, 1, 1); + Point2 measured(0, 0); + 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); + Pose2 pose(0, 0, 0); + 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(), H, 1e-6)); +} + +/* ************************************************************************* */ +TEST(PlanarProjectionFactor1, Error3) { + // Example: upper left corner projection and Jacobian with distortion + Point3 landmark(1, 1, 1); + Point2 measured(0, 0); + 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); + Pose2 pose(0, 0, 0); + 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(), H, 1e-6)); +} + +/* ************************************************************************* */ +TEST(PlanarProjectionFactor1, Jacobian) { + // 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 + 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(dist(rng), dist(rng), dist(rng)); + Matrix H1; + factor.evaluateError(pose, H1); + auto expectedH1 = numericalDerivative11( + [&factor](const Pose2& p) { + return factor.evaluateError(p, {});}, + pose); + CHECK(assert_equal(expectedH1, H1, 5e-6)); + } +} + +/* ************************************************************************* */ +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)); + + // 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); + + 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)); + + Values initialEstimate; + initialEstimate.insert(X(0), x0); + + // 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)); + + // 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)); + + // pose stddev + Vector3 sigma = cov.diagonal().cwiseSqrt(); + CHECK(assert_equal((Vector3() << // + 0.0035, + 0.0359, + 0.0354 + ).finished(), sigma, 1e-4)); + +} + +/* ************************************************************************* */ +TEST(PlanarProjectionFactor3, Error1) { + // Example: center projection and Jacobian + Point3 landmark(1, 0, 0); + Point2 measured(200, 200); + SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1)); + PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model); + Pose2 pose(0, 0, 0); + 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; + CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, offset, calib, H1, H2, H3), 1e-6)); + CHECK(assert_equal((Matrix23() < 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 + 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); + + Pose2 pose(dist(rng), dist(rng), dist(rng)); + + // 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, 5e-6)); + CHECK(assert_equal(expectedH2, H2, 5e-6)); + CHECK(assert_equal(expectedH3, H3, 5e-6)); + } +} + +/* ************************************************************************* */ +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; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/wrap/pybind11/docs/requirements.txt b/wrap/pybind11/docs/requirements.txt index 293db6a06..8dffd36b5 100644 --- a/wrap/pybind11/docs/requirements.txt +++ b/wrap/pybind11/docs/requirements.txt @@ -130,9 +130,9 @@ imagesize==1.4.1 \ --hash=sha256:0d8d18d08f840c19d0ee7ca1fd82490fdc3729b7ac93f49870406ddde8ef8d8b \ --hash=sha256:69150444affb9cb0d5cc5a92b3676f0b2fb7cd9ae39e947a5e11a36b4497cd4a # via sphinx -jinja2==3.1.4 \ - --hash=sha256:4a3aee7acbbe7303aede8e9648d13b8bf88a429282aa6122a993f0ac800cb369 \ - --hash=sha256:bc5dd2abb727a5319567b7a813e6a2e7318c39f4f487cfe6c89c6f9c7d25197d +jinja2==3.1.5 \ + --hash=sha256:8fefff8dc3034e27bb80d67c671eb8a9bc424c0ef4c0826edbff304cceff43bb \ + --hash=sha256:aba0f4dc9ed8013c424088f68a5c226f7d6097ed89b246d7749c2ec4175c6adb # via sphinx markupsafe==2.1.5 \ --hash=sha256:00e046b6dd71aa03a41079792f8473dc494d564611a8f89bbbd7cb93295ebdcf \