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

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

@ -16,53 +16,47 @@
* @brief Tests the OrientedPlane3 class * @brief Tests the OrientedPlane3 class
*/ */
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/OrientedPlane3.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 <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
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));
} }
} }
@ -78,23 +72,23 @@ 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;
} }
//******************************************************************************* //*******************************************************************************
TEST(OrientedPlane3, localCoordinates_retract) { TEST(OrientedPlane3, localCoordinates_retract) {
size_t numIterations = 10000; size_t numIterations = 10000;
gtsam::Vector minPlaneLimit(4), maxPlaneLimit(4); gtsam::Vector minPlaneLimit(4), maxPlaneLimit(4);
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);
@ -104,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);

47
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;
@ -13,19 +12,27 @@ using namespace std;
namespace gtsam { 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"; cout << "Prior Factor on " << landmarkKey_ << "\n";
measured_p_.print("Measured Plane"); measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
//*************************************************************************** //***************************************************************************
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);
} }
//*************************************************************************** //***************************************************************************
@ -33,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;
} }
} }
} }

76
gtsam/slam/OrientedPlane3Factor.h
View File

@ -7,57 +7,53 @@
#pragma once #pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/OrientedPlane3.h> #include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/Key.h>
#include <iostream>
namespace gtsam { namespace gtsam {
/** /**
* Factor to measure a planar landmark from a given pose * Factor to measure a planar landmark from a given pose
*/ */
class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> { class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
protected: protected:
Symbol poseSymbol_; Key poseKey_;
Symbol landmarkSymbol_; Key landmarkKey_;
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 Key& pose, const Key& landmark) :
const Symbol& landmark) Base(noiseModel, pose, landmark), poseKey_(pose), landmarkKey_(landmark), measured_coeffs_(
: Base (noiseModel, pose, landmark), 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
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, 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
@ -65,30 +61,30 @@ 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
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 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

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

@ -16,21 +16,12 @@
* @brief Tests the OrientedPlane3Factor class * @brief Tests the OrientedPlane3Factor class
*/ */
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/slam/OrientedPlane3Factor.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/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/nonlinear/ISAM2.h>
#include <gtsam/base/Testable.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp> #include <boost/bind.hpp>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
@ -43,92 +34,100 @@ using namespace std;
GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_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. // 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); Symbol lm_sym('p', 0);
gtsam::OrientedPlane3 test_lm0 (-1.0, 0.0, 0.0, 3.0); OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
gtsam::ISAM2 isam2; ISAM2 isam2;
gtsam::Values new_values; Values new_values;
gtsam::NonlinearFactorGraph new_graph; 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); Symbol init_sym('x', 0);
gtsam::Pose3 init_pose (gtsam::Rot3::ypr (0.0, 0.0, 0.0), Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
gtsam::Point3 (0.0, 0.0, 0.0)); 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) ); PriorFactor<Pose3> pose_prior(init_sym, init_pose,
new_values.insert (init_sym, init_pose); 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); Vector sigmas3(3);
sigmas3 << 0.1, 0.1, 0.1; 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::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);
// 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);
// 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));
}
TEST (OrientedPlane3, 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::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);
// // Add two landmark measurements, differing in angle
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); OrientedPlane3Factor test_meas0(test_meas0_mean,
new_graph.add (test_meas0); noiseModel::Diagonal::Sigmas(sigmas3), 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 << -1.0, 0.0, 0.0, 1.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,
new_graph.add (test_meas1); noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
new_graph.add(test_meas1);
// Optimize // Optimize
gtsam::ISAM2Result result = isam2.update (new_graph, new_values); ISAM2Result result = isam2.update(new_graph, new_values);
gtsam::Values result_values = isam2.calculateEstimate (); Values result_values = isam2.calculateEstimate();
gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<gtsam::OrientedPlane3>(lm_sym); OrientedPlane3 optimized_plane_landmark = result_values.at<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); 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( OrientedPlane3DirectionPriorFactor, Constructor ) { TEST (OrientedPlane3Factor, lm_rotation_error) {
// Tests one pose, two measurements of the landmark that differ in angle only.
// Normal along -x, 3m away
Symbol lm_sym('p', 0);
OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
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
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);
// // Add two landmark measurements, differing in angle
new_values.insert(lm_sym, test_lm0);
Vector test_meas0_mean(4);
test_meas0_mean << -1.0, 0.0, 0.0, 3.0;
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;
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
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
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( OrientedPlane3DirectionPrior, Constructor ) {
// Example: pitch and roll of aircraft in an ENU Cartesian frame. // Example: pitch and roll of aircraft in an ENU Cartesian frame.
// If pitch and roll are zero for an aerospace frame, // If pitch and roll are zero for an aerospace frame,
@ -138,29 +137,30 @@ TEST( OrientedPlane3DirectionPriorFactor, Constructor ) {
// Factor // Factor
Key key(1); 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); 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;