Merged in feature/oriented_plane3 (pull request #98)
3D planes and related data structsrelease/4.3a0
Frank Dellaert
commit
acf8ec0963
|
|
@ -0,0 +1,103 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file OrientedPlane3.cpp
|
||||
* @date Dec 19, 2013
|
||||
* @author Alex Trevor
|
||||
* @brief A plane, represented by a normal direction and perpendicular distance
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/OrientedPlane3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// The print fuction
|
||||
void OrientedPlane3::print(const std::string& s) const {
|
||||
Vector coeffs = planeCoefficients();
|
||||
cout << s << " : " << coeffs << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
OrientedPlane3 OrientedPlane3::Transform(const gtsam::OrientedPlane3& plane,
|
||||
const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr,
|
||||
OptionalJacobian<3, 3> Hp) {
|
||||
Matrix n_hr;
|
||||
Matrix n_hp;
|
||||
Unit3 n_rotated = xr.rotation().unrotate(plane.n_, n_hr, n_hp);
|
||||
|
||||
Vector n_unit = plane.n_.unitVector();
|
||||
Vector unit_vec = n_rotated.unitVector();
|
||||
double pred_d = n_unit.dot(xr.translation().vector()) + plane.d_;
|
||||
OrientedPlane3 transformed_plane(unit_vec(0), unit_vec(1), unit_vec(2),
|
||||
pred_d);
|
||||
|
||||
if (Hr) {
|
||||
*Hr = gtsam::zeros(3, 6);
|
||||
(*Hr).block<2, 3>(0, 0) = n_hr;
|
||||
(*Hr).block<1, 3>(2, 3) = unit_vec;
|
||||
}
|
||||
if (Hp) {
|
||||
Vector xrp = xr.translation().vector();
|
||||
Matrix n_basis = plane.n_.basis();
|
||||
Vector hpp = n_basis.transpose() * xrp;
|
||||
*Hp = gtsam::zeros(3, 3);
|
||||
(*Hp).block<2, 2>(0, 0) = n_hp;
|
||||
(*Hp).block<1, 2>(2, 0) = hpp;
|
||||
(*Hp)(2, 2) = 1;
|
||||
}
|
||||
|
||||
return (transformed_plane);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const {
|
||||
Vector2 n_error = -n_.localCoordinates(plane.n_);
|
||||
double d_error = d_ - plane.d_;
|
||||
Vector3 e;
|
||||
e << n_error(0), n_error(1), d_error;
|
||||
return (e);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
OrientedPlane3 OrientedPlane3::retract(const Vector& v) const {
|
||||
// Retract the Unit3
|
||||
Vector2 n_v(v(0), v(1));
|
||||
Unit3 n_retracted = n_.retract(n_v);
|
||||
double d_retracted = d_ + v(2);
|
||||
return OrientedPlane3(n_retracted, d_retracted);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
|
||||
Vector n_local = n_.localCoordinates(y.n_);
|
||||
double d_local = d_ - y.d_;
|
||||
Vector3 e;
|
||||
e << n_local(0), n_local(1), -d_local;
|
||||
return e;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 OrientedPlane3::planeCoefficients() const {
|
||||
Vector unit_vec = n_.unitVector();
|
||||
Vector3 a;
|
||||
a << unit_vec[0], unit_vec[1], unit_vec[2], d_;
|
||||
return a;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,116 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file OrientedPlane3.h
|
||||
* @date Dec 19, 2013
|
||||
* @author Alex Trevor
|
||||
* @author Frank Dellaert
|
||||
* @brief An infinite plane, represented by a normal direction and perpendicular distance
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/DerivedValue.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// Represents an infinite plane in 3D.
|
||||
class OrientedPlane3: public DerivedValue<OrientedPlane3> {
|
||||
|
||||
private:
|
||||
|
||||
Unit3 n_; /// The direction of the planar normal
|
||||
double d_; /// The perpendicular distance to this plane
|
||||
|
||||
public:
|
||||
enum {
|
||||
dimension = 3
|
||||
};
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/// Default constructor
|
||||
OrientedPlane3() :
|
||||
n_(), d_(0.0) {
|
||||
}
|
||||
|
||||
/// Construct from a Unit3 and a distance
|
||||
OrientedPlane3(const Unit3& s, double d) :
|
||||
n_(s), d_(d) {
|
||||
}
|
||||
|
||||
OrientedPlane3(Vector vec) :
|
||||
n_(vec(0), vec(1), vec(2)), d_(vec(3)) {
|
||||
}
|
||||
|
||||
/// Construct from a, b, c, d
|
||||
OrientedPlane3(double a, double b, double c, double d) {
|
||||
Point3 p(a, b, c);
|
||||
n_ = Unit3(p);
|
||||
d_ = d;
|
||||
}
|
||||
|
||||
/// The print fuction
|
||||
void print(const std::string& s = std::string()) const;
|
||||
|
||||
/// The equals function with tolerance
|
||||
bool equals(const OrientedPlane3& s, double tol = 1e-9) const {
|
||||
return (n_.equals(s.n_, tol) && (fabs(d_ - s.d_) < tol));
|
||||
}
|
||||
|
||||
/// Transforms a plane to the specified pose
|
||||
static OrientedPlane3 Transform(const gtsam::OrientedPlane3& plane,
|
||||
const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none,
|
||||
OptionalJacobian<3, 3> Hp = boost::none);
|
||||
|
||||
/// Computes the error between two poses
|
||||
Vector3 error(const gtsam::OrientedPlane3& plane) const;
|
||||
|
||||
/// Dimensionality of tangent space = 3 DOF
|
||||
inline static size_t Dim() {
|
||||
return 3;
|
||||
}
|
||||
|
||||
/// Dimensionality of tangent space = 3 DOF
|
||||
inline size_t dim() const {
|
||||
return 3;
|
||||
}
|
||||
|
||||
/// The retract function
|
||||
OrientedPlane3 retract(const Vector& v) const;
|
||||
|
||||
/// The local coordinates function
|
||||
Vector3 localCoordinates(const OrientedPlane3& s) const;
|
||||
|
||||
/// Returns the plane coefficients (a, b, c, d)
|
||||
Vector3 planeCoefficients() const;
|
||||
|
||||
inline Unit3 normal() const {
|
||||
return n_;
|
||||
}
|
||||
|
||||
/// @}
|
||||
};
|
||||
|
||||
template<> struct traits<OrientedPlane3> : public internal::Manifold<
|
||||
OrientedPlane3> {
|
||||
};
|
||||
|
||||
template<> struct traits<const OrientedPlane3> : public internal::Manifold<
|
||||
OrientedPlane3> {
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
@ -108,7 +108,7 @@ Matrix3 Unit3::skew() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
|
||||
Vector Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
|
||||
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
|
||||
Matrix23 Bt = basis().transpose();
|
||||
Vector2 xi = Bt * q.p_.vector();
|
||||
|
|
|
|||
|
|
@ -38,7 +38,9 @@ private:
|
|||
|
||||
public:
|
||||
|
||||
enum { dimension = 2 };
|
||||
enum {
|
||||
dimension = 2
|
||||
};
|
||||
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
|
@ -65,8 +67,8 @@ public:
|
|||
}
|
||||
|
||||
/// Named constructor from Point3 with optional Jacobian
|
||||
static Unit3 FromPoint3(const Point3& point, OptionalJacobian<2,3> H =
|
||||
boost::none);
|
||||
static Unit3 FromPoint3(const Point3& point, //
|
||||
OptionalJacobian<2, 3> H = boost::none);
|
||||
|
||||
/// Random direction, using boost::uniform_on_sphere
|
||||
static Unit3 Random(boost::mt19937 & rng);
|
||||
|
|
@ -99,24 +101,29 @@ public:
|
|||
Matrix3 skew() const;
|
||||
|
||||
/// Return unit-norm Point3
|
||||
const Point3& point3(OptionalJacobian<3,2> H = boost::none) const {
|
||||
const Point3& point3(OptionalJacobian<3, 2> H = boost::none) const {
|
||||
if (H)
|
||||
*H = basis();
|
||||
return p_;
|
||||
}
|
||||
|
||||
/// Return unit-norm Vector
|
||||
Vector unitVector(boost::optional<Matrix&> H = boost::none) const {
|
||||
if (H)
|
||||
*H = basis();
|
||||
return (p_.vector());
|
||||
}
|
||||
|
||||
/// Return scaled direction as Point3
|
||||
friend Point3 operator*(double s, const Unit3& d) {
|
||||
return s * d.p_;
|
||||
}
|
||||
|
||||
/// Signed, vector-valued error between two directions
|
||||
Vector2 error(const Unit3& q,
|
||||
OptionalJacobian<2,2> H = boost::none) const;
|
||||
Vector error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const;
|
||||
|
||||
/// Distance between two directions
|
||||
double distance(const Unit3& q,
|
||||
OptionalJacobian<1,2> H = boost::none) const;
|
||||
double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
@ -153,25 +160,27 @@ private:
|
|||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(p_);
|
||||
// homebrew serialize Eigen Matrix
|
||||
ar & boost::serialization::make_nvp("B11", (*B_)(0,0));
|
||||
ar & boost::serialization::make_nvp("B12", (*B_)(0,1));
|
||||
ar & boost::serialization::make_nvp("B21", (*B_)(1,0));
|
||||
ar & boost::serialization::make_nvp("B22", (*B_)(1,1));
|
||||
ar & boost::serialization::make_nvp("B31", (*B_)(2,0));
|
||||
ar & boost::serialization::make_nvp("B32", (*B_)(2,1));
|
||||
}
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(p_);
|
||||
// homebrew serialize Eigen Matrix
|
||||
ar & boost::serialization::make_nvp("B11", (*B_)(0, 0));
|
||||
ar & boost::serialization::make_nvp("B12", (*B_)(0, 1));
|
||||
ar & boost::serialization::make_nvp("B21", (*B_)(1, 0));
|
||||
ar & boost::serialization::make_nvp("B22", (*B_)(1, 1));
|
||||
ar & boost::serialization::make_nvp("B31", (*B_)(2, 0));
|
||||
ar & boost::serialization::make_nvp("B32", (*B_)(2, 1));
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
// Define GTSAM traits
|
||||
template <> struct traits<Unit3> : public internal::Manifold<Unit3> {};
|
||||
template<> struct traits<Unit3> : public internal::Manifold<Unit3> {
|
||||
};
|
||||
|
||||
template <> struct traits<const Unit3> : public internal::Manifold<Unit3> {};
|
||||
template<> struct traits<const Unit3> : public internal::Manifold<Unit3> {
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,125 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file testOrientedPlane3.cpp
|
||||
* @date Dec 19, 2012
|
||||
* @author Alex Trevor
|
||||
* @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;
|
||||
|
||||
GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
|
||||
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
|
||||
|
||||
//*******************************************************************************
|
||||
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));
|
||||
|
||||
// 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);
|
||||
|
||||
OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, actualH1, boost::none);
|
||||
EXPECT (assert_equal (expectedH1, actualH1, 1e-9));
|
||||
}
|
||||
{
|
||||
expectedH2 = numericalDerivative11<OrientedPlane3, OrientedPlane3> (boost::bind (&OrientedPlane3::Transform, _1, pose, boost::none, boost::none), plane);
|
||||
|
||||
OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, boost::none, actualH2);
|
||||
EXPECT (assert_equal (expectedH2, actualH2, 1e-9));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
// Returns a random vector -- copied from testUnit3.cpp
|
||||
inline static Vector randomVector(const Vector& minLimits,
|
||||
const Vector& maxLimits) {
|
||||
|
||||
// Get the number of dimensions and create the return vector
|
||||
size_t numDims = dim(minLimits);
|
||||
Vector vector = zero(numDims);
|
||||
|
||||
// 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);
|
||||
}
|
||||
return vector;
|
||||
}
|
||||
|
||||
//*******************************************************************************
|
||||
TEST(OrientedPlane3, localCoordinates_retract) {
|
||||
|
||||
size_t numIterations = 10000;
|
||||
gtsam::Vector minPlaneLimit(4), maxPlaneLimit(4);
|
||||
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;
|
||||
for (size_t i = 0; i < numIterations; i++) {
|
||||
|
||||
sleep(0);
|
||||
|
||||
// Create a Plane
|
||||
OrientedPlane3 p1(randomVector(minPlaneLimit, maxPlaneLimit));
|
||||
Vector v12 = randomVector(minXiLimit, maxXiLimit);
|
||||
|
||||
// Magnitude of the rotation can be at most pi
|
||||
if (v12.segment<3>(0).norm () > M_PI)
|
||||
v12.segment<3>(0) = v12.segment<3>(0) / M_PI;
|
||||
OrientedPlane3 p2 = p1.retract(v12);
|
||||
|
||||
// Check if the local coordinates and retract return the same results.
|
||||
Vector actual_v12 = p1.localCoordinates(p2);
|
||||
EXPECT(assert_equal(v12, actual_v12, 1e-3));
|
||||
OrientedPlane3 actual_p2 = p1.retract(actual_v12);
|
||||
EXPECT(assert_equal(p2, actual_p2, 1e-3));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
srand(time(NULL));
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -886,6 +886,7 @@ namespace gtsam {
|
|||
typedef noiseModel::Gaussian::shared_ptr SharedGaussian;
|
||||
typedef noiseModel::Diagonal::shared_ptr SharedDiagonal;
|
||||
typedef noiseModel::Constrained::shared_ptr SharedConstrained;
|
||||
typedef noiseModel::Isotropic::shared_ptr SharedIsotropic;
|
||||
|
||||
/// traits
|
||||
template<> struct traits<noiseModel::Gaussian> : public Testable<noiseModel::Gaussian> {};
|
||||
|
|
|
|||
|
|
@ -0,0 +1,54 @@
|
|||
/*
|
||||
* OrientedPlane3Factor.cpp
|
||||
*
|
||||
* Created on: Jan 29, 2014
|
||||
* Author: Natesh Srinivasan
|
||||
*/
|
||||
|
||||
|
||||
#include "OrientedPlane3Factor.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
//***************************************************************************
|
||||
|
||||
void OrientedPlane3DirectionPrior::print(const string& s) 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);
|
||||
}
|
||||
|
||||
//***************************************************************************
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,95 @@
|
|||
/*
|
||||
* @file OrientedPlane3Factor.cpp
|
||||
* @brief OrientedPlane3 Factor class
|
||||
* @author Alex Trevor
|
||||
* @date December 22, 2013
|
||||
*/
|
||||
|
||||
#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>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Factor to measure a planar landmark from a given pose
|
||||
*/
|
||||
class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
|
||||
|
||||
protected:
|
||||
Symbol poseSymbol_;
|
||||
Symbol landmarkSymbol_;
|
||||
Vector measured_coeffs_;
|
||||
OrientedPlane3 measured_p_;
|
||||
|
||||
typedef NoiseModelFactor2<Pose3, OrientedPlane3 > Base;
|
||||
|
||||
public:
|
||||
|
||||
/// Constructor
|
||||
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));
|
||||
}
|
||||
|
||||
/// print
|
||||
void print(const std::string& s="PlaneFactor") const;
|
||||
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);
|
||||
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
|
||||
class OrientedPlane3DirectionPrior: public NoiseModelFactor1<OrientedPlane3> {
|
||||
protected:
|
||||
OrientedPlane3 measured_p_; /// measured plane parameters
|
||||
Key landmarkKey_;
|
||||
typedef NoiseModelFactor1<OrientedPlane3 > Base;
|
||||
public:
|
||||
|
||||
typedef OrientedPlane3DirectionPrior This;
|
||||
/// Constructor
|
||||
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));
|
||||
}
|
||||
/// print
|
||||
void print(const std::string& s) 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;
|
||||
};
|
||||
|
||||
} // gtsam
|
||||
|
||||
|
|
@ -0,0 +1,183 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file testOrientedPlane3.cpp
|
||||
* @date Dec 19, 2012
|
||||
* @author Alex Trevor
|
||||
* @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/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;
|
||||
|
||||
GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
|
||||
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
|
||||
|
||||
TEST (OrientedPlane3, 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::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::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);
|
||||
|
||||
// Add two landmark measurements, differing in range
|
||||
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::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);
|
||||
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);
|
||||
|
||||
// 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 (-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 ) {
|
||||
|
||||
// Example: pitch and roll of aircraft in an ENU Cartesian frame.
|
||||
// If pitch and roll are zero for an aerospace frame,
|
||||
// that means Z is pointing down, i.e., direction of Z = (0,0,-1)
|
||||
|
||||
Vector planeOrientation = (Vector(4) << 0.0, 0.0, -1.0, 10.0).finished(); // all vertical planes directly facing the origin
|
||||
|
||||
// Factor
|
||||
Key key(1);
|
||||
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 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 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);
|
||||
|
||||
// Use the factor to calculate the derivative
|
||||
Matrix actualH1, actualH2, actualH3;
|
||||
factor.evaluateError(T1, actualH1);
|
||||
factor.evaluateError(T2, actualH2);
|
||||
factor.evaluateError(T3, actualH3);
|
||||
|
||||
// Verify we get the expected error
|
||||
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
|
||||
EXPECT(assert_equal(expectedH2, actualH2, 1e-8));
|
||||
EXPECT(assert_equal(expectedH3, actualH3, 1e-8));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
srand(time(NULL));
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue