Merged in feature/cleanupOrientedPlane3 (pull request #104)
Clean up some formatting, warnings, link error (?)release/4.3a0
Frank Dellaert
commit
d617462e74
|
|
@ -16,53 +16,47 @@
|
|||
* @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));
|
||||
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));
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -78,9 +72,9 @@ inline static Vector randomVector(const Vector& minLimits,
|
|||
|
||||
// Create the random vector
|
||||
for (size_t i = 0; i < numDims; i++) {
|
||||
double range = maxLimits(i) - minLimits(i);
|
||||
vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i);
|
||||
}
|
||||
double range = maxLimits(i) - minLimits(i);
|
||||
vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i);
|
||||
}
|
||||
return vector;
|
||||
}
|
||||
|
||||
|
|
@ -92,9 +86,9 @@ 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);
|
||||
minXiLimit << -M_PI, -M_PI, -10.0;
|
||||
maxXiLimit << M_PI, M_PI, 10.0;
|
||||
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++) {
|
||||
|
||||
sleep(0);
|
||||
|
|
@ -104,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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
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);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
|
|
@ -33,21 +40,21 @@ bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
|
|||
Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane,
|
||||
boost::optional<Matrix&> H) const {
|
||||
|
||||
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);
|
||||
return e;
|
||||
} 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;
|
||||
}
|
||||
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);
|
||||
return e;
|
||||
} 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;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -7,57 +7,53 @@
|
|||
|
||||
#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 {
|
||||
|
||||
/**
|
||||
/**
|
||||
* Factor to measure a planar landmark from a given pose
|
||||
*/
|
||||
class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
|
||||
|
||||
protected:
|
||||
Symbol poseSymbol_;
|
||||
Symbol landmarkSymbol_;
|
||||
Key poseKey_;
|
||||
Key landmarkKey_;
|
||||
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 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_);
|
||||
err << predicted_plane.error(measured_p_);
|
||||
return (err);
|
||||
};
|
||||
}
|
||||
;
|
||||
};
|
||||
|
||||
// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
|
||||
|
|
@ -65,30 +61,30 @@ 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
|
||||
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,
|
||||
boost::optional<Matrix&> H = boost::none) const;
|
||||
boost::optional<Matrix&> H = boost::none) const;
|
||||
};
|
||||
|
||||
} // gtsam
|
||||
|
|
|
|||
|
|
@ -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,92 +34,100 @@ 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) );
|
||||
new_values.insert (init_sym, init_pose);
|
||||
new_graph.add (pose_prior);
|
||||
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);
|
||||
new_values.insert(lm_sym, test_lm0);
|
||||
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::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);
|
||||
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);
|
||||
OrientedPlane3Factor test_meas0(test_meas0_mean,
|
||||
noiseModel::Diagonal::Sigmas(sigmas3), 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);
|
||||
test_meas1_mean << -1.0, 0.0, 0.0, 1.0;
|
||||
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 (-sqrt (2.0)/2.0, -sqrt (2.0)/2.0, 0.0, 3.0);
|
||||
EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark));
|
||||
OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0);
|
||||
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.
|
||||
// If pitch and roll are zero for an aerospace frame,
|
||||
|
|
@ -138,29 +137,30 @@ 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
|
||||
Vector theta1 = Vector4(0.0, 0.02, -1.2, 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 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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue