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

Working on getting a simple typedef to compile - as well as dealing with Point3() now creating uninitialized memory.

release/4.3a0
Frank 2016-02-10 17:48:52 -08:00
parent fefb74350a
commit 0a7fd27f28
33 changed files with 155 additions and 356 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/base/serializationTestHelpers.h
View File

@ -30,6 +30,11 @@ const bool verbose = false;
namespace gtsam { namespace gtsam {
namespace serializationTestHelpers { namespace serializationTestHelpers {
// templated default object creation so we only need to declare one friend (if applicable)
template<class T>
T create() {
return T();
}
// Templated round-trip serialization // Templated round-trip serialization
template<class T> template<class T>
@ -44,7 +49,7 @@ void roundtrip(const T& input, T& output) {
// This version requires equality operator // This version requires equality operator
template<class T> template<class T>
bool equality(const T& input = T()) { bool equality(const T& input = T()) {
T output; T output = create<T>();
roundtrip<T>(input,output); roundtrip<T>(input,output);
return input==output; return input==output;
} }
@ -52,7 +57,7 @@ bool equality(const T& input = T()) {
// This version requires Testable // This version requires Testable
template<class T> template<class T>
bool equalsObj(const T& input = T()) { bool equalsObj(const T& input = T()) {
T output; T output = create<T>();
roundtrip<T>(input,output); roundtrip<T>(input,output);
return assert_equal(input, output); return assert_equal(input, output);
} }
@ -60,7 +65,7 @@ bool equalsObj(const T& input = T()) {
// De-referenced version for pointers, requires equals method // De-referenced version for pointers, requires equals method
template<class T> template<class T>
bool equalsDereferenced(const T& input) { bool equalsDereferenced(const T& input) {
T output; T output = create<T>();
roundtrip<T>(input,output); roundtrip<T>(input,output);
return input->equals(*output); return input->equals(*output);
} }
@ -79,7 +84,7 @@ void roundtripXML(const T& input, T& output) {
// This version requires equality operator // This version requires equality operator
template<class T> template<class T>
bool equalityXML(const T& input = T()) { bool equalityXML(const T& input = T()) {
T output; T output = create<T>();
roundtripXML<T>(input,output); roundtripXML<T>(input,output);
return input==output; return input==output;
} }
@ -87,7 +92,7 @@ bool equalityXML(const T& input = T()) {
// This version requires Testable // This version requires Testable
template<class T> template<class T>
bool equalsXML(const T& input = T()) { bool equalsXML(const T& input = T()) {
T output; T output = create<T>();
roundtripXML<T>(input,output); roundtripXML<T>(input,output);
return assert_equal(input, output); return assert_equal(input, output);
} }
@ -95,7 +100,7 @@ bool equalsXML(const T& input = T()) {
// This version is for pointers, requires equals method // This version is for pointers, requires equals method
template<class T> template<class T>
bool equalsDereferencedXML(const T& input = T()) { bool equalsDereferencedXML(const T& input = T()) {
T output; T output = create<T>();
roundtripXML<T>(input,output); roundtripXML<T>(input,output);
return input->equals(*output); return input->equals(*output);
} }
@ -114,7 +119,7 @@ void roundtripBinary(const T& input, T& output) {
// This version requires equality operator // This version requires equality operator
template<class T> template<class T>
bool equalityBinary(const T& input = T()) { bool equalityBinary(const T& input = T()) {
T output; T output = create<T>();
roundtripBinary<T>(input,output); roundtripBinary<T>(input,output);
return input==output; return input==output;
} }
@ -122,7 +127,7 @@ bool equalityBinary(const T& input = T()) {
// This version requires Testable // This version requires Testable
template<class T> template<class T>
bool equalsBinary(const T& input = T()) { bool equalsBinary(const T& input = T()) {
T output; T output = create<T>();
roundtripBinary<T>(input,output); roundtripBinary<T>(input,output);
return assert_equal(input, output); return assert_equal(input, output);
} }
@ -130,7 +135,7 @@ bool equalsBinary(const T& input = T()) {
// This version is for pointers, requires equals method // This version is for pointers, requires equals method
template<class T> template<class T>
bool equalsDereferencedBinary(const T& input = T()) { bool equalsDereferencedBinary(const T& input = T()) {
T output; T output = create<T>();
roundtripBinary<T>(input,output); roundtripBinary<T>(input,output);
return input->equals(*output); return input->equals(*output);
} }

2
gtsam/geometry/EssentialMatrix.cpp
View File

@ -107,7 +107,7 @@ ostream& operator <<(ostream& os, const EssentialMatrix& E) {
Rot3 R = E.rotation(); Rot3 R = E.rotation();
Unit3 d = E.direction(); Unit3 d = E.direction();
os.precision(10); os.precision(10);
os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " "; os << R.xyz().transpose() << " " << d.point3().transpose() << " ";
return os; return os;
} }

4
gtsam/geometry/OrientedPlane3.cpp
View File

@ -39,7 +39,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose); Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose);
Vector3 unit_vec = n_rotated.unitVector(); Vector3 unit_vec = n_rotated.unitVector();
double pred_d = n_.unitVector().dot(xr.translation().vector()) + d_; double pred_d = n_.unitVector().dot(xr.translation()) + d_;
if (Hr) { if (Hr) {
*Hr = zeros(3, 6); *Hr = zeros(3, 6);
@ -47,7 +47,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
Hr->block<1, 3>(2, 3) = unit_vec; Hr->block<1, 3>(2, 3) = unit_vec;
} }
if (Hp) { if (Hp) {
Vector2 hpp = n_.basis().transpose() * xr.translation().vector(); Vector2 hpp = n_.basis().transpose() * xr.translation();
*Hp = Z_3x3; *Hp = Z_3x3;
Hp->block<2, 2>(0, 0) = D_rotated_pose; Hp->block<2, 2>(0, 0) = D_rotated_pose;
Hp->block<1, 2>(2, 0) = hpp; Hp->block<1, 2>(2, 0) = hpp;

27
gtsam/geometry/Point3.cpp
View File

@ -21,40 +21,16 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ #ifndef GTSAM_USE_VECTOR3_POINTS
bool Point3::equals(const Point3 &q, double tol) const { bool Point3::equals(const Point3 &q, double tol) const {
return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol &&
fabs(z() - q.z()) < tol); fabs(z() - q.z()) < tol);
} }
/* ************************************************************************* */
void Point3::print(const string& s) const { void Point3::print(const string& s) const {
cout << s << *this << endl; cout << s << *this << endl;
} }
#ifndef GTSAM_USE_VECTOR3_POINTS
/* ************************************************************************* */
bool Point3::operator==(const Point3& q) const {
return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
}
Point3 Point3::operator+(const Point3& q) const {
return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_);
}
Point3 Point3::operator-(const Point3& q) const {
return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_);
}
Point3 Point3::operator*(double s) const {
return Point3(x_ * s, y_ * s, z_ * s);
}
Point3 Point3::operator/(double s) const {
return Point3(x_ / s, y_ / s, z_ / s);
}
#endif
/* ************************************************************************* */ /* ************************************************************************* */
double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1, double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) const { OptionalJacobian<1, 3> H2) const {
@ -102,6 +78,7 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
} }
#endif #endif
#endif
/* ************************************************************************* */ /* ************************************************************************* */
double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1, double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) { OptionalJacobian<1, 3> H2) {

173
gtsam/geometry/Point3.h
View File

@ -30,6 +30,12 @@ namespace gtsam {
//#define GTSAM_USE_VECTOR3_POINTS //#define GTSAM_USE_VECTOR3_POINTS
#ifdef GTSAM_USE_VECTOR3_POINTS #ifdef GTSAM_USE_VECTOR3_POINTS
// As of GTSAM4, we just typedef Point3 to Vector3
typedef Vector3 Point3;
#else
/** /**
* A 3D point is just a Vector3 with some additional methods * A 3D point is just a Vector3 with some additional methods
* @addtogroup geometry * @addtogroup geometry
@ -38,13 +44,17 @@ namespace gtsam {
class GTSAM_EXPORT Point3 : public Vector3 { class GTSAM_EXPORT Point3 : public Vector3 {
public: public:
enum { dimension = 3 }; enum { dimension = 3 };
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
/// Default constructor creates a zero-Point3 #ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4
Point3() { setZero(); } /// Default constructor now creates *uninitialized* point !!!!
Point3() {}
#endif
/// Construct from x, y, and z coordinates /// Construct from x, y, and z coordinates
Point3(double x, double y, double z): Vector3(x,y, z) {} Point3(double x, double y, double z): Vector3(x,y, z) {}
@ -68,7 +78,7 @@ class GTSAM_EXPORT Point3 : public Vector3 {
/// @{ /// @{
/// identity for group operation /// identity for group operation
inline static Point3 identity() { return Point3();} inline static Point3 identity() { return Point3(0.0, 0.0, 0.0); }
/// @} /// @}
/// @name Vector Space /// @name Vector Space
@ -82,7 +92,7 @@ class GTSAM_EXPORT Point3 : public Vector3 {
double norm(OptionalJacobian<1,3> H = boost::none) const; double norm(OptionalJacobian<1,3> H = boost::none) const;
/** normalize, with optional Jacobian */ /** normalize, with optional Jacobian */
Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const; Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;
/** cross product @return this x q */ /** cross product @return this x q */
Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, // Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
@ -104,136 +114,10 @@ class GTSAM_EXPORT Point3 : public Vector3 {
/// Output stream operator /// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p); GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3);
}
};
#else
/**
* A 3D point
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Point3 {
private:
double x_, y_, z_;
public:
enum { dimension = 3 };
/// @name Standard Constructors
/// @{
/// Default constructor creates a zero-Point3
Point3(): x_(0), y_(0), z_(0) {}
/// Construct from x, y, and z coordinates
Point3(double x, double y, double z): x_(x), y_(y), z_(z) {}
/// @}
/// @name Advanced Constructors
/// @{
/// Construct from 3-element vector
explicit Point3(const Vector3& v) {
x_ = v(0);
y_ = v(1);
z_ = v(2);
}
/// @}
/// @name Testable
/// @{
/** print with optional string */
void print(const std::string& s = "") const;
/** equals with an tolerance */
bool equals(const Point3& p, double tol = 1e-9) const;
/// @}
/// @name Group
/// @{
/// identity for group operation
inline static Point3 identity() { return Point3();}
/// inverse
Point3 operator - () const { return Point3(-x_,-y_,-z_);}
/// add vector on right
inline Point3 operator +(const Vector3& v) const {
return Point3(x_ + v[0], y_ + v[1], z_ + v[2]);
}
/// add
Point3 operator + (const Point3& q) const;
/// subtract
Point3 operator - (const Point3& q) const;
/// @}
/// @name Vector Space
/// @{
///multiply with a scalar
Point3 operator * (double s) const;
///divide by a scalar
Point3 operator / (double s) const;
/** distance between two points */
double distance(const Point3& q, OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) const;
/** Distance of the point from the origin, with Jacobian */
double norm(OptionalJacobian<1,3> H = boost::none) const;
/** normalize, with optional Jacobian */
Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;
/** cross product @return this x q */
Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
OptionalJacobian<3, 3> H_q = boost::none) const;
/** dot product @return this * q*/
double dot(const Point3 &q, OptionalJacobian<1, 3> H_p = boost::none, //
OptionalJacobian<1, 3> H_q = boost::none) const;
/// @}
/// @name Standard Interface
/// @{
/// equality
bool operator ==(const Point3& q) const;
/** return vectorized form (column-wise)*/
Vector3 vector() const { return Vector3(x_,y_,z_); }
/// get x
inline double x() const {return x_;}
/// get y
inline double y() const {return y_;}
/// get z
inline double z() const {return z_;}
/// @}
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated /// @name Deprecated
/// @{ /// @{
Point3() { setZero(); }
Point3 inverse() const { return -(*this);} Point3 inverse() const { return -(*this);}
Point3 compose(const Point3& q) const { return (*this)+q;} Point3 compose(const Point3& q) const { return (*this)+q;}
Point3 between(const Point3& q) const { return q-(*this);} Point3 between(const Point3& q) const { return q-(*this);}
@ -252,37 +136,26 @@ class GTSAM_EXPORT Point3 {
private: private:
/// @name Advanced Interface
/// @{
/** 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_BASE_OBJECT_NVP(Vector3);
ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_);
ar & BOOST_SERIALIZATION_NVP(z_);
} }
/// @}
}; };
/// Syntactic sugar for multiplying coordinates by a scalar s*p
inline Point3 operator*(double s, const Point3& p) { return p*s;}
#endif
// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
template<> template<>
struct traits<Point3> : public internal::VectorSpace<Point3> {}; struct traits<Point3> : public internal::VectorSpace<Point3> {};
template<> template<>
struct traits<const Point3> : public internal::VectorSpace<Point3> {}; struct traits<const Point3> : public internal::VectorSpace<Point3> {};
#endif
// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
/// distance between two points /// distance between two points
double distance(const Point3& p1, const Point3& q, double distance(const Point3& p1, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none, OptionalJacobian<1, 3> H1 = boost::none,

50
gtsam/geometry/Pose3.cpp
View File

@ -48,8 +48,7 @@ Pose3 Pose3::inverse() const {
// Experimental - unit tests of derivatives based on it do not check out yet // Experimental - unit tests of derivatives based on it do not check out yet
Matrix6 Pose3::AdjointMap() const { Matrix6 Pose3::AdjointMap() const {
const Matrix3 R = R_.matrix(); const Matrix3 R = R_.matrix();
const Vector3 t = t_.vector(); Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
Matrix3 A = skewSymmetric(t) * R;
Matrix6 adj; Matrix6 adj;
adj << R, Z_3x3, A, R; adj << R, Z_3x3, A, R;
return adj; return adj;
@ -101,12 +100,12 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
void Pose3::print(const string& s) const { void Pose3::print(const string& s) const {
cout << s; cout << s;
R_.print("R:\n"); R_.print("R:\n");
t_.print("t: "); traits<Point3>::Print(t_, "t: ");
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool Pose3::equals(const Pose3& pose, double tol) const { bool Pose3::equals(const Pose3& pose, double tol) const {
return R_.equals(pose.R_, tol) && t_.equals(pose.t_, tol); return R_.equals(pose.R_, tol) && traits<Point3>::Equals(t_, pose.t_, tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -115,14 +114,14 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
if (H) *H = ExpmapDerivative(xi); if (H) *H = ExpmapDerivative(xi);
// get angular velocity omega and translational velocity v from twist xi // get angular velocity omega and translational velocity v from twist xi
Point3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
Rot3 R = Rot3::Expmap(omega.vector()); Rot3 R = Rot3::Expmap(omega);
double theta2 = omega.dot(omega); double theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<double>::epsilon()) { if (theta2 > std::numeric_limits<double>::epsilon()) {
Point3 t_parallel = omega * omega.dot(v); // translation parallel to axis Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis
Point3 omega_cross_v = omega.cross(v); // points towards axis Vector3 omega_cross_v = omega.cross(v); // points towards axis
Point3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
return Pose3(R, t); return Pose3(R, t);
} else { } else {
return Pose3(R, v); return Pose3(R, v);
@ -132,19 +131,20 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
/* ************************************************************************* */ /* ************************************************************************* */
Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) {
if (H) *H = LogmapDerivative(p); if (H) *H = LogmapDerivative(p);
Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); const Vector3 w = Rot3::Logmap(p.rotation());
double t = w.norm(); const Vector3 T = p.translation();
const double t = w.norm();
if (t < 1e-10) { if (t < 1e-10) {
Vector6 log; Vector6 log;
log << w, T; log << w, T;
return log; return log;
} else { } else {
Matrix3 W = skewSymmetric(w / t); const Matrix3 W = skewSymmetric(w / t);
// Formula from Agrawal06iros, equation (14) // Formula from Agrawal06iros, equation (14)
// simplified with Mathematica, and multiplying in T to avoid matrix math // simplified with Mathematica, and multiplying in T to avoid matrix math
double Tan = tan(0.5 * t); const double Tan = tan(0.5 * t);
Vector3 WT = W * T; const Vector3 WT = W * T;
Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT); const Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
Vector6 log; Vector6 log;
log << w, u; log << w, u;
return log; return log;
@ -178,7 +178,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
H->topLeftCorner<3,3>() = DR; H->topLeftCorner<3,3>() = DR;
} }
Vector6 xi; Vector6 xi;
xi << omega, T.translation().vector(); xi << omega, T.translation();
return xi; return xi;
#endif #endif
} }
@ -264,7 +264,7 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const {
Matrix4 Pose3::matrix() const { Matrix4 Pose3::matrix() const {
static const Matrix14 A14 = (Matrix14() << 0.0, 0.0, 0.0, 1.0).finished(); static const Matrix14 A14 = (Matrix14() << 0.0, 0.0, 0.0, 1.0).finished();
Matrix4 mat; Matrix4 mat;
mat << R_.matrix(), t_.vector(), A14; mat << R_.matrix(), t_, A14;
return mat; return mat;
} }
@ -288,7 +288,7 @@ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
if (Dpoint) { if (Dpoint) {
*Dpoint = R; *Dpoint = R;
} }
return Point3(R * p.vector()) + t_; return R_ * p + t_;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -297,7 +297,7 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
// Only get transpose once, to avoid multiple allocations, // Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case // as well as multiple conversions in the Quaternion case
const Matrix3 Rt = R_.transpose(); const Matrix3 Rt = R_.transpose();
const Point3 q(Rt*(p.vector() - t_.vector())); const Point3 q(Rt*(p - t_));
if (Dpose) { if (Dpose) {
const double wx = q.x(), wy = q.y(), wz = q.z(); const double wx = q.x(), wy = q.y(), wz = q.z();
(*Dpose) << (*Dpose) <<
@ -321,7 +321,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
return local.norm(); return local.norm();
} else { } else {
Matrix13 D_r_local; Matrix13 D_r_local;
const double r = local.norm(D_r_local); const double r = norm(local, D_r_local);
if (H1) *H1 = D_r_local * D_local_pose; if (H1) *H1 = D_r_local * D_local_pose;
if (H2) *H2 = D_r_local * D_local_point; if (H2) *H2 = D_r_local * D_local_point;
return r; return r;
@ -361,10 +361,10 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
return boost::none; // we need at least three pairs return boost::none; // we need at least three pairs
// calculate centroids // calculate centroids
Vector3 cp = Vector3::Zero(), cq = Vector3::Zero(); Point3 cp(0,0,0), cq(0,0,0);
BOOST_FOREACH(const Point3Pair& pair, pairs) { BOOST_FOREACH(const Point3Pair& pair, pairs) {
cp += pair.first.vector(); cp += pair.first;
cq += pair.second.vector(); cq += pair.second;
} }
double f = 1.0 / n; double f = 1.0 / n;
cp *= f; cp *= f;
@ -373,8 +373,8 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
// Add to form H matrix // Add to form H matrix
Matrix3 H = Z_3x3; Matrix3 H = Z_3x3;
BOOST_FOREACH(const Point3Pair& pair, pairs) { BOOST_FOREACH(const Point3Pair& pair, pairs) {
Vector3 dp = pair.first.vector() - cp; Point3 dp = pair.first - cp;
Vector3 dq = pair.second.vector() - cq; Point3 dq = pair.second - cq;
H += dp * dq.transpose(); H += dp * dq.transpose();
} }

10
gtsam/geometry/Pose3.h
View File

@ -52,8 +52,7 @@ public:
/// @{ /// @{
/** Default constructor is origin */ /** Default constructor is origin */
Pose3() { Pose3() : R_(traits<Rot3>::Identity()), t_(traits<Point3>::Identity()) {}
}
/** Copy constructor */ /** Copy constructor */
Pose3(const Pose3& pose) : Pose3(const Pose3& pose) :
@ -65,13 +64,6 @@ public:
R_(R), t_(t) { R_(R), t_(t) {
} }
#ifndef GTSAM_USE_VECTOR3_POINTS
/** Construct from R,t */
Pose3(const Rot3& R, const Vector3& t) :
R_(R), t_(t) {
}
#endif
/** Construct from Pose2 */ /** Construct from Pose2 */
explicit Pose3(const Pose2& pose2); explicit Pose3(const Pose2& pose2);

2
gtsam/geometry/Rot3.cpp
View File

@ -82,7 +82,7 @@ Unit3 Rot3::operator*(const Unit3& p) const {
Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1, Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const { OptionalJacobian<3,3> H2) const {
const Matrix3& Rt = transpose(); const Matrix3& Rt = transpose();
Point3 q(Rt * p.vector()); // q = Rt*p Point3 q(Rt * p); // q = Rt*p
const double wx = q.x(), wy = q.y(), wz = q.z(); const double wx = q.x(), wy = q.y(), wz = q.z();
if (H1) if (H1)
*H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;

36
gtsam/geometry/Rot3.h
View File

@ -171,22 +171,6 @@ namespace gtsam {
return Rot3(q); return Rot3(q);
} }
#ifndef GTSAM_USE_VECTOR3_POINTS
/**
* Convert from axis/angle representation
* @param axis is the rotation axis, unit length
* @param angle rotation angle
* @return incremental rotation
*/
static Rot3 AxisAngle(const Vector3& axis, double angle) {
#ifdef GTSAM_USE_QUATERNIONS
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis));
#else
return Rot3(SO3::AxisAngle(axis,angle));
#endif
}
#endif
/** /**
* Convert from axis/angle representation * Convert from axis/angle representation
* @param axisw is the rotation axis, unit length * @param axisw is the rotation axis, unit length
@ -197,7 +181,7 @@ namespace gtsam {
#ifdef GTSAM_USE_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis.vector())); return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis.vector()));
#else #else
return Rot3(SO3::AxisAngle(axis.vector(),angle)); return Rot3(SO3::AxisAngle(axis,angle));
#endif #endif
} }
@ -365,24 +349,6 @@ namespace gtsam {
Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2=boost::none) const; OptionalJacobian<3,3> H2=boost::none) const;
#ifndef GTSAM_USE_VECTOR3_POINTS
/// operator* for Vector3
inline Vector3 operator*(const Vector3& v) const {
return rotate(Point3(v)).vector();
}
/// rotate for Vector3
Vector3 rotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const {
return rotate(Point3(v), H1, H2).vector();
}
/// unrotate for Vector3
Vector3 unrotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const {
return unrotate(Point3(v), H1, H2).vector();
}
#endif
/// @} /// @}
/// @name Group Action on Unit3 /// @name Group Action on Unit3
/// @{ /// @{

8
gtsam/geometry/Rot3M.cpp
View File

@ -36,9 +36,9 @@ Rot3::Rot3() : rot_(I_3x3) {}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
rot_.col(0) = col1.vector(); rot_.col(0) = (Vector3)col1;
rot_.col(1) = col2.vector(); rot_.col(1) = (Vector3)col2;
rot_.col(2) = col3.vector(); rot_.col(2) = (Vector3)col3;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -121,7 +121,7 @@ Point3 Rot3::rotate(const Point3& p,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = rot_; if (H2) *H2 = rot_;
return Point3(rot_ * p.vector()); return rot_ * p;
} }
/* ************************************************************************* */ /* ************************************************************************* */

21
gtsam/geometry/Unit3.cpp
View File

@ -45,9 +45,8 @@ namespace gtsam {
Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
// 3*3 Derivative of representation with respect to point is 3*3: // 3*3 Derivative of representation with respect to point is 3*3:
Matrix3 D_p_point; Matrix3 D_p_point;
Point3 normalized = normalize(point, H ? &D_p_point : 0);
Unit3 direction; Unit3 direction;
direction.p_ = normalized.vector(); direction.p_ = normalize(point, H ? &D_p_point : 0);
if (H) if (H)
*H << direction.basis().transpose() * D_p_point; *H << direction.basis().transpose() * D_p_point;
return direction; return direction;
@ -90,22 +89,18 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
const Point3 n(p_); const Point3 n(p_);
// Get the axis of rotation with the minimum projected length of the point // Get the axis of rotation with the minimum projected length of the point
Point3 axis; Point3 axis(0, 0, 1);
double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
if ((mx <= my) && (mx <= mz)) { if ((mx <= my) && (mx <= mz)) {
axis = Point3(1.0, 0.0, 0.0); axis = Point3(1.0, 0.0, 0.0);
} else if ((my <= mx) && (my <= mz)) { } else if ((my <= mx) && (my <= mz)) {
axis = Point3(0.0, 1.0, 0.0); axis = Point3(0.0, 1.0, 0.0);
} else if ((mz <= mx) && (mz <= my)) {
axis = Point3(0.0, 0.0, 1.0);
} else {
assert(false);
} }
// Choose the direction of the first basis vector b1 in the tangent plane by crossing n with // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with
// the chosen axis. // the chosen axis.
Matrix33 H_B1_n; Matrix33 H_B1_n;
Point3 B1 = n.cross(axis, H ? &H_B1_n : 0); Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : 0);
// Normalize result to get a unit vector: b1 = B1 / |B1|. // Normalize result to get a unit vector: b1 = B1 / |B1|.
Matrix33 H_b1_B1; Matrix33 H_b1_B1;
@ -114,7 +109,7 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
// Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them.
// No need to normalize this, p and b1 are orthogonal unit vectors. // No need to normalize this, p and b1 are orthogonal unit vectors.
Matrix33 H_b2_n, H_b2_b1; Matrix33 H_b2_n, H_b2_b1;
Point3 b2 = n.cross(b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0); Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0);
// Create the basis by stacking b1 and b2. // Create the basis by stacking b1 and b2.
B_.reset(Matrix32()); B_.reset(Matrix32());
@ -176,7 +171,7 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,
// Compute the dot product of the Point3s. // Compute the dot product of the Point3s.
Matrix13 H_dot_pn, H_dot_qn; Matrix13 H_dot_pn, H_dot_qn;
double d = pn.dot(qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0); double d = gtsam::dot(pn, qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0);
if (H_p) { if (H_p) {
(*H_p) << H_dot_pn * H_pn_p; (*H_p) << H_dot_pn * H_pn_p;
@ -209,7 +204,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ
// 2D error here is projecting q into the tangent plane of this (p). // 2D error here is projecting q into the tangent plane of this (p).
Matrix62 H_B_p; Matrix62 H_B_p;
Matrix23 Bt = basis(H_p ? &H_B_p : 0).transpose(); Matrix23 Bt = basis(H_p ? &H_B_p : 0).transpose();
Vector2 xi = Bt * qn.vector(); Vector2 xi = Bt * qn;
if (H_p) { if (H_p) {
// Derivatives of each basis vector. // Derivatives of each basis vector.
@ -217,8 +212,8 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ
const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0); const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0);
// Derivatives of the two entries of xi wrt the basis vectors. // Derivatives of the two entries of xi wrt the basis vectors.
Matrix13 H_xi1_b1 = qn.vector().transpose(); Matrix13 H_xi1_b1 = qn.transpose();
Matrix13 H_xi2_b2 = qn.vector().transpose(); Matrix13 H_xi2_b2 = qn.transpose();
// Assemble dxi/dp = dxi/dB * dB/dp. // Assemble dxi/dp = dxi/dB * dB/dp.
Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p; Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p;

10
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -103,16 +103,16 @@ TEST( PinholeCamera, level2)
TEST( PinholeCamera, lookat) TEST( PinholeCamera, lookat)
{ {
// Create a level camera, looking in Y-direction // Create a level camera, looking in Y-direction
Point3 C(10.0,0.0,0.0); Point3 C(10,0,0);
Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); Camera camera = Camera::Lookat(C, Point3(0,0,0), Point3(0,0,1));
// expected // expected
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
Pose3 expected(Rot3(xc,yc,zc),C); Pose3 expected(Rot3(xc,yc,zc),C);
EXPECT(assert_equal(camera.pose(), expected)); EXPECT(assert_equal(camera.pose(), expected));
Point3 C2(30.0,0.0,10.0); Point3 C2(30,0,10);
Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0,0,1));
Matrix R = camera2.pose().rotation().matrix(); Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R; Matrix I = trans(R)*R;
@ -278,7 +278,7 @@ TEST( PinholeCamera, range0) {
double result = camera.range(point1, D1, D2); double result = camera.range(point1, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result,
1e-9); 1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7)); EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7)); EXPECT(assert_equal(Hexpected2, D2, 1e-7));

13
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -74,16 +74,16 @@ TEST(PinholeCamera, Pose) {
TEST( PinholePose, lookat) TEST( PinholePose, lookat)
{ {
// Create a level camera, looking in Y-direction // Create a level camera, looking in Y-direction
Point3 C(10.0,0.0,0.0); Point3 C(10,0,0);
Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); Camera camera = Camera::Lookat(C, Point3(0,0,0), Point3(0,0,1));
// expected // expected
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
Pose3 expected(Rot3(xc,yc,zc),C); Pose3 expected(Rot3(xc,yc,zc),C);
EXPECT(assert_equal( camera.pose(), expected)); EXPECT(assert_equal( camera.pose(), expected));
Point3 C2(30.0,0.0,10.0); Point3 C2(30,0,10);
Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); Camera camera2 = Camera::Lookat(C2, Point3(0,0,0), Point3(0,0,1));
Matrix R = camera2.pose().rotation().matrix(); Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R; Matrix I = trans(R)*R;
@ -120,7 +120,7 @@ TEST( PinholePose, backprojectInfinity)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( PinholePose, backproject2) TEST( PinholePose, backproject2)
{ {
Point3 origin; Point3 origin(0,0,0);
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K); Camera camera(Pose3(rot, origin), K);
@ -212,8 +212,7 @@ TEST( PinholePose, range0) {
double result = camera.range(point1, D1, D2); double result = camera.range(point1, D1, D2);
Matrix expectedDcamera = numericalDerivative21(range0, camera, point1); Matrix expectedDcamera = numericalDerivative21(range0, camera, point1);
Matrix expectedDpoint = numericalDerivative22(range0, camera, point1); Matrix expectedDpoint = numericalDerivative22(range0, camera, point1);
EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, 1e-9);
1e-9);
EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
} }

18
gtsam/geometry/tests/testPoint3.cpp
View File

@ -71,14 +71,14 @@ TEST( Point3, arithmetic) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Point3, equals) { TEST( Point3, equals) {
CHECK(P.equals(P)); CHECK(traits<Point3>::Equals(P,P));
Point3 Q; Point3 Q(0,0,0);
CHECK(!P.equals(Q)); CHECK(!traits<Point3>::Equals(P,Q));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Point3, dot) { TEST( Point3, dot) {
Point3 origin, ones(1, 1, 1); Point3 origin(0,0,0), ones(1, 1, 1);
CHECK(origin.dot(Point3(1, 1, 0)) == 0); CHECK(origin.dot(Point3(1, 1, 0)) == 0);
CHECK(ones.dot(Point3(1, 1, 0)) == 2); CHECK(ones.dot(Point3(1, 1, 0)) == 2);
} }
@ -111,20 +111,20 @@ TEST (Point3, norm) {
Matrix actualH; Matrix actualH;
Point3 point(3,4,5); // arbitrary point Point3 point(3,4,5); // arbitrary point
double expected = sqrt(50); double expected = sqrt(50);
EXPECT_DOUBLES_EQUAL(expected, point.norm(actualH), 1e-8); EXPECT_DOUBLES_EQUAL(expected, norm(point, actualH), 1e-8);
Matrix expectedH = numericalDerivative11<double, Point3>(norm_proxy, point); Matrix expectedH = numericalDerivative11<double, Point3>(norm_proxy, point);
EXPECT(assert_equal(expectedH, actualH, 1e-8)); EXPECT(assert_equal(expectedH, actualH, 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
double testFunc(const Point3& P, const Point3& Q) { double testFunc(const Point3& P, const Point3& Q) {
return P.distance(Q); return distance(P,Q);
} }
TEST (Point3, distance) { TEST (Point3, distance) {
Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3); Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3);
Matrix H1, H2; Matrix H1, H2;
double d = P.distance(Q, H1, H2); double d = distance(P, Q, H1, H2);
double expectedDistance = 55.542686; double expectedDistance = 55.542686;
Matrix numH1 = numericalDerivative21(testFunc, P, Q); Matrix numH1 = numericalDerivative21(testFunc, P, Q);
Matrix numH2 = numericalDerivative22(testFunc, P, Q); Matrix numH2 = numericalDerivative22(testFunc, P, Q);
@ -137,9 +137,9 @@ TEST (Point3, distance) {
TEST(Point3, cross) { TEST(Point3, cross) {
Matrix aH1, aH2; Matrix aH1, aH2;
boost::function<Point3(const Point3&, const Point3&)> f = boost::function<Point3(const Point3&, const Point3&)> f =
boost::bind(&Point3::cross, _1, _2, boost::none, boost::none); boost::bind(&gtsam::cross, _1, _2, boost::none, boost::none);
const Point3 omega(0, 1, 0), theta(4, 6, 8); const Point3 omega(0, 1, 0), theta(4, 6, 8);
omega.cross(theta, aH1, aH2); cross(omega, theta, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
} }

12
gtsam/geometry/tests/testPose3.cpp
View File

@ -62,7 +62,7 @@ TEST( Pose3, retract_first_order)
Pose3 id; Pose3 id;
Vector v = zero(6); Vector v = zero(6);
v(0) = 0.3; v(0) = 0.3;
EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),1e-2)); EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), id.retract(v),1e-2));
v(3)=0.2;v(4)=0.7;v(5)=-2; v(3)=0.2;v(4)=0.7;v(5)=-2;
EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2)); EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2));
} }
@ -72,7 +72,7 @@ TEST( Pose3, retract_expmap)
{ {
Vector v = zero(6); v(0) = 0.3; Vector v = zero(6); v(0) = 0.3;
Pose3 pose = Pose3::Expmap(v); Pose3 pose = Pose3::Expmap(v);
EXPECT(assert_equal(Pose3(R, Point3()), pose, 1e-2)); EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), pose, 1e-2));
EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2)); EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2));
} }
@ -82,7 +82,7 @@ TEST( Pose3, expmap_a_full)
Pose3 id; Pose3 id;
Vector v = zero(6); Vector v = zero(6);
v(0) = 0.3; v(0) = 0.3;
EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3()))); EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3(0,0,0))));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
EXPECT(assert_equal(Pose3(R, P),expmap_default<Pose3>(id, v),1e-5)); EXPECT(assert_equal(Pose3(R, P),expmap_default<Pose3>(id, v),1e-5));
} }
@ -93,7 +93,7 @@ TEST( Pose3, expmap_a_full2)
Pose3 id; Pose3 id;
Vector v = zero(6); Vector v = zero(6);
v(0) = 0.3; v(0) = 0.3;
EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3()))); EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3(0,0,0))));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
EXPECT(assert_equal(Pose3(R, P),expmap_default<Pose3>(id, v),1e-5)); EXPECT(assert_equal(Pose3(R, P),expmap_default<Pose3>(id, v),1e-5));
} }
@ -388,7 +388,7 @@ TEST( Pose3, transform_to_translate)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_to_rotate) TEST( Pose3, transform_to_rotate)
{ {
Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3()); Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(0,0,0));
Point3 actual = transform.transform_to(Point3(2,1,10)); Point3 actual = transform.transform_to(Point3(2,1,10));
Point3 expected(-1,2,10); Point3 expected(-1,2,10);
EXPECT(assert_equal(expected, actual, 0.001)); EXPECT(assert_equal(expected, actual, 0.001));
@ -406,7 +406,7 @@ TEST( Pose3, transform_to)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3, transform_from) TEST( Pose3, transform_from)
{ {
Point3 actual = T3.transform_from(Point3()); Point3 actual = T3.transform_from(Point3(0,0,0));
Point3 expected = Point3(1.,2.,3.); Point3 expected = Point3(1.,2.,3.);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }

4
gtsam/geometry/tests/testRot3.cpp
View File

@ -542,8 +542,8 @@ TEST(Rot3, quaternion) {
Point3 expected1 = R1*p1; Point3 expected1 = R1*p1;
Point3 expected2 = R2*p2; Point3 expected2 = R2*p2;
Point3 actual1 = Point3(q1*p1.vector()); Point3 actual1 = Point3(q1*p1);
Point3 actual2 = Point3(q2*p2.vector()); Point3 actual2 = Point3(q2*p2);
EXPECT(assert_equal(expected1, actual1)); EXPECT(assert_equal(expected1, actual1));
EXPECT(assert_equal(expected2, actual2)); EXPECT(assert_equal(expected2, actual2));

10
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -63,16 +63,16 @@ TEST( SimpleCamera, level2)
TEST( SimpleCamera, lookat) TEST( SimpleCamera, lookat)
{ {
// Create a level camera, looking in Y-direction // Create a level camera, looking in Y-direction
Point3 C(10.0,0.0,0.0); Point3 C(10,0,0);
SimpleCamera camera = SimpleCamera::Lookat(C, Point3(), Point3(0.0,0.0,1.0)); SimpleCamera camera = SimpleCamera::Lookat(C, Point3(0,0,0), Point3(0,0,1));
// expected // expected
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
Pose3 expected(Rot3(xc,yc,zc),C); Pose3 expected(Rot3(xc,yc,zc),C);
CHECK(assert_equal( camera.pose(), expected)); CHECK(assert_equal( camera.pose(), expected));
Point3 C2(30.0,0.0,10.0); Point3 C2(30,0,10);
SimpleCamera camera2 = SimpleCamera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); SimpleCamera camera2 = SimpleCamera::Lookat(C2, Point3(0,0,0), Point3(0,0,1));
Matrix R = camera2.pose().rotation().matrix(); Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R; Matrix I = trans(R)*R;
@ -100,7 +100,7 @@ TEST( SimpleCamera, backproject)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SimpleCamera, backproject2) TEST( SimpleCamera, backproject2)
{ {
Point3 origin; Point3 origin(0,0,0);
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down
SimpleCamera camera(Pose3(rot, origin), K); SimpleCamera camera(Pose3(rot, origin), K);

2
gtsam/geometry/tests/testUnit3.cpp
View File

@ -377,7 +377,7 @@ TEST(Unit3, retract_expmap) {
TEST(Unit3, Random) { TEST(Unit3, Random) {
boost::mt19937 rng(42); boost::mt19937 rng(42);
// Check that means are all zero at least // Check that means are all zero at least
Point3 expectedMean, actualMean; Point3 expectedMean(0,0,0), actualMean(0,0,0);
for (size_t i = 0; i < 100; i++) for (size_t i = 0; i < 100; i++)
actualMean = actualMean + Unit3::Random(rng).point3(); actualMean = actualMean + Unit3::Random(rng).point3();
actualMean = actualMean / 100; actualMean = actualMean / 100;

2
gtsam/geometry/triangulation.h
View File

@ -436,7 +436,7 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
BOOST_FOREACH(const CAMERA& camera, cameras) { BOOST_FOREACH(const CAMERA& camera, cameras) {
const Pose3& pose = camera.pose(); const Pose3& pose = camera.pose();
if (params.landmarkDistanceThreshold > 0 if (params.landmarkDistanceThreshold > 0
&& pose.translation().distance(point) && distance(pose.translation(), point)
> params.landmarkDistanceThreshold) > params.landmarkDistanceThreshold)
return TriangulationResult::Degenerate(); return TriangulationResult::Degenerate();
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION

8
gtsam/navigation/GPSFactor.cpp
View File

@ -25,14 +25,14 @@ namespace gtsam {
//*************************************************************************** //***************************************************************************
void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor on " << keyFormatter(key()) << "\n"; cout << s << "GPSFactor on " << keyFormatter(key()) << "\n";
nT_.print(" prior mean: "); cout << " prior mean: " << nT_ << "\n";
noiseModel_->print(" noise model: "); noiseModel_->print(" noise model: ");
} }
//*************************************************************************** //***************************************************************************
bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected); const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && nT_.equals(e->nT_, tol); return e != NULL && Base::equals(*e, tol) && traits<Point3>::Equals(nT_, e->nT_, tol);
} }
//*************************************************************************** //***************************************************************************
@ -43,7 +43,7 @@ Vector GPSFactor::evaluateError(const Pose3& p,
H->block < 3, 3 > (0, 0) << zeros(3, 3); H->block < 3, 3 > (0, 0) << zeros(3, 3);
H->block < 3, 3 > (0, 3) << p.rotation().matrix(); H->block < 3, 3 > (0, 3) << p.rotation().matrix();
} }
return p.translation().vector() -nT_.vector(); return p.translation() -nT_;
} }
//*************************************************************************** //***************************************************************************
@ -66,7 +66,7 @@ pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,
// Construct initial pose // Construct initial pose
Pose3 nTb(nRb, nT); // nTb Pose3 nTb(nRb, nT); // nTb
return make_pair(nTb, nV.vector()); return make_pair(nTb, nV);
} }
//*************************************************************************** //***************************************************************************

3
gtsam/navigation/GPSFactor.h
View File

@ -48,8 +48,7 @@ public:
typedef GPSFactor This; typedef GPSFactor This;
/** default constructor - only use for serialization */ /** default constructor - only use for serialization */
GPSFactor() { GPSFactor(): nT_(0, 0, 0) {}
}
virtual ~GPSFactor() { virtual ~GPSFactor() {
} }

11
gtsam/navigation/NavState.cpp
View File

@ -89,7 +89,7 @@ void NavState::print(const string& s) const {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool NavState::equals(const NavState& other, double tol) const { bool NavState::equals(const NavState& other, double tol) const {
return R_.equals(other.R_, tol) && t_.equals(other.t_, tol) return R_.equals(other.R_, tol) && traits<Point3>::Equals(t_, other.t_, tol)
&& equal_with_abs_tol(v_, other.v_, tol); && equal_with_abs_tol(v_, other.v_, tol);
} }
@ -121,11 +121,6 @@ Point3 NavState::operator*(const Point3& b_t) const {
return Point3(R_ * b_t + t_); return Point3(R_ * b_t + t_);
} }
//------------------------------------------------------------------------------
Velocity3 NavState::operator*(const Velocity3& b_v) const {
return Velocity3(R_ * b_v + v_);
}
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
OptionalJacobian<9, 9> H) { OptionalJacobian<9, 9> H) {
@ -189,7 +184,7 @@ Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) {
throw runtime_error("NavState::Logmap derivative not implemented yet"); throw runtime_error("NavState::Logmap derivative not implemented yet");
TIE(nRb, n_p, n_v, nTb); TIE(nRb, n_p, n_v, nTb);
Vector3 n_t = n_p.vector(); Vector3 n_t = n_p;
// NOTE(frank): See Pose3::Logmap // NOTE(frank): See Pose3::Logmap
Vector9 xi; Vector9 xi;
@ -300,7 +295,7 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
dV(xi) << ((-2.0 * dt) * omega_cross_vel); dV(xi) << ((-2.0 * dt) * omega_cross_vel);
if (secondOrder) { if (secondOrder) {
const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t.vector())); const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t));
dP(xi) -= (0.5 * dt2) * omega_cross2_t; dP(xi) -= (0.5 * dt2) * omega_cross2_t;
dV(xi) -= dt * omega_cross2_t; dV(xi) -= dt * omega_cross2_t;
} }

7
gtsam/navigation/NavState.h
View File

@ -49,7 +49,7 @@ public:
/// Default constructor /// Default constructor
NavState() : NavState() :
v_(Vector3::Zero()) { t_(0,0,0), v_(Vector3::Zero()) {
} }
/// Construct from attitude, position, velocity /// Construct from attitude, position, velocity
NavState(const Rot3& R, const Point3& t, const Velocity3& v) : NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
@ -97,7 +97,7 @@ public:
} }
/// Return position as Vector3 /// Return position as Vector3
Vector3 t() const { Vector3 t() const {
return t_.vector(); return t_;
} }
/// Return velocity as Vector3. Computation-free. /// Return velocity as Vector3. Computation-free.
const Vector3& v() const { const Vector3& v() const {
@ -147,9 +147,6 @@ public:
/// Act on position alone, n_t = nRb * b_t + n_t /// Act on position alone, n_t = nRb * b_t + n_t
Point3 operator*(const Point3& b_t) const; Point3 operator*(const Point3& b_t) const;
/// Act on velocity alone, n_v = nRb * b_v + n_v
Velocity3 operator*(const Velocity3& b_v) const;
/// @} /// @}
/// @name Manifold /// @name Manifold
/// @{ /// @{

4
gtsam/navigation/PreintegrationBase.cpp
View File

@ -94,7 +94,7 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
if (D_correctedOmega_unbiasedOmega) *D_correctedOmega_unbiasedOmega = bRs; if (D_correctedOmega_unbiasedOmega) *D_correctedOmega_unbiasedOmega = bRs;
// Centrifugal acceleration // Centrifugal acceleration
const Vector3 b_arm = p().body_P_sensor->translation().vector(); const Vector3 b_arm = p().body_P_sensor->translation();
if (!b_arm.isZero()) { if (!b_arm.isZero()) {
// Subtract out the the centripetal acceleration from the unbiased one // Subtract out the the centripetal acceleration from the unbiased one
// to get linear acceleration vector in the body frame: // to get linear acceleration vector in the body frame:
@ -188,7 +188,7 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
if (p().body_P_sensor) { if (p().body_P_sensor) {
// More complicated derivatives in case of non-trivial sensor pose // More complicated derivatives in case of non-trivial sensor pose
*C *= D_correctedOmega_omega; *C *= D_correctedOmega_omega;
if (!p().body_P_sensor->translation().vector().isZero()) if (!p().body_P_sensor->translation().isZero())
*C += *B* D_correctedAcc_omega; *C += *B* D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
} }

2
gtsam/navigation/Scenario.h
View File

@ -80,7 +80,7 @@ class AcceleratingScenario : public Scenario {
AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0, AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
const Vector3& a_n, const Vector3& a_n,
const Vector3& omega_b = Vector3::Zero()) const Vector3& omega_b = Vector3::Zero())
: nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(a_n), omega_b_(omega_b) {} : nRb_(nRb), p0_(p0), v0_(v0), a_n_(a_n), omega_b_(omega_b) {}
Pose3 pose(double t) const override { Pose3 pose(double t) const override {
return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0); return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0);

4
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -84,9 +84,9 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
} else if (const GenericValue<Point2>* p = dynamic_cast<const GenericValue<Point2>*>(&value)) { } else if (const GenericValue<Point2>* p = dynamic_cast<const GenericValue<Point2>*>(&value)) {
t << p->value().x(), p->value().y(), 0; t << p->value().x(), p->value().y(), 0;
} else if (const GenericValue<Pose3>* p = dynamic_cast<const GenericValue<Pose3>*>(&value)) { } else if (const GenericValue<Pose3>* p = dynamic_cast<const GenericValue<Pose3>*>(&value)) {
t = p->value().translation().vector(); t = p->value().translation();
} else if (const GenericValue<Point3>* p = dynamic_cast<const GenericValue<Point3>*>(&value)) { } else if (const GenericValue<Point3>* p = dynamic_cast<const GenericValue<Point3>*>(&value)) {
t = p->value().vector(); t = p->value();
} else { } else {
return boost::none; return boost::none;
} }

2
gtsam/slam/InitializePose3.cpp
View File

@ -327,7 +327,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){ BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){
Key key = key_value.key; Key key = key_value.key;
const Rot3& rot = initialRot.at<Rot3>(key); const Rot3& rot = initialRot.at<Rot3>(key);
Pose3 initializedPose = Pose3(rot, Point3()); Pose3 initializedPose = Pose3(rot, Point3(0,0,0));
initialPose.insert(key, initializedPose); initialPose.insert(key, initializedPose);
} }
// add prior // add prior

4
gtsam/slam/dataset.cpp
View File

@ -808,7 +808,7 @@ bool writeBAL(const string& filename, SfM_data &data) {
Cal3Bundler cameraCalibration = data.cameras[i].calibration(); Cal3Bundler cameraCalibration = data.cameras[i].calibration();
Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
os << Rot3::Logmap(poseOpenGL.rotation()) << endl; os << Rot3::Logmap(poseOpenGL.rotation()) << endl;
os << poseOpenGL.translation().vector() << endl; os << poseOpenGL.translation() << endl;
os << cameraCalibration.fx() << endl; os << cameraCalibration.fx() << endl;
os << cameraCalibration.k1() << endl; os << cameraCalibration.k1() << endl;
os << cameraCalibration.k2() << endl; os << cameraCalibration.k2() << endl;
@ -880,7 +880,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data,
dataValues.tracks[j].r = 1.0; dataValues.tracks[j].r = 1.0;
dataValues.tracks[j].g = 0.0; dataValues.tracks[j].g = 0.0;
dataValues.tracks[j].b = 0.0; dataValues.tracks[j].b = 0.0;
dataValues.tracks[j].p = Point3(); dataValues.tracks[j].p = Point3(0,0,0);
} }
} }

1
gtsam/slam/dataset.h
View File

@ -147,6 +147,7 @@ typedef std::pair<size_t, size_t> SIFT_Index;
/// Define the structure for the 3D points /// Define the structure for the 3D points
struct SfM_Track { struct SfM_Track {
SfM_Track():p(0,0,0) {}
Point3 p; ///< 3D position of the point Point3 p; ///< 3D position of the point
float r, g, b; ///< RGB color of the 3D point float r, g, b; ///< RGB color of the 3D point
std::vector<SfM_Measurement> measurements; ///< The 2D image projections (id,(u,v)) std::vector<SfM_Measurement> measurements; ///< The 2D image projections (id,(u,v))

2
gtsam_unstable/geometry/Event.h
View File

@ -35,7 +35,7 @@ public:
/// Default Constructor /// Default Constructor
Event() : Event() :
time_(0) { time_(0), location_(0,0,0) {
} }
/// Constructor from time and location /// Constructor from time and location

4
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -23,11 +23,11 @@
namespace gtsam { namespace gtsam {
Similarity3::Similarity3() : Similarity3::Similarity3() :
R_(), t_(), s_(1) { t_(0,0,0), s_(1) {
} }
Similarity3::Similarity3(double s) : Similarity3::Similarity3(double s) :
s_(s) { t_(0,0,0), s_(s) {
} }
Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) : Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) :

4
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -71,7 +71,7 @@ TEST(Similarity3, Constructors) {
TEST(Similarity3, Getters) { TEST(Similarity3, Getters) {
Similarity3 sim3_default; Similarity3 sim3_default;
EXPECT(assert_equal(Rot3(), sim3_default.rotation())); EXPECT(assert_equal(Rot3(), sim3_default.rotation()));
EXPECT(assert_equal(Point3(), sim3_default.translation())); EXPECT(assert_equal(Point3(0,0,0), sim3_default.translation()));
EXPECT_DOUBLES_EQUAL(1.0, sim3_default.scale(), 1e-9); EXPECT_DOUBLES_EQUAL(1.0, sim3_default.scale(), 1e-9);
Similarity3 sim3(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7); Similarity3 sim3(Rot3::Ypr(1, 2, 3), Point3(4, 5, 6), 7);
@ -158,7 +158,7 @@ TEST( Similarity3, retract_first_order) {
Similarity3 id; Similarity3 id;
Vector v = zero(7); Vector v = zero(7);
v(0) = 0.3; v(0) = 0.3;
EXPECT(assert_equal(Similarity3(R, Point3(), 1), id.retract(v), 1e-2)); EXPECT(assert_equal(Similarity3(R, Point3(0,0,0), 1), id.retract(v), 1e-2));
// v(3) = 0.2; // v(3) = 0.2;
// v(4) = 0.7; // v(4) = 0.7;
// v(5) = -2; // v(5) = -2;