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

71
gtsam/geometry/OrientedPlane3.cpp
View File

@ -27,77 +27,72 @@ namespace gtsam {
/* ************************************************************************* */
/// The print fuction
void OrientedPlane3::print(const std::string& s) const {
Vector coeffs = planeCoefficients ();
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) {
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);
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);
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 (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 ();
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;
*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_);
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;
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);
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_);
Vector n_local = n_.localCoordinates(y.n_);
double d_local = d_ - y.d_;
Vector3 e;
e << n_local (0), n_local (1), -d_local;
e << n_local(0), n_local(1), -d_local;
return e;
}
/* ************************************************************************* */
Vector3 OrientedPlane3::planeCoefficients () const
{
Vector unit_vec = n_.unitVector ();
Vector3 OrientedPlane3::planeCoefficients() const {
Vector unit_vec = n_.unitVector();
Vector3 a;
a << unit_vec[0], unit_vec[1], unit_vec[2], d_;
return a;

48
gtsam/geometry/OrientedPlane3.h
View File

@ -35,32 +35,29 @@ private:
double d_; /// The perpendicular distance to this plane
public:
enum { dimension = 3 };
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)
{}
OrientedPlane3 (Vector vec)
: n_ (vec (0), vec (1), vec (2)),
d_ (vec (3))
{
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);
Point3 p(a, b, c);
n_ = Unit3(p);
d_ = d;
}
@ -70,17 +67,16 @@ public:
/// 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));
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);
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;
Vector3 error(const gtsam::OrientedPlane3& plane) const;
/// Dimensionality of tangent space = 3 DOF
inline static size_t Dim() {
@ -99,18 +95,22 @@ public:
Vector3 localCoordinates(const OrientedPlane3& s) const;
/// Returns the plane coefficients (a, b, c, d)
Vector3 planeCoefficients () const;
Vector3 planeCoefficients() const;
inline Unit3 normal () const {
inline Unit3 normal() const {
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

44
gtsam/geometry/Unit3.h
View File

@ -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,7 +101,7 @@ 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_;
@ -109,7 +111,7 @@ public:
Vector unitVector(boost::optional<Matrix&> H = boost::none) const {
if (H)
*H = basis();
return (p_.vector ());
return (p_.vector());
}
/// Return scaled direction as Point3
@ -118,12 +120,10 @@ public:
}
/// Signed, vector-valued error between two directions
Vector 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;
/// @}
@ -160,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