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

fixed formatting issues

release/4.3a0
nsrinivasan7 2015-02-12 08:22:25 -05:00
parent e4a6a2a951
commit ce85eecaff
6 changed files with 164 additions and 170 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
gtsam/geometry/OrientedPlane3.cpp
View File

@ -33,49 +33,48 @@ void OrientedPlane3::print(const std::string& s) const {
/* ************************************************************************* */
OrientedPlane3 OrientedPlane3::Transform (const gtsam::OrientedPlane3& plane,
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);
OptionalJacobian<3, 3> Hp) {
Matrix n_hr;
Matrix n_hp;
Unit3 n_rotated = xr.rotation ().unrotate (plane.n_, n_hr, n_hp);
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 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);
}
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 {
@ -96,13 +95,13 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
}
/* ************************************************************************* */
Vector3 OrientedPlane3::planeCoefficients () const
{
Vector unit_vec = n_.unitVector ();
Vector3 a;
a << unit_vec[0], unit_vec[1], unit_vec[2], d_;
return a;
}
Vector3 OrientedPlane3::planeCoefficients () const
{
Vector unit_vec = n_.unitVector ();
Vector3 a;
a << unit_vec[0], unit_vec[1], unit_vec[2], d_;
return a;
}
/* ************************************************************************* */

30
gtsam/geometry/OrientedPlane3.h
View File

@ -31,8 +31,8 @@ class OrientedPlane3: public DerivedValue<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 };
@ -46,16 +46,16 @@ public:
}
/// Construct from a Unit3 and a distance
OrientedPlane3 (const Unit3& s, double d)
: n_ (s),
d_ (d)
{}
OrientedPlane3 (const Unit3& s, double d)
: n_ (s),
d_ (d)
{}
OrientedPlane3 (Vector vec)
: n_ (vec (0), vec (1), vec (2)),
d_ (vec (3))
{
}
OrientedPlane3 (Vector vec)
: n_ (vec (0), vec (1), vec (2)),
d_ (vec (3))
{
}
/// Construct from a, b, c, d
@ -75,9 +75,9 @@ public:
/// 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);
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;
@ -104,7 +104,7 @@ public:
inline Unit3 normal () const {
return n_;
}
/// @}
};

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

@ -42,30 +42,30 @@ 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 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);
// 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));
}
}
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
@ -78,9 +78,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;
}
@ -102,7 +102,7 @@ TEST(OrientedPlane3, localCoordinates_retract) {
// 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;

32
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -23,7 +23,7 @@ void OrientedPlane3DirectionPrior::print(const string& s) const {
//***************************************************************************
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);
}
@ -33,21 +33,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;
}
}
}

131
gtsam/slam/OrientedPlane3Factor.h
View File

@ -16,85 +16,80 @@
namespace gtsam {
/**
/**
* Factor to measure a planar landmark from a given pose
*/
class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
protected:
Symbol poseSymbol_;
Symbol landmarkSymbol_;
Vector3 measured_coeffs_;
OrientedPlane3 measured_p_;
typedef NoiseModelFactor2<Pose3, OrientedPlane3 > Base;
protected:
Symbol poseSymbol_;
Symbol landmarkSymbol_;
Vector3 measured_coeffs_;
OrientedPlane3 measured_p_;
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));
}
typedef NoiseModelFactor2<Pose3, OrientedPlane3 > Base;
/// 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);
};
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> {
// 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:
protected:
OrientedPlane3 measured_p_; /// measured plane parameters
Key landmarkKey_;
typedef OrientedPlane3DirectionPrior This;
/// Constructor
OrientedPlane3DirectionPrior ()
{}
typedef NoiseModelFactor1<OrientedPlane3 > Base;
/// 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;
public:
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
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;
};
virtual Vector evaluateError(const OrientedPlane3& plane,
boost::optional<Matrix&> H = boost::none) const;
};
} // gtsam

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

@ -141,7 +141,7 @@ TEST( OrientedPlane3DirectionPriorFactor, Constructor ) {
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
// 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);