projection and SFM for 2d poses
Joel Truher
parent
bb7b6b39c7
commit
06d45c416e
|
|
@ -0,0 +1,154 @@
|
|||
/**
|
||||
* Similar to GenericProjectionFactor, but:
|
||||
*
|
||||
* * 2d pose variable (robot on the floor)
|
||||
* * constant landmarks
|
||||
* * batched input
|
||||
* * numeric differentiation
|
||||
*
|
||||
* This factor is useful for high-school robotics competitions,
|
||||
* which run robots on the floor, with use fixed maps and fiducial
|
||||
* markers. The camera offset and calibration are fixed, perhaps
|
||||
* found using PlanarSFMFactor.
|
||||
*
|
||||
* The python version of this factor uses batches, to save on calls
|
||||
* across the C++/python boundary, but here the only extra cost
|
||||
* is instantiating the camera, so there's no batch.
|
||||
*
|
||||
* @see https://www.firstinspires.org/
|
||||
* @see PlanarSFMFactor.h
|
||||
*
|
||||
* @file PlanarSFMFactor.h
|
||||
* @brief for planar smoothing
|
||||
* @date Dec 2, 2024
|
||||
* @author joel@truher.org
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <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 PlanarProjectionFactor
|
||||
* @brief Camera projection for robot on the floor.
|
||||
*/
|
||||
class PlanarProjectionFactor : public NoiseModelFactorN<Pose2> {
|
||||
typedef NoiseModelFactorN<Pose2> Base;
|
||||
static const Pose3 CAM_COORD;
|
||||
|
||||
protected:
|
||||
|
||||
Point3 landmark_; // landmark
|
||||
Point2 measured_; // pixel measurement
|
||||
Pose3 offset_; // camera offset to robot pose
|
||||
Cal3DS2 calib_; // camera calibration
|
||||
|
||||
public:
|
||||
// Provide access to the Matrix& version of evaluateError:
|
||||
using Base::evaluateError;
|
||||
|
||||
PlanarProjectionFactor() {}
|
||||
|
||||
/**
|
||||
* @param landmarks point in the world
|
||||
* @param measured corresponding point in the camera frame
|
||||
* @param offset constant camera offset from pose
|
||||
* @param calib constant camera calibration
|
||||
* @param model stddev of the measurements, ~one pixel?
|
||||
* @param poseKey index of the robot pose in the z=0 plane
|
||||
*/
|
||||
PlanarProjectionFactor(
|
||||
const Point3& landmark,
|
||||
const Point2& measured,
|
||||
const Pose3& offset,
|
||||
const Cal3DS2& calib,
|
||||
const SharedNoiseModel& model,
|
||||
Key poseKey)
|
||||
: NoiseModelFactorN(model, poseKey),
|
||||
landmark_(landmark),
|
||||
measured_(measured),
|
||||
offset_(offset),
|
||||
calib_(calib)
|
||||
{
|
||||
assert(2 == model->dim());
|
||||
}
|
||||
|
||||
~PlanarProjectionFactor() override {}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
gtsam::NonlinearFactor::shared_ptr clone() const override {
|
||||
return std::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor(*this)));
|
||||
}
|
||||
|
||||
Point2 h2(const Pose2& pose, OptionalMatrixType H1) const {
|
||||
// this is x-forward z-up
|
||||
gtsam::Matrix H0;
|
||||
Pose3 offset_pose = Pose3(pose).compose(offset_, H0);
|
||||
// this is z-forward y-down
|
||||
gtsam::Matrix H00;
|
||||
Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
|
||||
PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib_);
|
||||
if (H1) {
|
||||
// Dpose is 2x6 (R,t)
|
||||
// H1 is 2x3 (x, y, theta)
|
||||
gtsam::Matrix Dpose;
|
||||
Point2 result = camera.project(landmark_, Dpose);
|
||||
Dpose = Dpose * H00 * H0;
|
||||
*H1 = Matrix::Zero(2, 3);
|
||||
(*H1)(0, 0) = Dpose(0, 3); // du/dx
|
||||
(*H1)(1, 0) = Dpose(1, 3); // dv/dx
|
||||
(*H1)(0, 1) = Dpose(0, 4); // du/dy
|
||||
(*H1)(1, 1) = Dpose(1, 4); // dv/dy
|
||||
(*H1)(0, 2) = Dpose(0, 2); // du/dyaw
|
||||
(*H1)(1, 2) = Dpose(1, 2); // dv/dyaw
|
||||
return result;
|
||||
}
|
||||
else {
|
||||
return camera.project(landmark_, {}, {}, {});
|
||||
}
|
||||
}
|
||||
|
||||
Vector evaluateError(
|
||||
const Pose2& pose,
|
||||
OptionalMatrixType H1 = OptionalNone) const override {
|
||||
try {
|
||||
return h2(pose, H1) - measured_;
|
||||
} catch (CheiralityException& e) {
|
||||
std::cout << "****** CHIRALITY EXCEPTION ******\n";
|
||||
std::cout << "landmark " << landmark_ << "\n";
|
||||
std::cout << "pose " << pose << "\n";
|
||||
std::cout << "offset " << offset_ << "\n";
|
||||
std::cout << "calib " << calib_ << "\n";
|
||||
if (H1) *H1 = Matrix::Zero(2, 3);
|
||||
// return a large error
|
||||
return Matrix::Constant(2, 1, 2.0 * calib_.fx());
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// camera "zero" is facing +z; this turns it to face +x
|
||||
const Pose3 PlanarProjectionFactor::CAM_COORD = Pose3(
|
||||
Rot3(0, 0, 1,//
|
||||
-1, 0, 0, //
|
||||
0, -1, 0),
|
||||
Vector3(0, 0, 0)
|
||||
);
|
||||
|
||||
template<>
|
||||
struct traits<PlanarProjectionFactor> :
|
||||
public Testable<PlanarProjectionFactor > {
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,165 @@
|
|||
/**
|
||||
* Similar to GeneralSFMFactor, but:
|
||||
*
|
||||
* * 2d pose variable (robot on the floor)
|
||||
* * camera offset variable
|
||||
* * constant landmarks
|
||||
* * batched input
|
||||
* * numeric differentiation
|
||||
*
|
||||
* This factor is useful to find camera calibration and placement, in
|
||||
* a sort of "autocalibrate" mode. Once a satisfactory solution is
|
||||
* found, the PlanarProjectionFactor should be used for localization.
|
||||
*
|
||||
* The python version of this factor uses batches, to save on calls
|
||||
* across the C++/python boundary, but here the only extra cost
|
||||
* is instantiating the camera, so there's no batch.
|
||||
*
|
||||
* @see https://www.firstinspires.org/
|
||||
* @see PlanarProjectionFactor.h
|
||||
*
|
||||
* @file PlanarSFMFactor.h
|
||||
* @brief for planar smoothing with unknown calibration
|
||||
* @date Dec 2, 2024
|
||||
* @author joel@truher.org
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <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 PlanarSFMFactor
|
||||
* @brief Camera calibration for robot on the floor.
|
||||
*/
|
||||
class PlanarSFMFactor : public NoiseModelFactorN<Pose2, Pose3, Cal3DS2> {
|
||||
private:
|
||||
typedef NoiseModelFactorN<Pose2, Pose3, Cal3DS2> Base;
|
||||
static const Pose3 CAM_COORD;
|
||||
|
||||
protected:
|
||||
|
||||
Point3 landmark_; // landmark
|
||||
Point2 measured_; // pixel measurement
|
||||
|
||||
public:
|
||||
// Provide access to the Matrix& version of evaluateError:
|
||||
using Base::evaluateError;
|
||||
|
||||
PlanarSFMFactor() {}
|
||||
/**
|
||||
* @param landmarks point in the world
|
||||
* @param measured corresponding point in the camera frame
|
||||
* @param model stddev of the measurements, ~one pixel?
|
||||
* @param poseKey index of the robot pose2 in the z=0 plane
|
||||
* @param offsetKey index of the 3d camera offset from the robot pose
|
||||
* @param calibKey index of the camera calibration
|
||||
*/
|
||||
PlanarSFMFactor(
|
||||
const Point3& landmark,
|
||||
const Point2& measured,
|
||||
const SharedNoiseModel& model,
|
||||
Key poseKey,
|
||||
Key offsetKey,
|
||||
Key calibKey)
|
||||
: NoiseModelFactorN(model, poseKey, offsetKey, calibKey),
|
||||
landmark_(landmark), measured_(measured)
|
||||
{
|
||||
assert(2 == model->dim());
|
||||
}
|
||||
|
||||
~PlanarSFMFactor() override {}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
gtsam::NonlinearFactor::shared_ptr clone() const override {
|
||||
return std::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
gtsam::NonlinearFactor::shared_ptr(new PlanarSFMFactor(*this)));
|
||||
}
|
||||
|
||||
Point2 h2(const Pose2& pose,
|
||||
const Pose3& offset,
|
||||
const Cal3DS2& calib,
|
||||
OptionalMatrixType H1,
|
||||
OptionalMatrixType H2,
|
||||
OptionalMatrixType H3
|
||||
) const {
|
||||
// this is x-forward z-up
|
||||
gtsam::Matrix H0;
|
||||
Pose3 offset_pose = Pose3(pose).compose(offset, H0);
|
||||
// this is z-forward y-down
|
||||
gtsam::Matrix H00;
|
||||
Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
|
||||
PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib);
|
||||
if (H1 || H2) {
|
||||
gtsam::Matrix Dpose;
|
||||
Point2 result = camera.project(landmark_, Dpose, {}, H3);
|
||||
gtsam::Matrix DposeOffset = Dpose * H00;
|
||||
if (H2)
|
||||
*H2 = DposeOffset; // a deep copy
|
||||
if (H1) {
|
||||
gtsam::Matrix DposeOffsetFwd = DposeOffset * H0;
|
||||
*H1 = Matrix::Zero(2,3);
|
||||
(*H1)(0,0) = DposeOffsetFwd(0,3); // du/dx
|
||||
(*H1)(1,0) = DposeOffsetFwd(1,3); // dv/dx
|
||||
(*H1)(0,1) = DposeOffsetFwd(0,4); // du/dy
|
||||
(*H1)(1,1) = DposeOffsetFwd(1,4); // dv/dy
|
||||
(*H1)(0,2) = DposeOffsetFwd(0,2); // du/dyaw
|
||||
(*H1)(1,2) = DposeOffsetFwd(1,2); // dv/dyaw
|
||||
}
|
||||
return result;
|
||||
} else {
|
||||
return camera.project(landmark_, {}, {}, {});
|
||||
}
|
||||
}
|
||||
|
||||
Vector evaluateError(
|
||||
const Pose2& pose,
|
||||
const Pose3& offset,
|
||||
const Cal3DS2& calib,
|
||||
OptionalMatrixType H1 = OptionalNone,
|
||||
OptionalMatrixType H2 = OptionalNone,
|
||||
OptionalMatrixType H3 = OptionalNone
|
||||
) const override {
|
||||
try {
|
||||
return h2(pose,offset,calib,H1,H2,H3) - measured_;
|
||||
}
|
||||
catch (CheiralityException& e) {
|
||||
std::cout << "****** CHIRALITY EXCEPTION ******\n";
|
||||
std::cout << "landmark " << landmark_ << "\n";
|
||||
std::cout << "pose " << pose << "\n";
|
||||
std::cout << "offset " << offset << "\n";
|
||||
std::cout << "calib " << calib << "\n";
|
||||
if (H1) *H1 = Matrix::Zero(2, 3);
|
||||
if (H2) *H2 = Matrix::Zero(2, 6);
|
||||
if (H3) *H3 = Matrix::Zero(2, 9);
|
||||
// return a large error
|
||||
return Matrix::Constant(2, 1, 2.0 * calib.fx());
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// camera "zero" is facing +z; this turns it to face +x
|
||||
const Pose3 PlanarSFMFactor::CAM_COORD = Pose3(
|
||||
Rot3(0, 0, 1,//
|
||||
-1, 0, 0, //
|
||||
0, -1, 0),
|
||||
Vector3(0, 0, 0)
|
||||
);
|
||||
|
||||
template<>
|
||||
struct traits<PlanarSFMFactor> :
|
||||
public Testable<PlanarSFMFactor > {
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -24,6 +24,18 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
|
|||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/slam/PlanarProjectionFactor.h>
|
||||
virtual class PlanarProjectionFactor : gtsam::NoiseModelFactor {
|
||||
PlanarProjectionFactor(
|
||||
const gtsam::Point3& landmark,
|
||||
const gtsam::Point2& measured,
|
||||
const gtsam::Pose3& offset,
|
||||
const gtsam::Cal3DS2& calib,
|
||||
const gtsam::noiseModel::Base* model,
|
||||
size_t poseKey);
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
template <POSE, LANDMARK, CALIBRATION>
|
||||
virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
|
||||
|
|
@ -67,6 +79,18 @@ typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
|
|||
gtsam::Cal3Unified>
|
||||
GenericProjectionFactorCal3Unified;
|
||||
|
||||
#include <gtsam/slam/PlanarSFMFactor.h>
|
||||
virtual class PlanarSFMFactor : gtsam::NoiseModelFactor {
|
||||
PlanarSFMFactor(
|
||||
const gtsam::Point3& landmark,
|
||||
const gtsam::Point2& measured,
|
||||
const gtsam::noiseModel::Base* model,
|
||||
size_t poseKey,
|
||||
size_t offsetKey,
|
||||
size_t calibKey);
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/slam/GeneralSFMFactor.h>
|
||||
template <CAMERA, LANDMARK>
|
||||
virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,156 @@
|
|||
/**
|
||||
* @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/Values.h>
|
||||
#include <gtsam/slam/PlanarProjectionFactor.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using symbol_shorthand::X;
|
||||
|
||||
TEST(PlanarProjectionFactor, error1) {
|
||||
// landmark is on the camera bore (which faces +x)
|
||||
Point3 landmark(1, 0, 0);
|
||||
// so pixel measurement is (cx, cy)
|
||||
Point2 measured(200, 200);
|
||||
Pose3 offset;
|
||||
Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
|
||||
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
|
||||
Values values;
|
||||
Pose2 pose(0, 0, 0);
|
||||
values.insert(X(0), pose);
|
||||
|
||||
PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
|
||||
|
||||
CHECK_EQUAL(2, factor.dim());
|
||||
CHECK(factor.active(values));
|
||||
std::vector<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));
|
||||
}
|
||||
|
||||
TEST(PlanarProjectionFactor, error2) {
|
||||
// landmark is in the upper left corner
|
||||
Point3 landmark(1, 1, 1);
|
||||
// upper left corner in pixels
|
||||
Point2 measured(0, 0);
|
||||
Pose3 offset;
|
||||
Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
|
||||
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
|
||||
PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
|
||||
Values values;
|
||||
Pose2 pose(0, 0, 0);
|
||||
|
||||
values.insert(X(0), pose);
|
||||
|
||||
CHECK_EQUAL(2, factor.dim());
|
||||
CHECK(factor.active(values));
|
||||
std::vector<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() << //
|
||||
-200, 200, 400, //
|
||||
-200, 0, 200).finished();
|
||||
CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
|
||||
}
|
||||
|
||||
TEST(PlanarProjectionFactor, error3) {
|
||||
// landmark is in the upper left corner
|
||||
Point3 landmark(1, 1, 1);
|
||||
// upper left corner in pixels
|
||||
Point2 measured(0, 0);
|
||||
Pose3 offset;
|
||||
// distortion
|
||||
Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
|
||||
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
|
||||
PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
|
||||
Values values;
|
||||
Pose2 pose(0, 0, 0);
|
||||
|
||||
values.insert(X(0), pose);
|
||||
|
||||
CHECK_EQUAL(2, factor.dim());
|
||||
CHECK(factor.active(values));
|
||||
std::vector<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() << //
|
||||
-360, 280, 640, //
|
||||
-360, 80, 440).finished();
|
||||
CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
|
||||
}
|
||||
|
||||
TEST(PlanarProjectionFactor, jacobian) {
|
||||
// test many jacobians with many randoms
|
||||
|
||||
std::default_random_engine g;
|
||||
std::uniform_real_distribution<double> s(-0.3, 0.3);
|
||||
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
|
||||
|
||||
for (int i = 0; i < 1000; ++i) {
|
||||
Point3 landmark(2 + s(g), s(g), s(g));
|
||||
Point2 measured(200 + 100*s(g), 200 + 100*s(g));
|
||||
Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
|
||||
Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
|
||||
|
||||
PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
|
||||
|
||||
Pose2 pose(s(g), s(g), s(g));
|
||||
|
||||
// actual H
|
||||
Matrix H1;
|
||||
factor.evaluateError(pose, H1);
|
||||
|
||||
Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
|
||||
[&factor](const Pose2& p) {
|
||||
return factor.evaluateError(p);},
|
||||
pose);
|
||||
CHECK(assert_equal(expectedH1, H1, 1e-6));
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -0,0 +1,243 @@
|
|||
/**
|
||||
* @file testPlanarSFMFactor.cpp
|
||||
* @date Dec 3, 2024
|
||||
* @author joel@truher.org
|
||||
* @brief unit tests for PlanarSFMFactor
|
||||
*/
|
||||
|
||||
#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/Values.h>
|
||||
#include <gtsam/slam/PlanarSFMFactor.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::C;
|
||||
using symbol_shorthand::K;
|
||||
|
||||
TEST(PlanarSFMFactor, error1) {
|
||||
// landmark is on the camera bore (facing +x)
|
||||
Point3 landmark(1, 0, 0);
|
||||
// so px is (cx, cy)
|
||||
Point2 measured(200, 200);
|
||||
// offset is identity
|
||||
Pose3 offset;
|
||||
Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
|
||||
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
|
||||
Values values;
|
||||
Pose2 pose(0, 0, 0);
|
||||
values.insert(X(0), pose);
|
||||
values.insert(C(0), offset);
|
||||
values.insert(K(0), calib);
|
||||
|
||||
PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
|
||||
|
||||
CHECK_EQUAL(2, factor.dim());
|
||||
CHECK(factor.active(values));
|
||||
std::vector<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());
|
||||
|
||||
// du/dx etc for the pose2d
|
||||
Matrix23 H1Expected = (Matrix23() <<//
|
||||
0, 200, 200,//
|
||||
0, 0, 0).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() <<//
|
||||
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(PlanarSFMFactor, 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));
|
||||
PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
|
||||
Values values;
|
||||
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() <<//
|
||||
-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() <<//
|
||||
-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));
|
||||
}
|
||||
|
||||
TEST(PlanarSFMFactor, 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));
|
||||
PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
|
||||
Values values;
|
||||
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() <<//
|
||||
-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() <<//
|
||||
-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));
|
||||
}
|
||||
|
||||
|
||||
|
||||
TEST(PlanarSFMFactor, jacobian) {
|
||||
// test many jacobians with many randoms
|
||||
|
||||
std::default_random_engine g;
|
||||
std::uniform_real_distribution<double> s(-0.3, 0.3);
|
||||
SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
|
||||
|
||||
for (int i = 0; i < 1000; ++i) {
|
||||
Point3 landmark(2 + s(g), s(g), s(g));
|
||||
Point2 measured(200 + 100*s(g), 200 + 100*s(g));
|
||||
Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
|
||||
Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
|
||||
|
||||
PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
|
||||
|
||||
Pose2 pose(s(g), s(g), s(g));
|
||||
|
||||
// actual H
|
||||
Matrix H1, H2, H3;
|
||||
factor.evaluateError(pose, offset, calib, H1, H2, H3);
|
||||
|
||||
Matrix expectedH1 = numericalDerivative31<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, 1e-6));
|
||||
CHECK(assert_equal(expectedH2, H2, 1e-6));
|
||||
CHECK(assert_equal(expectedH3, H3, 1e-6));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue