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

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

@ -24,32 +24,38 @@
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));
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);
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);
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);
OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, none,
actualH2);
EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
}

4
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);
}
//***************************************************************************

35
gtsam/slam/OrientedPlane3Factor.h
View File

@ -31,18 +31,14 @@ protected:
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)
{
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));
}
@ -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_);
return (err);
};
}
;
};
// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
@ -72,15 +69,13 @@ 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)
{
const SharedGaussian& noiseModel) :
Base(noiseModel, key), landmarkKey_(key) {
measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3));
}

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

@ -34,8 +34,7 @@ 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);
@ -51,7 +50,8 @@ TEST (OrientedPlane3Factor, lm_translation_error)
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) );
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);
@ -61,17 +61,20 @@ TEST (OrientedPlane3Factor, lm_translation_error)
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);
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);
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::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);
@ -79,8 +82,7 @@ TEST (OrientedPlane3Factor, lm_translation_error)
}
// *************************************************************************
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);
@ -94,7 +96,9 @@ TEST (OrientedPlane3Factor, lm_rotation_error)
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()));
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);
@ -102,20 +106,26 @@ TEST (OrientedPlane3Factor, lm_rotation_error)
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);
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);
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::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);
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,7 +140,8 @@ 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
@ -138,21 +149,22 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
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);
boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1,
boost::none), T1);
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>(
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
Matrix actualH1, actualH2, actualH3;