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

BORG formatting

release/4.3a0
dellaert 2015-02-17 00:17:36 +01:00
parent 9ab97a23b0
commit 754b770cad
4 changed files with 136 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
gtsam/geometry/tests/testOrientedPlane3.cpp
View File

@ -24,33 +24,39 @@
using namespace boost::assign; using namespace boost::assign;
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
using boost::none;
GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
//******************************************************************************* //*******************************************************************************
TEST (OrientedPlane3, transform) TEST (OrientedPlane3, transform) {
{
// Test transforming a plane to a pose // Test transforming a plane to a pose
gtsam::Pose3 pose(gtsam::Rot3::ypr (-M_PI/4.0, 0.0, 0.0), gtsam::Point3(2.0, 3.0, 4.0)); gtsam::Pose3 pose(gtsam::Rot3::ypr(-M_PI / 4.0, 0.0, 0.0),
OrientedPlane3 plane (-1 , 0, 0, 5); gtsam::Point3(2.0, 3.0, 4.0));
OrientedPlane3 expected_meas (-sqrt (2.0)/2.0, -sqrt (2.0)/2.0, 0.0, 3); OrientedPlane3 plane(-1, 0, 0, 5);
OrientedPlane3 transformed_plane = OrientedPlane3::Transform (plane, pose, boost::none, boost::none); OrientedPlane3 expected_meas(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3);
EXPECT (assert_equal (expected_meas, transformed_plane, 1e-9)); OrientedPlane3 transformed_plane = OrientedPlane3::Transform(plane, pose,
none, none);
EXPECT(assert_equal(expected_meas, transformed_plane, 1e-9));
// Test the jacobians of transform // Test the jacobians of transform
Matrix actualH1, expectedH1, actualH2, expectedH2; Matrix actualH1, expectedH1, actualH2, expectedH2;
{ {
expectedH1 = numericalDerivative11<OrientedPlane3, Pose3>(boost::bind (&OrientedPlane3::Transform, plane, _1, boost::none, boost::none), pose); expectedH1 = numericalDerivative11<OrientedPlane3, Pose3>(
boost::bind(&OrientedPlane3::Transform, plane, _1, none, none), pose);
OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, actualH1, boost::none); OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, actualH1,
EXPECT (assert_equal (expectedH1, actualH1, 1e-9)); none);
EXPECT(assert_equal(expectedH1, actualH1, 1e-9));
} }
{ {
expectedH2 = numericalDerivative11<OrientedPlane3, OrientedPlane3> (boost::bind (&OrientedPlane3::Transform, _1, pose, boost::none, boost::none), plane); expectedH2 = numericalDerivative11<OrientedPlane3, OrientedPlane3>(
boost::bind(&OrientedPlane3::Transform, _1, pose, none, none), plane);
OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, boost::none, actualH2); OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, none,
EXPECT (assert_equal (expectedH2, actualH2, 1e-9)); actualH2);
EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
} }
} }
@ -66,9 +72,9 @@ inline static Vector randomVector(const Vector& minLimits,
// Create the random vector // Create the random vector
for (size_t i = 0; i < numDims; i++) { for (size_t i = 0; i < numDims; i++) {
double range = maxLimits(i) - minLimits(i); double range = maxLimits(i) - minLimits(i);
vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i); vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i);
} }
return vector; return vector;
} }
@ -80,9 +86,9 @@ TEST(OrientedPlane3, localCoordinates_retract) {
minPlaneLimit << -1.0, -1.0, -1.0, 0.01; minPlaneLimit << -1.0, -1.0, -1.0, 0.01;
maxPlaneLimit << 1.0, 1.0, 1.0, 10.0; maxPlaneLimit << 1.0, 1.0, 1.0, 10.0;
Vector minXiLimit(3),maxXiLimit(3); Vector minXiLimit(3), maxXiLimit(3);
minXiLimit << -M_PI, -M_PI, -10.0; minXiLimit << -M_PI, -M_PI, -10.0;
maxXiLimit << M_PI, M_PI, 10.0; maxXiLimit << M_PI, M_PI, 10.0;
for (size_t i = 0; i < numIterations; i++) { for (size_t i = 0; i < numIterations; i++) {
sleep(0); sleep(0);
@ -92,7 +98,7 @@ TEST(OrientedPlane3, localCoordinates_retract) {
Vector v12 = randomVector(minXiLimit, maxXiLimit); Vector v12 = randomVector(minXiLimit, maxXiLimit);
// Magnitude of the rotation can be at most pi // Magnitude of the rotation can be at most pi
if (v12.segment<3>(0).norm () > M_PI) if (v12.segment<3>(0).norm() > M_PI)
v12.segment<3>(0) = v12.segment<3>(0) / M_PI; v12.segment<3>(0) = v12.segment<3>(0) / M_PI;
OrientedPlane3 p2 = p1.retract(v12); OrientedPlane3 p2 = p1.retract(v12);

36
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -5,7 +5,6 @@
* Author: Natesh Srinivasan * Author: Natesh Srinivasan
*/ */
#include "OrientedPlane3Factor.h" #include "OrientedPlane3Factor.h"
using namespace std; using namespace std;
@ -30,9 +29,10 @@ void OrientedPlane3DirectionPrior::print(const string& s,
//*************************************************************************** //***************************************************************************
bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
double tol) const { double tol) const {
const This* e = dynamic_cast<const This*>(&expected); const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && this->measured_p_.equals(e->measured_p_, tol); return e != NULL && Base::equals(*e, tol)
&& this->measured_p_.equals(e->measured_p_, tol);
} }
//*************************************************************************** //***************************************************************************
@ -40,21 +40,21 @@ bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane, Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane,
boost::optional<Matrix&> H) const { boost::optional<Matrix&> H) const {
if(H) { if (H) {
Matrix H_p; Matrix H_p;
Unit3 n_hat_p = measured_p_.normal(); Unit3 n_hat_p = measured_p_.normal();
Unit3 n_hat_q = plane.normal(); Unit3 n_hat_q = plane.normal();
Vector e = n_hat_p.error(n_hat_q,H_p); Vector e = n_hat_p.error(n_hat_q, H_p);
H->resize(2,3); H->resize(2, 3);
H->block <2,2>(0,0) << H_p; H->block<2, 2>(0, 0) << H_p;
H->block <2,1>(0,2) << Matrix::Zero(2, 1); H->block<2, 1>(0, 2) << Matrix::Zero(2, 1);
return e; return e;
} else { } else {
Unit3 n_hat_p = measured_p_.normal(); Unit3 n_hat_p = measured_p_.normal();
Unit3 n_hat_q = plane.normal(); Unit3 n_hat_q = plane.normal();
Vector e = n_hat_p.error(n_hat_q); Vector e = n_hat_p.error(n_hat_q);
return e; return e;
} }
} }
} }

51
gtsam/slam/OrientedPlane3Factor.h
View File

@ -26,24 +26,20 @@ protected:
Vector measured_coeffs_; Vector measured_coeffs_;
OrientedPlane3 measured_p_; OrientedPlane3 measured_p_;
typedef NoiseModelFactor2<Pose3, OrientedPlane3 > Base; typedef NoiseModelFactor2<Pose3, OrientedPlane3> Base;
public: public:
/// Constructor /// Constructor
OrientedPlane3Factor () OrientedPlane3Factor() {
{} }
/// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol
OrientedPlane3Factor (const Vector&z, const SharedGaussian& noiseModel, OrientedPlane3Factor(const Vector&z, const SharedGaussian& noiseModel,
const Symbol& pose, const Symbol& pose, const Symbol& landmark) :
const Symbol& landmark) Base(noiseModel, pose, landmark), poseSymbol_(pose), landmarkSymbol_(
: Base (noiseModel, pose, landmark), landmark), measured_coeffs_(z) {
poseSymbol_ (pose), measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3));
landmarkSymbol_ (landmark),
measured_coeffs_ (z)
{
measured_p_ = OrientedPlane3 (Unit3 (z (0), z (1), z (2)), z (3));
} }
/// print /// print
@ -52,14 +48,15 @@ public:
/// evaluateError /// evaluateError
virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::optional<Matrix&> H2 = boost::none) const boost::none) const {
{ OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1,
OrientedPlane3 predicted_plane = OrientedPlane3::Transform (plane, pose, H1, H2); H2);
Vector err(3); Vector err(3);
err << predicted_plane.error (measured_p_); err << predicted_plane.error(measured_p_);
return (err); return (err);
}; }
;
}; };
// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
@ -67,21 +64,19 @@ class OrientedPlane3DirectionPrior: public NoiseModelFactor1<OrientedPlane3> {
protected: protected:
OrientedPlane3 measured_p_; /// measured plane parameters OrientedPlane3 measured_p_; /// measured plane parameters
Key landmarkKey_; Key landmarkKey_;
typedef NoiseModelFactor1<OrientedPlane3 > Base; typedef NoiseModelFactor1<OrientedPlane3> Base;
public: public:
typedef OrientedPlane3DirectionPrior This; typedef OrientedPlane3DirectionPrior This;
/// Constructor /// Constructor
OrientedPlane3DirectionPrior () OrientedPlane3DirectionPrior() {
{} }
/// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol /// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol
OrientedPlane3DirectionPrior (Key key, const Vector&z, OrientedPlane3DirectionPrior(Key key, const Vector&z,
const SharedGaussian& noiseModel) const SharedGaussian& noiseModel) :
: Base (noiseModel, key), Base(noiseModel, key), landmarkKey_(key) {
landmarkKey_ (key) measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3));
{
measured_p_ = OrientedPlane3 (Unit3 (z (0), z (1), z (2)), z (3));
} }
/// print /// print
@ -92,7 +87,7 @@ public:
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
virtual Vector evaluateError(const OrientedPlane3& plane, virtual Vector evaluateError(const OrientedPlane3& plane,
boost::optional<Matrix&> H = boost::none) const; boost::optional<Matrix&> H = boost::none) const;
}; };
} // gtsam } // gtsam

114
gtsam/slam/tests/testOrientedPlane3Factor.cpp
View File

@ -34,89 +34,99 @@ GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
// ************************************************************************* // *************************************************************************
TEST (OrientedPlane3Factor, lm_translation_error) TEST (OrientedPlane3Factor, lm_translation_error) {
{
// Tests one pose, two measurements of the landmark that differ in range only. // Tests one pose, two measurements of the landmark that differ in range only.
// Normal along -x, 3m away // Normal along -x, 3m away
gtsam::Symbol lm_sym ('p', 0); gtsam::Symbol lm_sym('p', 0);
gtsam::OrientedPlane3 test_lm0 (-1.0, 0.0, 0.0, 3.0); gtsam::OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
gtsam::ISAM2 isam2; gtsam::ISAM2 isam2;
gtsam::Values new_values; gtsam::Values new_values;
gtsam::NonlinearFactorGraph new_graph; gtsam::NonlinearFactorGraph new_graph;
// Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose
gtsam::Symbol init_sym ('x', 0); gtsam::Symbol init_sym('x', 0);
gtsam::Pose3 init_pose (gtsam::Rot3::ypr (0.0, 0.0, 0.0), gtsam::Pose3 init_pose(gtsam::Rot3::ypr(0.0, 0.0, 0.0),
gtsam::Point3 (0.0, 0.0, 0.0)); gtsam::Point3(0.0, 0.0, 0.0));
gtsam::Vector sigmas(6); gtsam::Vector sigmas(6);
sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001;
gtsam::PriorFactor<gtsam::Pose3> pose_prior (init_sym, init_pose, gtsam::noiseModel::Diagonal::Sigmas (sigmas) ); gtsam::PriorFactor<gtsam::Pose3> pose_prior(init_sym, init_pose,
new_values.insert (init_sym, init_pose); gtsam::noiseModel::Diagonal::Sigmas(sigmas));
new_graph.add (pose_prior); new_values.insert(init_sym, init_pose);
new_graph.add(pose_prior);
// Add two landmark measurements, differing in range // Add two landmark measurements, differing in range
new_values.insert (lm_sym, test_lm0); new_values.insert(lm_sym, test_lm0);
gtsam::Vector sigmas3(3); gtsam::Vector sigmas3(3);
sigmas3 << 0.1, 0.1, 0.1; sigmas3 << 0.1, 0.1, 0.1;
gtsam::Vector test_meas0_mean(4); gtsam::Vector test_meas0_mean(4);
test_meas0_mean << -1.0, 0.0, 0.0, 3.0; test_meas0_mean << -1.0, 0.0, 0.0, 3.0;
gtsam::OrientedPlane3Factor test_meas0 (test_meas0_mean, gtsam::noiseModel::Diagonal::Sigmas (sigmas3), init_sym, lm_sym); gtsam::OrientedPlane3Factor test_meas0(test_meas0_mean,
new_graph.add (test_meas0); gtsam::noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
new_graph.add(test_meas0);
gtsam::Vector test_meas1_mean(4); gtsam::Vector test_meas1_mean(4);
test_meas1_mean << -1.0, 0.0, 0.0, 1.0; test_meas1_mean << -1.0, 0.0, 0.0, 1.0;
gtsam::OrientedPlane3Factor test_meas1 (test_meas1_mean, gtsam::noiseModel::Diagonal::Sigmas (sigmas3), init_sym, lm_sym); gtsam::OrientedPlane3Factor test_meas1(test_meas1_mean,
new_graph.add (test_meas1); gtsam::noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
new_graph.add(test_meas1);
// Optimize // Optimize
gtsam::ISAM2Result result = isam2.update (new_graph, new_values); gtsam::ISAM2Result result = isam2.update(new_graph, new_values);
gtsam::Values result_values = isam2.calculateEstimate (); gtsam::Values result_values = isam2.calculateEstimate();
gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<gtsam::OrientedPlane3>(lm_sym); gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<
gtsam::OrientedPlane3>(lm_sym);
// Given two noisy measurements of equal weight, expect result between the two // Given two noisy measurements of equal weight, expect result between the two
gtsam::OrientedPlane3 expected_plane_landmark (-1.0, 0.0, 0.0, 2.0); gtsam::OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0);
EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark)); EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
} }
// ************************************************************************* // *************************************************************************
TEST (OrientedPlane3Factor, lm_rotation_error) TEST (OrientedPlane3Factor, lm_rotation_error) {
{
// Tests one pose, two measurements of the landmark that differ in angle only. // Tests one pose, two measurements of the landmark that differ in angle only.
// Normal along -x, 3m away // Normal along -x, 3m away
gtsam::Symbol lm_sym ('p', 0); gtsam::Symbol lm_sym('p', 0);
gtsam::OrientedPlane3 test_lm0 (-1.0, 0.0, 0.0, 3.0); gtsam::OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
gtsam::ISAM2 isam2; gtsam::ISAM2 isam2;
gtsam::Values new_values; gtsam::Values new_values;
gtsam::NonlinearFactorGraph new_graph; gtsam::NonlinearFactorGraph new_graph;
// Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose
gtsam::Symbol init_sym ('x', 0); gtsam::Symbol init_sym('x', 0);
gtsam::Pose3 init_pose (gtsam::Rot3::ypr (0.0, 0.0, 0.0), gtsam::Pose3 init_pose(gtsam::Rot3::ypr(0.0, 0.0, 0.0),
gtsam::Point3 (0.0, 0.0, 0.0)); gtsam::Point3(0.0, 0.0, 0.0));
gtsam::PriorFactor<gtsam::Pose3> pose_prior (init_sym, init_pose, gtsam::noiseModel::Diagonal::Sigmas ((Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); gtsam::PriorFactor<gtsam::Pose3> pose_prior(init_sym, init_pose,
new_values.insert (init_sym, init_pose); gtsam::noiseModel::Diagonal::Sigmas(
new_graph.add (pose_prior); (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished()));
new_values.insert(init_sym, init_pose);
new_graph.add(pose_prior);
// // Add two landmark measurements, differing in angle // // Add two landmark measurements, differing in angle
new_values.insert (lm_sym, test_lm0); new_values.insert(lm_sym, test_lm0);
Vector test_meas0_mean(4); Vector test_meas0_mean(4);
test_meas0_mean << -1.0, 0.0, 0.0, 3.0; test_meas0_mean << -1.0, 0.0, 0.0, 3.0;
gtsam::OrientedPlane3Factor test_meas0 (test_meas0_mean, gtsam::noiseModel::Diagonal::Sigmas(Vector3( 0.1, 0.1, 0.1)), init_sym, lm_sym); gtsam::OrientedPlane3Factor test_meas0(test_meas0_mean,
new_graph.add (test_meas0); gtsam::noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym,
lm_sym);
new_graph.add(test_meas0);
Vector test_meas1_mean(4); Vector test_meas1_mean(4);
test_meas1_mean << 0.0, -1.0, 0.0, 3.0; test_meas1_mean << 0.0, -1.0, 0.0, 3.0;
gtsam::OrientedPlane3Factor test_meas1 (test_meas1_mean, gtsam::noiseModel::Diagonal::Sigmas (Vector3( 0.1, 0.1, 0.1)), init_sym, lm_sym); gtsam::OrientedPlane3Factor test_meas1(test_meas1_mean,
new_graph.add (test_meas1); gtsam::noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym,
lm_sym);
new_graph.add(test_meas1);
// Optimize // Optimize
gtsam::ISAM2Result result = isam2.update (new_graph, new_values); gtsam::ISAM2Result result = isam2.update(new_graph, new_values);
gtsam::Values result_values = isam2.calculateEstimate (); gtsam::Values result_values = isam2.calculateEstimate();
gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<gtsam::OrientedPlane3>(lm_sym); gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<
gtsam::OrientedPlane3>(lm_sym);
// Given two noisy measurements of equal weight, expect result between the two // Given two noisy measurements of equal weight, expect result between the two
gtsam::OrientedPlane3 expected_plane_landmark (-sqrt (2.0)/2.0, -sqrt (2.0)/2.0, 0.0, 3.0); gtsam::OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0,
EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark)); -sqrt(2.0) / 2.0, 0.0, 3.0);
EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
} }
// ************************************************************************* // *************************************************************************
@ -130,29 +140,31 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
// Factor // Factor
Key key(1); Key key(1);
SharedGaussian model = gtsam::noiseModel::Diagonal::Sigmas (Vector3(0.1, 0.1, 10.0)); SharedGaussian model = gtsam::noiseModel::Diagonal::Sigmas(
Vector3(0.1, 0.1, 10.0));
OrientedPlane3DirectionPrior factor(key, planeOrientation, model); OrientedPlane3DirectionPrior factor(key, planeOrientation, model);
// Create a linearization point at the zero-error point // Create a linearization point at the zero-error point
Vector theta1 = Vector4(0.0, 0.02, -1.2, 10.0); Vector theta1 = Vector4(0.0, 0.02, -1.2, 10.0);
Vector theta2 = Vector4(0.0, 0.1, - 0.8, 10.0); Vector theta2 = Vector4(0.0, 0.1, -0.8, 10.0);
Vector theta3 = Vector4(0.0, 0.2, -0.9, 10.0); Vector theta3 = Vector4(0.0, 0.2, -0.9, 10.0);
OrientedPlane3 T1(theta1); OrientedPlane3 T1(theta1);
OrientedPlane3 T2(theta2); OrientedPlane3 T2(theta2);
OrientedPlane3 T3(theta3); OrientedPlane3 T3(theta3);
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector,OrientedPlane3>( Matrix expectedH1 = numericalDerivative11<Vector, OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T1); boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
boost::none), T1);
Matrix expectedH2 = numericalDerivative11<Vector,OrientedPlane3>( Matrix expectedH2 = numericalDerivative11<Vector, OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T2); boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
boost::none), T2);
Matrix expectedH3 = numericalDerivative11<Vector,OrientedPlane3>( Matrix expectedH3 = numericalDerivative11<Vector, OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T3); boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
boost::none), T3);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative
Matrix actualH1, actualH2, actualH3; Matrix actualH1, actualH2, actualH3;