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

Merged in feature/updateOrientedPlane3 (pull request #165) 

Update the OrientedPlane3
release/4.3a0
Frank Dellaert 2015-06-25 10:02:11 -07:00
parent a933b404f3 b569a4ab92
commit 6895b75ecd
7 changed files with 141 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/geometry/Cal3Bundler.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {

1
gtsam/geometry/Cal3DS2.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Cal3DS2_Base.h>
namespace gtsam {

1
gtsam/geometry/Cal3Unified.h
View File

@ -23,7 +23,6 @@
#pragma once
#include <gtsam/geometry/Cal3DS2_Base.h>
#include <gtsam/base/DerivedValue.h>
namespace gtsam {

70
gtsam/geometry/OrientedPlane3.cpp
View File

@ -13,6 +13,7 @@
* @file OrientedPlane3.cpp
* @date Dec 19, 2013
* @author Alex Trevor
* @author Zhaoyang Lv
* @brief A plane, represented by a normal direction and perpendicular distance
*/
@ -25,79 +26,54 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
/// The print fuction
void OrientedPlane3::print(const std::string& s) const {
Vector coeffs = planeCoefficients();
void OrientedPlane3::print(const string& s) const {
Vector4 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);
OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3> Hp,
OptionalJacobian<3, 6> Hr) const {
Matrix23 D_rotated_plane;
Matrix22 D_rotated_pose;
Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose);
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);
Vector3 unit_vec = n_rotated.unitVector();
double pred_d = n_.unitVector().dot(xr.translation().vector()) + d_;
if (Hr) {
*Hr = gtsam::zeros(3, 6);
(*Hr).block<2, 3>(0, 0) = n_hr;
(*Hr).block<1, 3>(2, 3) = unit_vec;
*Hr = zeros(3, 6);
Hr->block<2, 3>(0, 0) = D_rotated_plane;
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;
Vector2 hpp = n_.basis().transpose() * xr.translation().vector();
*Hp = Z_3x3;
Hp->block<2, 2>(0, 0) = D_rotated_pose;
Hp->block<1, 2>(2, 0) = hpp;
(*Hp)(2, 2) = 1;
}
return (transformed_plane);
return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d);
}
/* ************************************************************************* */
Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const {
Vector3 OrientedPlane3::error(const 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);
return Vector3(n_error(0), n_error(1), d_ - plane.d_);
}
/* ************************************************************************* */
OrientedPlane3 OrientedPlane3::retract(const Vector3& 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);
return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2));
}
/* ************************************************************************* */
Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
Vector2 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;
return Vector3(n_local(0), n_local(1), -d_local);
}
/* ************************************************************************* */
Vector3 OrientedPlane3::planeCoefficients() const {
Vector unit_vec = n_.unitVector();
Vector3 a;
a << unit_vec[0], unit_vec[1], unit_vec[2], d_;
return a;
}
/* ************************************************************************* */
}

83
gtsam/geometry/OrientedPlane3.h
View File

@ -14,6 +14,7 @@
* @date Dec 19, 2013
* @author Alex Trevor
* @author Frank Dellaert
* @author Zhaoyang Lv
* @brief An infinite plane, represented by a normal direction and perpendicular distance
*/
@ -22,46 +23,56 @@
#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> {
/**
* @brief Represents an infinite plane in 3D, which is composed of a planar normal and its
* perpendicular distance to the origin.
* Currently it provides a transform of the plane, and a norm 1 differencing of two planes.
* Refer to Trevor12iros for more math details.
*/
class GTSAM_EXPORT OrientedPlane3 {
private:
Unit3 n_; /// The direction of the planar normal
double d_; /// The perpendicular distance to this plane
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) {
n_(), d_(0.0) {
}
/// Construct from a Unit3 and a distance
OrientedPlane3(const Unit3& s, double d) :
n_(s), d_(d) {
n_(s), d_(d) {
}
/// Construct from a vector of plane coefficients
OrientedPlane3(const Vector4& vec) :
n_(vec(0), vec(1), vec(2)), d_(vec(3)) {
n_(vec(0), vec(1), vec(2)), d_(vec(3)) {
}
/// Construct from a, b, c, d
/// Construct from four numbers of plane coeffcients (a, b, c, d)
OrientedPlane3(double a, double b, double c, double d) {
Point3 p(a, b, c);
n_ = Unit3(p);
d_ = d;
}
/// @}
/// @name Testable
/// @{
/// The print function
void print(const std::string& s = std::string()) const;
@ -70,13 +81,38 @@ public:
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;
/** Transforms a plane to the specified pose
* @param xr a transformation in current coordiante
* @param Hp optional Jacobian wrpt the destination plane
* @param Hr optional jacobian wrpt the pose transformation
* @return the transformed plane
*/
OrientedPlane3 transform(const Pose3& xr,
OptionalJacobian<3, 3> Hp = boost::none,
OptionalJacobian<3, 6> Hr = boost::none) const;
/**
* @deprecated the static method has wrong Jacobian order,
* please use the member method transform()
* @param The raw plane
* @param xr a transformation in current coordiante
* @param Hr optional jacobian wrpt the pose transformation
* @param Hp optional Jacobian wrpt the destination plane
* @return the transformed plane
*/
static OrientedPlane3 Transform(const OrientedPlane3& plane,
const Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none,
OptionalJacobian<3, 3> Hp = boost::none) {
return plane.transform(xr, Hp, Hr);
}
/** Computes the error between two planes.
* The error is a norm 1 difference in tangent space.
* @param the other plane
*/
Vector3 error(const OrientedPlane3& plane) const;
/// Dimensionality of tangent space = 3 DOF
inline static size_t Dim() {
@ -94,22 +130,31 @@ public:
/// The local coordinates function
Vector3 localCoordinates(const OrientedPlane3& s) const;
/// Returns the plane coefficients (a, b, c, d)
Vector3 planeCoefficients() const;
/// Returns the plane coefficients
inline Vector4 planeCoefficients() const {
Vector3 unit_vec = n_.unitVector();
return Vector4(unit_vec[0], unit_vec[1], unit_vec[2], d_);
}
/// Return the normal
inline Unit3 normal() const {
return n_;
}
/// Return the perpendicular distance to the origin
inline double distance() const {
return d_;
}
/// @}
};
template<> struct traits<OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
OrientedPlane3> {
};
template<> struct traits<const OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
OrientedPlane3> {
};
} // namespace gtsam

2
gtsam/geometry/Unit3.h
View File

@ -22,7 +22,7 @@
#include <gtsam/base/Manifold.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/DerivedValue.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/optional.hpp>

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

@ -13,6 +13,7 @@
* @file testOrientedPlane3.cpp
* @date Dec 19, 2012
* @author Alex Trevor
* @author Zhaoyang Lv
* @brief Tests the OrientedPlane3 class
*/
@ -30,39 +31,67 @@ GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
//*******************************************************************************
TEST (OrientedPlane3, getMethods) {
Vector4 c;
c << -1, 0, 0, 5;
OrientedPlane3 plane1(c);
OrientedPlane3 plane2(c[0], c[1], c[2], c[3]);
Vector4 coefficient1 = plane1.planeCoefficients();
double distance1 = plane1.distance();
EXPECT(assert_equal(coefficient1, c, 1e-8));
EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane1.normal().unitVector()));
EXPECT_DOUBLES_EQUAL(distance1, 5, 1e-8);
Vector4 coefficient2 = plane2.planeCoefficients();
double distance2 = plane2.distance();
EXPECT(assert_equal(coefficient2, c, 1e-8));
EXPECT_DOUBLES_EQUAL(distance2, 5, 1e-8);
EXPECT(assert_equal(Unit3(-1,0,0).unitVector(), plane2.normal().unitVector()));
}
//*******************************************************************************
OrientedPlane3 Transform_(const OrientedPlane3& plane, const Pose3& xr) {
return OrientedPlane3::Transform(plane, xr);
}
OrientedPlane3 transform_(const OrientedPlane3& plane, const Pose3& xr) {
return plane.transform(xr);
}
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,
OrientedPlane3 expectedPlane(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3);
OrientedPlane3 transformedPlane1 = OrientedPlane3::Transform(plane, pose,
none, none);
EXPECT(assert_equal(expected_meas, transformed_plane, 1e-9));
OrientedPlane3 transformedPlane2 = plane.transform(pose, none, none);
EXPECT(assert_equal(expectedPlane, transformedPlane1, 1e-9));
EXPECT(assert_equal(expectedPlane, transformedPlane2, 1e-9));
// Test the jacobians of transform
Matrix actualH1, expectedH1, actualH2, expectedH2;
{
expectedH1 = numericalDerivative11<OrientedPlane3, Pose3>(
boost::bind(&OrientedPlane3::Transform, plane, _1, none, none), pose);
OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, actualH1,
none);
// because the Transform class uses a wrong order of Jacobians in interface
OrientedPlane3::Transform(plane, pose, actualH1, none);
expectedH1 = numericalDerivative22(Transform_, plane, pose);
EXPECT(assert_equal(expectedH1, actualH1, 1e-9));
}
{
expectedH2 = numericalDerivative11<OrientedPlane3, OrientedPlane3>(
boost::bind(&OrientedPlane3::Transform, _1, pose, none, none), plane);
OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, none,
actualH2);
OrientedPlane3::Transform(plane, pose, none, actualH2);
expectedH2 = numericalDerivative21(Transform_, plane, pose);
EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
}
{
plane.transform(pose, actualH1, none);
expectedH1 = numericalDerivative21(transform_, plane, pose);
EXPECT(assert_equal(expectedH1, actualH1, 1e-9));
plane.transform(pose, none, actualH2);
expectedH2 = numericalDerivative22(Transform_, plane, pose);
EXPECT(assert_equal(expectedH2, actualH2, 1e-9));
}
}
//*******************************************************************************
// Returns a random vector -- copied from testUnit3.cpp
// Returns a any size random vector
inline static Vector randomVector(const Vector& minLimits,
const Vector& maxLimits) {
@ -82,11 +111,11 @@ inline static Vector randomVector(const Vector& minLimits,
TEST(OrientedPlane3, localCoordinates_retract) {
size_t numIterations = 10000;
gtsam::Vector minPlaneLimit(4), maxPlaneLimit(4);
Vector4 minPlaneLimit, maxPlaneLimit;
minPlaneLimit << -1.0, -1.0, -1.0, 0.01;
maxPlaneLimit << 1.0, 1.0, 1.0, 10.0;
Vector minXiLimit(3), maxXiLimit(3);
Vector3 minXiLimit, maxXiLimit;
minXiLimit << -M_PI, -M_PI, -10.0;
maxXiLimit << M_PI, M_PI, 10.0;
for (size_t i = 0; i < numIterations; i++) {
@ -98,15 +127,15 @@ 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)
v12.segment<3>(0) = v12.segment<3>(0) / M_PI;
if (v12.head<3>().norm() > M_PI)
v12.head<3>() = v12.head<3>() / 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));
EXPECT(assert_equal(v12, actual_v12, 1e-6));
OrientedPlane3 actual_p2 = p1.retract(actual_v12);
EXPECT(assert_equal(p2, actual_p2, 1e-3));
EXPECT(assert_equal(p2, actual_p2, 1e-6));
}
}