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

BORG-style comments

release/4.3a0
dellaert 2015-02-12 16:31:04 +01:00
parent 91b16766dd
commit 235cb532f4
3 changed files with 80 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
gtsam/geometry/OrientedPlane3.cpp
View File

@ -27,77 +27,72 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
/// The print fuction /// The print fuction
void OrientedPlane3::print(const std::string& s) const { void OrientedPlane3::print(const std::string& s) const {
Vector coeffs = planeCoefficients (); Vector coeffs = planeCoefficients();
cout << s << " : " << coeffs << endl; cout << s << " : " << coeffs << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
OrientedPlane3 OrientedPlane3::Transform (const gtsam::OrientedPlane3& plane, OrientedPlane3 OrientedPlane3::Transform(const gtsam::OrientedPlane3& plane,
const gtsam::Pose3& xr, const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr,
OptionalJacobian<3, 6> Hr, OptionalJacobian<3, 3> Hp) {
OptionalJacobian<3, 3> Hp) {
Matrix n_hr; Matrix n_hr;
Matrix n_hp; Matrix n_hp;
Unit3 n_rotated = xr.rotation ().unrotate (plane.n_, n_hr, n_hp); Unit3 n_rotated = xr.rotation().unrotate(plane.n_, n_hr, n_hp);
Vector n_unit = plane.n_.unitVector (); Vector n_unit = plane.n_.unitVector();
Vector unit_vec = n_rotated.unitVector (); Vector unit_vec = n_rotated.unitVector();
double pred_d = n_unit.dot (xr.translation ().vector ()) + plane.d_; 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); OrientedPlane3 transformed_plane(unit_vec(0), unit_vec(1), unit_vec(2),
pred_d);
if (Hr) if (Hr) {
{ *Hr = gtsam::zeros(3, 6);
*Hr = gtsam::zeros (3, 6); (*Hr).block<2, 3>(0, 0) = n_hr;
(*Hr).block<2,3> (0,0) = n_hr; (*Hr).block<1, 3>(2, 3) = unit_vec;
(*Hr).block<1,3> (2,3) = unit_vec;
} }
if (Hp) if (Hp) {
{ Vector xrp = xr.translation().vector();
Vector xrp = xr.translation ().vector ();
Matrix n_basis = plane.n_.basis(); Matrix n_basis = plane.n_.basis();
Vector hpp = n_basis.transpose() * xrp; Vector hpp = n_basis.transpose() * xrp;
*Hp = gtsam::zeros (3,3); *Hp = gtsam::zeros(3, 3);
(*Hp).block<2,2> (0,0) = n_hp; (*Hp).block<2, 2>(0, 0) = n_hp;
(*Hp).block<1,2> (2,0) = hpp; (*Hp).block<1, 2>(2, 0) = hpp;
(*Hp) (2,2) = 1; (*Hp)(2, 2) = 1;
} }
return (transformed_plane); return (transformed_plane);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 OrientedPlane3::error (const gtsam::OrientedPlane3& plane) const Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const {
{ Vector2 n_error = -n_.localCoordinates(plane.n_);
Vector2 n_error = -n_.localCoordinates (plane.n_);
double d_error = d_ - plane.d_; double d_error = d_ - plane.d_;
Vector3 e; Vector3 e;
e << n_error (0), n_error (1), d_error; e << n_error(0), n_error(1), d_error;
return (e); return (e);
} }
/* ************************************************************************* */ /* ************************************************************************* */
OrientedPlane3 OrientedPlane3::retract(const Vector& v) const { OrientedPlane3 OrientedPlane3::retract(const Vector& v) const {
// Retract the Unit3 // Retract the Unit3
Vector2 n_v (v (0), v (1)); Vector2 n_v(v(0), v(1));
Unit3 n_retracted = n_.retract (n_v); Unit3 n_retracted = n_.retract(n_v);
double d_retracted = d_ + v (2); double d_retracted = d_ + v(2);
return OrientedPlane3 (n_retracted, d_retracted); return OrientedPlane3(n_retracted, d_retracted);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const { Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
Vector n_local = n_.localCoordinates (y.n_); Vector n_local = n_.localCoordinates(y.n_);
double d_local = d_ - y.d_; double d_local = d_ - y.d_;
Vector3 e; Vector3 e;
e << n_local (0), n_local (1), -d_local; e << n_local(0), n_local(1), -d_local;
return e; return e;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 OrientedPlane3::planeCoefficients () const Vector3 OrientedPlane3::planeCoefficients() const {
{ Vector unit_vec = n_.unitVector();
Vector unit_vec = n_.unitVector ();
Vector3 a; Vector3 a;
a << unit_vec[0], unit_vec[1], unit_vec[2], d_; a << unit_vec[0], unit_vec[1], unit_vec[2], d_;
return a; return a;

46
gtsam/geometry/OrientedPlane3.h
View File

@ -35,32 +35,29 @@ private:
double d_; /// The perpendicular distance to this plane double d_; /// The perpendicular distance to this plane
public: public:
enum { dimension = 3 }; enum {
dimension = 3
};
/// @name Constructors /// @name Constructors
/// @{ /// @{
/// Default constructor /// Default constructor
OrientedPlane3() : OrientedPlane3() :
n_(), n_(), d_(0.0) {
d_ (0.0){
} }
/// Construct from a Unit3 and a distance /// Construct from a Unit3 and a distance
OrientedPlane3 (const Unit3& s, double d) OrientedPlane3(const Unit3& s, double d) :
: n_ (s), n_(s), d_(d) {
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 /// Construct from a, b, c, d
OrientedPlane3(double a, double b, double c, double d) { OrientedPlane3(double a, double b, double c, double d) {
Point3 p (a, b, c); Point3 p(a, b, c);
n_ = Unit3(p); n_ = Unit3(p);
d_ = d; d_ = d;
} }
@ -70,17 +67,16 @@ public:
/// The equals function with tolerance /// The equals function with tolerance
bool equals(const OrientedPlane3& s, double tol = 1e-9) const { bool equals(const OrientedPlane3& s, double tol = 1e-9) const {
return (n_.equals(s.n_, tol) && (fabs (d_ - s.d_) < tol)); return (n_.equals(s.n_, tol) && (fabs(d_ - s.d_) < tol));
} }
/// Transforms a plane to the specified pose /// Transforms a plane to the specified pose
static OrientedPlane3 Transform (const gtsam::OrientedPlane3& plane, static OrientedPlane3 Transform(const gtsam::OrientedPlane3& plane,
const gtsam::Pose3& xr, const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none,
OptionalJacobian<3, 6> Hr = boost::none, OptionalJacobian<3, 3> Hp = boost::none);
OptionalJacobian<3, 3> Hp = boost::none);
/// Computes the error between two poses /// Computes the error between two poses
Vector3 error (const gtsam::OrientedPlane3& plane) const; Vector3 error(const gtsam::OrientedPlane3& plane) const;
/// Dimensionality of tangent space = 3 DOF /// Dimensionality of tangent space = 3 DOF
inline static size_t Dim() { inline static size_t Dim() {
@ -99,18 +95,22 @@ public:
Vector3 localCoordinates(const OrientedPlane3& s) const; Vector3 localCoordinates(const OrientedPlane3& s) const;
/// Returns the plane coefficients (a, b, c, d) /// Returns the plane coefficients (a, b, c, d)
Vector3 planeCoefficients () const; Vector3 planeCoefficients() const;
inline Unit3 normal () const { inline Unit3 normal() const {
return n_; return n_;
} }
/// @} /// @}
}; };
template <> struct traits<OrientedPlane3> : public internal::Manifold<OrientedPlane3> {}; template<> struct traits<OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
};
template <> struct traits<const OrientedPlane3> : public internal::Manifold<OrientedPlane3> {}; template<> struct traits<const OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
};
} // namespace gtsam } // namespace gtsam

44
gtsam/geometry/Unit3.h
View File

@ -38,7 +38,9 @@ private:
public: public:
enum { dimension = 2 }; enum {
dimension = 2
};
/// @name Constructors /// @name Constructors
/// @{ /// @{
@ -65,8 +67,8 @@ public:
} }
/// Named constructor from Point3 with optional Jacobian /// Named constructor from Point3 with optional Jacobian
static Unit3 FromPoint3(const Point3& point, OptionalJacobian<2,3> H = static Unit3 FromPoint3(const Point3& point, //
boost::none); OptionalJacobian<2, 3> H = boost::none);
/// Random direction, using boost::uniform_on_sphere /// Random direction, using boost::uniform_on_sphere
static Unit3 Random(boost::mt19937 & rng); static Unit3 Random(boost::mt19937 & rng);
@ -99,7 +101,7 @@ public:
Matrix3 skew() const; Matrix3 skew() const;
/// Return unit-norm Point3 /// 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) if (H)
*H = basis(); *H = basis();
return p_; return p_;
@ -109,7 +111,7 @@ public:
Vector unitVector(boost::optional<Matrix&> H = boost::none) const { Vector unitVector(boost::optional<Matrix&> H = boost::none) const {
if (H) if (H)
*H = basis(); *H = basis();
return (p_.vector ()); return (p_.vector());
} }
/// Return scaled direction as Point3 /// Return scaled direction as Point3
@ -118,12 +120,10 @@ public:
} }
/// Signed, vector-valued error between two directions /// Signed, vector-valued error between two directions
Vector error(const Unit3& q, Vector error(const Unit3& q, OptionalJacobian<2, 2> H = boost::none) const;
OptionalJacobian<2,2> H = boost::none) const;
/// Distance between two directions /// Distance between two directions
double distance(const Unit3& q, double distance(const Unit3& q, OptionalJacobian<1, 2> H = boost::none) const;
OptionalJacobian<1,2> H = boost::none) const;
/// @} /// @}
@ -160,25 +160,27 @@ private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(p_); ar & BOOST_SERIALIZATION_NVP(p_);
// homebrew serialize Eigen Matrix // homebrew serialize Eigen Matrix
ar & boost::serialization::make_nvp("B11", (*B_)(0,0)); ar & boost::serialization::make_nvp("B11", (*B_)(0, 0));
ar & boost::serialization::make_nvp("B12", (*B_)(0,1)); ar & boost::serialization::make_nvp("B12", (*B_)(0, 1));
ar & boost::serialization::make_nvp("B21", (*B_)(1,0)); ar & boost::serialization::make_nvp("B21", (*B_)(1, 0));
ar & boost::serialization::make_nvp("B22", (*B_)(1,1)); ar & boost::serialization::make_nvp("B22", (*B_)(1, 1));
ar & boost::serialization::make_nvp("B31", (*B_)(2,0)); ar & boost::serialization::make_nvp("B31", (*B_)(2, 0));
ar & boost::serialization::make_nvp("B32", (*B_)(2,1)); ar & boost::serialization::make_nvp("B32", (*B_)(2, 1));
} }
/// @} /// @}
}; };
// Define GTSAM traits // 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 } // namespace gtsam