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

Merged in feature/cleanupOrientedPlane3 (pull request #104) 

Clean up some formatting, warnings, link error (?)
release/4.3a0
Frank Dellaert 2015-02-17 01:17:50 +01:00
parent cd0f8c8c57 a375d06caa
commit d617462e74
5 changed files with 1846 additions and 1837 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3316
.cproject
View File

File diff suppressed because it is too large Load Diff

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

@ -16,52 +16,46 @@
* @brief Tests the OrientedPlane3 class
*/
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
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));
}

15
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -5,7 +5,6 @@
* Author: Natesh Srinivasan
*/
#include "OrientedPlane3Factor.h"
using namespace std;
@ -13,19 +12,27 @@ using namespace std;
namespace gtsam {
//***************************************************************************
void OrientedPlane3Factor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << "OrientedPlane3Factor Factor on " << landmarkKey_ << "\n";
measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: ");
}
void OrientedPlane3DirectionPrior::print(const string& s) const {
//***************************************************************************
void OrientedPlane3DirectionPrior::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << "Prior Factor on " << landmarkKey_ << "\n";
measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: ");
}
//***************************************************************************
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);
}
//***************************************************************************

58
gtsam/slam/OrientedPlane3Factor.h
View File

@ -7,12 +7,8 @@
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Key.h>
#include <iostream>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
@ -22,8 +18,8 @@ namespace gtsam {
class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
protected:
Symbol poseSymbol_;
Symbol landmarkSymbol_;
Key poseKey_;
Key landmarkKey_;
Vector measured_coeffs_;
OrientedPlane3 measured_p_;
@ -32,32 +28,32 @@ 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 Key& pose, const Key& landmark) :
Base(noiseModel, pose, landmark), poseKey_(pose), landmarkKey_(landmark), measured_coeffs_(
z) {
measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3));
}
/// print
void print(const std::string& s="PlaneFactor") const;
virtual void print(const std::string& s = "OrientedPlane3Factor",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// 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
@ -70,21 +66,21 @@ 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));
}
/// print
void print(const std::string& s) const;
/** equals */
/// print
virtual void print(const std::string& s = "OrientedPlane3DirectionPrior",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/// equals
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
virtual Vector evaluateError(const OrientedPlane3& plane,

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

@ -16,21 +16,12 @@
* @brief Tests the OrientedPlane3Factor class
*/
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/slam/OrientedPlane3Factor.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/foreach.hpp>
@ -43,66 +34,70 @@ using namespace std;
GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
TEST (OrientedPlane3, 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);
Symbol lm_sym('p', 0);
OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
gtsam::ISAM2 isam2;
gtsam::Values new_values;
gtsam::NonlinearFactorGraph new_graph;
ISAM2 isam2;
Values new_values;
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::Vector sigmas(6);
Symbol init_sym('x', 0);
Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
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) );
PriorFactor<Pose3> pose_prior(init_sym, init_pose,
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);
gtsam::Vector sigmas3(3);
Vector sigmas3(3);
sigmas3 << 0.1, 0.1, 0.1;
gtsam::Vector test_meas0_mean(4);
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);
OrientedPlane3Factor test_meas0(test_meas0_mean,
noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
new_graph.add(test_meas0);
gtsam::Vector test_meas1_mean(4);
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);
OrientedPlane3Factor test_meas1(test_meas1_mean,
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);
ISAM2Result result = isam2.update(new_graph, new_values);
Values result_values = isam2.calculateEstimate();
OrientedPlane3 optimized_plane_landmark = result_values.at<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);
OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0);
EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
}
TEST (OrientedPlane3, 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);
Symbol lm_sym('p', 0);
OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
gtsam::ISAM2 isam2;
gtsam::Values new_values;
gtsam::NonlinearFactorGraph new_graph;
ISAM2 isam2;
Values new_values;
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()));
Symbol init_sym('x', 0);
Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
PriorFactor<Pose3> pose_prior(init_sym, init_pose,
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);
@ -110,25 +105,29 @@ TEST (OrientedPlane3, 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);
OrientedPlane3Factor test_meas0(test_meas0_mean,
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);
OrientedPlane3Factor test_meas1(test_meas1_mean,
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);
ISAM2Result result = isam2.update(new_graph, new_values);
Values result_values = isam2.calculateEstimate();
OrientedPlane3 optimized_plane_landmark = result_values.at<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);
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));
}
// *************************************************************************
TEST( OrientedPlane3DirectionPriorFactor, Constructor ) {
TEST( OrientedPlane3DirectionPrior, Constructor ) {
// Example: pitch and roll of aircraft in an ENU Cartesian frame.
// If pitch and roll are zero for an aerospace frame,
@ -138,7 +137,7 @@ TEST( OrientedPlane3DirectionPriorFactor, Constructor ) {
// Factor
Key key(1);
SharedGaussian model = gtsam::noiseModel::Diagonal::Sigmas (Vector3(0.1, 0.1, 10.0));
SharedGaussian model = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 10.0));
OrientedPlane3DirectionPrior factor(key, planeOrientation, model);
// Create a linearization point at the zero-error point
@ -146,21 +145,22 @@ TEST( OrientedPlane3DirectionPriorFactor, 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;