Merge pull request #1934 from truher/team100_frc_factors
projection and SFM for 2d posesrelease/4.3a0
Frank Dellaert
GitHub
commit
a2f917aa09
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Lie.h>
|
||||
|
||||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
|
||||
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<Cal3DS2> camera = PinholeCamera<Cal3DS2>(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<Pose2> {
|
||||
public:
|
||||
typedef NoiseModelFactorN<Pose2> 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>(
|
||||
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<PlanarProjectionFactor1> :
|
||||
public Testable<PlanarProjectionFactor1> {};
|
||||
|
||||
/**
|
||||
* @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<Pose2, Point3> {
|
||||
public:
|
||||
typedef NoiseModelFactorN<Pose2, Point3> 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>(
|
||||
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<PlanarProjectionFactor2> :
|
||||
public Testable<PlanarProjectionFactor2> {};
|
||||
|
||||
/**
|
||||
* @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<Pose2, Pose3, Cal3DS2> {
|
||||
public:
|
||||
typedef NoiseModelFactorN<Pose2, Pose3, Cal3DS2> 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>(
|
||||
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<PlanarProjectionFactor3> :
|
||||
public Testable<PlanarProjectionFactor3> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -24,6 +24,38 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
|
|||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/slam/PlanarProjectionFactor.h>
|
||||
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 <gtsam/slam/ProjectionFactor.h>
|
||||
template <POSE, LANDMARK, CALIBRATION>
|
||||
virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,437 @@
|
|||
/**
|
||||
* @file testPlanarProjectionFactor.cpp
|
||||
* @date Dec 3, 2024
|
||||
* @author joel@truher.org
|
||||
* @brief unit tests for PlanarProjectionFactor
|
||||
*/
|
||||
|
||||
#include <random>
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#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>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
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<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 + 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<Vector, Pose2>(
|
||||
[&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<Pose2>(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<Pose2>(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() <<//
|
||||
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);
|
||||
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;
|
||||
gtsam::Vector actual = factor.evaluateError(pose, offset, calib, H1, H2, H3);
|
||||
CHECK(assert_equal(Vector2(0, 0), actual));
|
||||
CHECK(assert_equal((Matrix23() <<//
|
||||
-200, 200, 400,//
|
||||
-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(), H3, 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(PlanarProjectionFactor3, Error3) {
|
||||
Point3 landmark(1, 1, 1);
|
||||
Point2 measured(0, 0);
|
||||
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.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(), 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(), H3, 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(PlanarProjectionFactor3, Jacobian) {
|
||||
// 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 + 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<Vector, Pose2, Pose3, Cal3DS2>(
|
||||
[&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
|
||||
return factor.evaluateError(p, o, c, {}, {}, {});},
|
||||
pose, offset, calib);
|
||||
Matrix expectedH2 = numericalDerivative32<Vector, Pose2, Pose3, Cal3DS2>(
|
||||
[&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
|
||||
return factor.evaluateError(p, o, c, {}, {}, {});},
|
||||
pose, offset, calib);
|
||||
Matrix expectedH3 = numericalDerivative33<Vector, Pose2, Pose3, Cal3DS2>(
|
||||
[&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<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;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue