12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

remove "cam coord" idea

release/4.3a0
Joel Truher 2024-12-24 09:06:25 -08:00
parent e4538d5b3e
commit 0f8fe15e31
4 changed files with 355 additions and 306 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/geometry/Pose3.cpp
View File

@ -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);
}

1
gtsam/geometry/Pose3.h
View File

@ -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 = {});
/**

110
gtsam/slam/PlanarProjectionFactor.h
View File

@ -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<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib);
Matrix63 Hp; // 6x3
Matrix66 H0; // 6x6
Pose3 wTc = Pose3::FromPose2(wTb, HwTb ? &Hp : nullptr).compose(bTc, HwTb ? &H0 : nullptr);
PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(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 (!)
*HbTc = Dpose;
if (HwTb)
*HwTb = DposeOffset * H0 * Hp;
*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>(
gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this)));
NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<NonlinearFactor>(
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_;
}
@ -188,7 +169,7 @@ namespace gtsam {
* This is similar to GeneralSFMFactor, used for SLAM.
*/
class PlanarProjectionFactor2
: public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Point3> {
: public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Point3> {
public:
typedef NoiseModelFactorN<Pose2, Point3> 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>(
gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this)));
NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<NonlinearFactor>(
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>(
gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor3(*this)));
NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<NonlinearFactor>(
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_;
}

535
gtsam/slam/tests/testPlanarProjectionFactor.cpp
View File

@ -16,6 +16,10 @@
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PlanarProjectionFactor.h>
@ -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<Matrix> 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<Matrix> 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<Matrix> 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<double> s(-0.3, 0.3);
// Verify Jacobians with numeric derivative
std::default_random_engine rng(42);
std::uniform_real_distribution<double> 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<Vector, Pose2>(
auto expectedH1 = numericalDerivative11<Vector, Pose2>(
[&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<Pose2>(X(0), x0, xNoise));
CHECK_EQUAL(2, factor.dim());
CHECK(factor.active(values));
std::vector<Matrix> 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<Pose2>(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() <<//
0, 200, 200,//
0, 0, 0).finished();
CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
// pose stddev
Vector3 sigma = cov.diagonal().cwiseSqrt();
CHECK(assert_equal((Vector3() << //
0.0035,
0.0359,
0.0354
).finished(), sigma, 1e-4));
// du/dx for the pose3d offset
// note this is (roll, pitch, yaw, x, y, z)
Matrix26 H2Expected = (Matrix26() <<//
0, 0, 200, 0, 200, 0,//
0, -200, 0, 0, 0, 200).finished();
CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
// du wrt calibration
// on-bore, f doesn't matter
// but c does
Matrix29 H3Expected = (Matrix29() <<//
0, 0, 0, 1, 0, 0, 0, 0, 0,//
0, 0, 0, 0, 1, 0, 0, 0, 0).finished();
CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
}
/* ************************************************************************* */
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() <<//
0, 200, 200,//
0, 0, 0).finished(), H1, 1e-6));
CHECK(assert_equal((Matrix26() <<//
0, -200, 0, -200, 0, 0,//
200, -0, 0, 0, -200, 0).finished(), H2, 1e-6));
CHECK(assert_equal((Matrix29() <<//
0, 0, 0, 1, 0, 0, 0, 0, 0,//
0, 0, 0, 0, 1, 0, 0, 0, 0).finished(), H3, 1e-6));
}
/* ************************************************************************* */
TEST(PlanarProjectionFactor3, error2) {
Point3 landmark(1, 1, 1);
Point2 measured(0, 0);
Pose3 offset;
Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
PlanarProjectionFactor3 factor(
X(0), C(0), K(0), landmark, measured, model);
Values values;
PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model);
Pose2 pose(0, 0, 0);
values.insert(X(0), pose);
values.insert(C(0), offset);
values.insert(K(0), calib);
CHECK_EQUAL(2, model->dim());
CHECK_EQUAL(2, factor.dim());
CHECK(factor.active(values));
std::vector<Matrix> 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() <<//
CHECK(assert_equal((Matrix23() <<//
-200, 200, 400,//
-200, 0, 200).finished();
CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
// du/dx for the pose3d offset
// note this is (roll, pitch, yaw, x, y, z)
Matrix26 H2Expected = (Matrix26() <<//
-200, -200, 400, -200, 200, 0,//
200, -400, 200, -200, 0, 200).finished();
CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
Matrix29 H3Expected = (Matrix29() <<//
-200, 0, 200).finished(), H1, 1e-6));
CHECK(assert_equal((Matrix26() <<//
200, -400, -200, -200, 0, -200,//
400, -200, 200, 0, -200, -200).finished(), H2, 1e-6));
CHECK(assert_equal((Matrix29() <<//
-1, 0, -1, 1, 0, -400, -800, 400, 800,//
0, -1, 0, 0, 1, -400, -800, 800, 400).finished();
CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
0, -1, 0, 0, 1, -400, -800, 800, 400).finished(), H3, 1e-6));
}
/* ************************************************************************* */
TEST(PlanarProjectionFactor3, error3) {
Point3 landmark(1, 1, 1);
Point2 measured(0, 0);
Pose3 offset;
Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
PlanarProjectionFactor3 factor(
X(0), C(0), K(0), landmark, measured, model);
Values values;
PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model);
Pose2 pose(0, 0, 0);
values.insert(X(0), pose);
values.insert(C(0), offset);
values.insert(K(0), calib);
CHECK_EQUAL(2, model->dim());
CHECK_EQUAL(2, factor.dim());
CHECK(factor.active(values));
std::vector<Matrix> 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() <<//
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);
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() <<//
-360, 280, 640,//
-360, 80, 440).finished();
CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
// du/dx for the pose3d offset
// note this is (roll, pitch, yaw, x, y, z)
Matrix26 H2Expected = (Matrix26() <<//
-200, -440, 640, -360, 280, 80,//
200, -640, 440, -360, 80, 280).finished();
CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
Matrix29 H3Expected = (Matrix29() <<//
-360, 80, 440).finished(), H1, 1e-6));
CHECK(assert_equal((Matrix26() <<//
440, -640, -200, -280, -80, -360,//
640, -440, 200, -80, -280, -360).finished(), H2, 1e-6));
CHECK(assert_equal((Matrix29() <<//
-1, 0, -1, 1, 0, -400, -800, 400, 800,//
0, -1, 0, 0, 1, -400, -800, 800, 400).finished();
CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
0, -1, 0, 0, 1, -400, -800, 800, 400).finished(), H3, 1e-6));
}
/* ************************************************************************* */
TEST(PlanarProjectionFactor3, jacobian) {
// test many jacobians with many randoms
// Verify Jacobians with numeric derivative
std::default_random_engine g;
std::uniform_real_distribution<double> s(-0.3, 0.3);
std::default_random_engine rng(42);
std::uniform_real_distribution<double> 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<Pose2>(X(0), x0, xNoise));
graph.add(PriorFactor<Pose3>(C(0), c0, cNoise));
graph.add(PriorFactor<Cal3DS2>(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<Pose2>(X(0)), 2e-3));
// verify the camera is pointing at +x
Pose3 cc0 = result.at<Pose3>(C(0));
CHECK(assert_equal(c0, cc0, 5e-3));
// verify the calibration
CHECK(assert_equal(calib, result.at<Cal3DS2>(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;