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;