Added OrientedPlane3 and OrientedPlane3Factor.
Alex Trevor
parent
d954cab884
commit
ebc038d6d1
|
|
@ -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,
|
||||
boost::optional<Matrix&> Hr,
|
||||
boost::optional<Matrix&> Hp)
|
||||
{
|
||||
Matrix n_hr;
|
||||
Matrix n_hp;
|
||||
Sphere2 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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector OrientedPlane3::error (const gtsam::OrientedPlane3& plane) const
|
||||
{
|
||||
Vector n_error = -n_.localCoordinates (plane.n_, Sphere2::EXPMAP);
|
||||
double d_error = d_ - plane.d_;
|
||||
return (Vector (3) << n_error (0), n_error (1), -d_error);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
OrientedPlane3 OrientedPlane3::retract(const Vector& v) const {
|
||||
// Retract the Sphere2
|
||||
Vector2 n_v (v (0), v (1));
|
||||
Sphere2 n_retracted = n_.retract (n_v, Sphere2::EXPMAP);
|
||||
double d_retracted = d_ + v (2);
|
||||
return OrientedPlane3 (n_retracted, d_retracted);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
|
||||
Vector n_local = n_.localCoordinates (y.n_, Sphere2::EXPMAP);
|
||||
double d_local = d_ - y.d_;
|
||||
return Vector (3) << n_local (0), n_local (1), -d_local;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector OrientedPlane3::planeCoefficients () const
|
||||
{
|
||||
Vector unit_vec = n_.unitVector ();
|
||||
return Vector (4) << unit_vec[0], unit_vec[1], unit_vec[2], d_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,108 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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/Sphere2.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:
|
||||
|
||||
Sphere2 n_; /// The direction of the planar normal
|
||||
double d_; /// The perpendicular distance to this plane
|
||||
|
||||
public:
|
||||
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/// Default constructor
|
||||
OrientedPlane3() :
|
||||
n_(),
|
||||
d_ (0.0){
|
||||
}
|
||||
|
||||
/// Construct from a Sphere2 and a distance
|
||||
OrientedPlane3 (const Sphere2& 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_ = Sphere2(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,
|
||||
boost::optional<Matrix&> Hr,
|
||||
boost::optional<Matrix&> Hp);
|
||||
|
||||
/// Computes the error between two poses
|
||||
Vector 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
|
||||
Vector localCoordinates(const OrientedPlane3& s) const;
|
||||
|
||||
/// Returns the plane coefficients (a, b, c, d)
|
||||
Vector planeCoefficients () const;
|
||||
|
||||
/// @}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
@ -0,0 +1,121 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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/Sphere2.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 testSphere2.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;
|
||||
Vector minPlaneLimit = Vector_(4, -1.0, -1.0, -1.0, 0.01), maxPlaneLimit =
|
||||
Vector_(4, 1.0, 1.0, 10.0);
|
||||
Vector minXiLimit = Vector_(3, -M_PI, -M_PI, -10.0), maxXiLimit = Vector_(3, 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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
/*
|
||||
* @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 <iostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Factor to measure a planar landmark from a given pose
|
||||
*/
|
||||
class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
|
||||
|
||||
protected:
|
||||
Symbol poseSymbol_;
|
||||
Symbol landmarkSymbol_;
|
||||
Vector3 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 (Sphere2 (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 error = predicted_plane.error (measured_p_);
|
||||
return (error);
|
||||
};
|
||||
};
|
||||
|
||||
} // gtsam
|
||||
|
||||
Loading…
Reference in New Issue