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

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

@ -24,33 +24,39 @@
using namespace boost::assign;
using namespace gtsam;
using namespace std;
using boost::none;
GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
//*******************************************************************************
TEST (OrientedPlane3, transform)
{
TEST (OrientedPlane3, transform) {
// 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));
OrientedPlane3 plane (-1 , 0, 0, 5);
OrientedPlane3 expected_meas (-sqrt (2.0)/2.0, -sqrt (2.0)/2.0, 0.0, 3);
OrientedPlane3 transformed_plane = OrientedPlane3::Transform (plane, pose, boost::none, boost::none);
EXPECT (assert_equal (expected_meas, transformed_plane, 1e-9));
gtsam::Pose3 pose(gtsam::Rot3::ypr(-M_PI / 4.0, 0.0, 0.0),
gtsam::Point3(2.0, 3.0, 4.0));
OrientedPlane3 plane(-1, 0, 0, 5);
OrientedPlane3 expected_meas(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3);
OrientedPlane3 transformed_plane = OrientedPlane3::Transform(plane, pose,
none, none);
EXPECT(assert_equal(expected_meas, transformed_plane, 1e-9));
// Test the jacobians of transform
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);
EXPECT (assert_equal (expectedH1, actualH1, 1e-9));
OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, actualH1,
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);
EXPECT (assert_equal (expectedH2, actualH2, 1e-9));
OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, none,
actualH2);
EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
}
}
@ -80,7 +86,7 @@ TEST(OrientedPlane3, localCoordinates_retract) {
minPlaneLimit << -1.0, -1.0, -1.0, 0.01;
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;
maxXiLimit << M_PI, M_PI, 10.0;
for (size_t i = 0; i < numIterations; i++) {
@ -92,7 +98,7 @@ TEST(OrientedPlane3, localCoordinates_retract) {
Vector v12 = randomVector(minXiLimit, maxXiLimit);
// 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;
OrientedPlane3 p2 = p1.retract(v12);

18
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -5,7 +5,6 @@
* Author: Natesh Srinivasan
*/
#include "OrientedPlane3Factor.h"
using namespace std;
@ -32,7 +31,8 @@ void OrientedPlane3DirectionPrior::print(const string& s,
bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
double tol) const {
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,
boost::optional<Matrix&> H) const {
if(H) {
if (H) {
Matrix H_p;
Unit3 n_hat_p = measured_p_.normal();
Unit3 n_hat_q = plane.normal();
Vector e = n_hat_p.error(n_hat_q,H_p);
H->resize(2,3);
H->block <2,2>(0,0) << H_p;
H->block <2,1>(0,2) << Matrix::Zero(2, 1);
Vector e = n_hat_p.error(n_hat_q, H_p);
H->resize(2, 3);
H->block<2, 2>(0, 0) << H_p;
H->block<2, 1>(0, 2) << Matrix::Zero(2, 1);
return e;
} else {
} else {
Unit3 n_hat_p = measured_p_.normal();
Unit3 n_hat_q = plane.normal();
Vector e = n_hat_p.error(n_hat_q);
return e;
}
}
}
}

49
gtsam/slam/OrientedPlane3Factor.h
View File

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

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

@ -34,89 +34,99 @@ GTSAM_CONCEPT_TESTABLE_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.
// Normal along -x, 3m away
gtsam::Symbol lm_sym ('p', 0);
gtsam::OrientedPlane3 test_lm0 (-1.0, 0.0, 0.0, 3.0);
gtsam::Symbol lm_sym('p', 0);
gtsam::OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
gtsam::ISAM2 isam2;
gtsam::Values new_values;
gtsam::NonlinearFactorGraph new_graph;
// 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::Pose3 init_pose (gtsam::Rot3::ypr (0.0, 0.0, 0.0),
gtsam::Point3 (0.0, 0.0, 0.0));
gtsam::Symbol init_sym('x', 0);
gtsam::Pose3 init_pose(gtsam::Rot3::ypr(0.0, 0.0, 0.0),
gtsam::Point3(0.0, 0.0, 0.0));
gtsam::Vector sigmas(6);
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) );
new_values.insert (init_sym, init_pose);
new_graph.add (pose_prior);
gtsam::PriorFactor<gtsam::Pose3> pose_prior(init_sym, init_pose,
gtsam::noiseModel::Diagonal::Sigmas(sigmas));
new_values.insert(init_sym, init_pose);
new_graph.add(pose_prior);
// 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);
sigmas3 << 0.1, 0.1, 0.1;
gtsam::Vector test_meas0_mean(4);
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);
new_graph.add (test_meas0);
gtsam::OrientedPlane3Factor test_meas0(test_meas0_mean,
gtsam::noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
new_graph.add(test_meas0);
gtsam::Vector test_meas1_mean(4);
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);
new_graph.add (test_meas1);
gtsam::OrientedPlane3Factor test_meas1(test_meas1_mean,
gtsam::noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
new_graph.add(test_meas1);
// Optimize
gtsam::ISAM2Result result = isam2.update (new_graph, new_values);
gtsam::Values result_values = isam2.calculateEstimate ();
gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<gtsam::OrientedPlane3>(lm_sym);
gtsam::ISAM2Result result = isam2.update(new_graph, new_values);
gtsam::Values result_values = isam2.calculateEstimate();
gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<
gtsam::OrientedPlane3>(lm_sym);
// 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);
EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark));
gtsam::OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0);
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.
// Normal along -x, 3m away
gtsam::Symbol lm_sym ('p', 0);
gtsam::OrientedPlane3 test_lm0 (-1.0, 0.0, 0.0, 3.0);
gtsam::Symbol lm_sym('p', 0);
gtsam::OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
gtsam::ISAM2 isam2;
gtsam::Values new_values;
gtsam::NonlinearFactorGraph new_graph;
// 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::Pose3 init_pose (gtsam::Rot3::ypr (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()));
new_values.insert (init_sym, init_pose);
new_graph.add (pose_prior);
gtsam::Symbol init_sym('x', 0);
gtsam::Pose3 init_pose(gtsam::Rot3::ypr(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()));
new_values.insert(init_sym, init_pose);
new_graph.add(pose_prior);
// // 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);
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);
new_graph.add (test_meas0);
gtsam::OrientedPlane3Factor test_meas0(test_meas0_mean,
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);
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);
new_graph.add (test_meas1);
gtsam::OrientedPlane3Factor test_meas1(test_meas1_mean,
gtsam::noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym,
lm_sym);
new_graph.add(test_meas1);
// Optimize
gtsam::ISAM2Result result = isam2.update (new_graph, new_values);
gtsam::Values result_values = isam2.calculateEstimate ();
gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<gtsam::OrientedPlane3>(lm_sym);
gtsam::ISAM2Result result = isam2.update(new_graph, new_values);
gtsam::Values result_values = isam2.calculateEstimate();
gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<
gtsam::OrientedPlane3>(lm_sym);
// 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);
EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark));
gtsam::OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0,
-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
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);
// Create a linearization point at the zero-error point
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);
OrientedPlane3 T1(theta1);
OrientedPlane3 T2(theta2);
OrientedPlane3 T3(theta3);
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector,OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T1);
Matrix expectedH1 = numericalDerivative11<Vector, OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
boost::none), T1);
Matrix expectedH2 = numericalDerivative11<Vector,OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T2);
Matrix expectedH2 = numericalDerivative11<Vector, OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
boost::none), T2);
Matrix expectedH3 = numericalDerivative11<Vector,OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T3);
Matrix expectedH3 = numericalDerivative11<Vector, OrientedPlane3>(
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
boost::none), T3);
// Use the factor to calculate the derivative
Matrix actualH1, actualH2, actualH3;