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

Merge branch 'support/2.4.0/mergeDevelop'

release/4.3a0
dellaert 2014-01-25 11:10:24 -05:00
parent 6be91a2df2 a606d0eab9
commit d3a61a9bb6
15 changed files with 817 additions and 565 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
gtsam.h
View File

@ -563,6 +563,49 @@ virtual class Pose3 : gtsam::Value {
void serialize() const; void serialize() const;
}; };
#include <gtsam/geometry/Sphere2.h>
virtual class Sphere2 : gtsam::Value {
// Standard Constructors
Sphere2();
Sphere2(const gtsam::Point3& pose);
// Testable
void print(string s) const;
bool equals(const gtsam::Sphere2& pose, double tol) const;
// Other functionality
Matrix basis() const;
Matrix skew() const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Sphere2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Sphere2& s) const;
};
#include <gtsam/geometry/EssentialMatrix.h>
virtual class EssentialMatrix : gtsam::Value {
// Standard Constructors
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Sphere2& aTb);
// Testable
void print(string s) const;
bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::EssentialMatrix retract(Vector v) const;
Vector localCoordinates(const gtsam::EssentialMatrix& s) const;
// Other methods:
gtsam::Rot3 rotation() const;
gtsam::Sphere2 direction() const;
Matrix matrix() const;
double error(Vector vA, Vector vB);
};
virtual class Cal3_S2 : gtsam::Value { virtual class Cal3_S2 : gtsam::Value {
// Standard Constructors // Standard Constructors
Cal3_S2(); Cal3_S2();
@ -2273,6 +2316,12 @@ virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D; typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D;
typedef gtsam::PoseRotationPrior<gtsam::Pose3> PoseRotationPrior3D; typedef gtsam::PoseRotationPrior<gtsam::Pose3> PoseRotationPrior3D;
#include <gtsam/slam/EssentialMatrixFactor.h>
virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB,
const gtsam::noiseModel::Base* noiseModel);
};
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename, pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);

407
gtsam/geometry/Rot3.cpp
View File

@ -1,198 +1,209 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file Rot3.cpp * @file Rot3.cpp
* @brief Rotation, common code between Rotation matrix and Quaternion * @brief Rotation, common code between Rotation matrix and Quaternion
* @author Alireza Fathi * @author Alireza Fathi
* @author Christian Potthast * @author Christian Potthast
* @author Frank Dellaert * @author Frank Dellaert
* @author Richard Roberts * @author Richard Roberts
*/ */
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
#include <boost/random.hpp> #include <boost/random.hpp>
#include <cmath> #include <cmath>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
static const Matrix3 I3 = Matrix3::Identity(); static const Matrix3 I3 = Matrix3::Identity();
/* ************************************************************************* */ /* ************************************************************************* */
void Rot3::print(const std::string& s) const { void Rot3::print(const std::string& s) const {
gtsam::print((Matrix)matrix(), s); gtsam::print((Matrix)matrix(), s);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::rodriguez(const Point3& w, double theta) { Rot3 Rot3::rodriguez(const Point3& w, double theta) {
return rodriguez((Vector)w.vector(),theta); return rodriguez((Vector)w.vector(),theta);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::rodriguez(const Sphere2& w, double theta) { Rot3 Rot3::rodriguez(const Sphere2& w, double theta) {
return rodriguez(w.point3(),theta); return rodriguez(w.point3(),theta);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Random(boost::random::mt19937 & rng) { Rot3 Rot3::Random(boost::random::mt19937 & rng) {
// TODO allow any engine without including all of boost :-( // TODO allow any engine without including all of boost :-(
Sphere2 w = Sphere2::Random(rng); Sphere2 w = Sphere2::Random(rng);
boost::random::uniform_real_distribution<double> randomAngle(-M_PI,M_PI); boost::random::uniform_real_distribution<double> randomAngle(-M_PI,M_PI);
double angle = randomAngle(rng); double angle = randomAngle(rng);
return rodriguez(w,angle); return rodriguez(w,angle);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w) { Rot3 Rot3::rodriguez(const Vector& w) {
double t = w.norm(); double t = w.norm();
if (t < 1e-10) return Rot3(); if (t < 1e-10) return Rot3();
return rodriguez(w/t, t); return rodriguez(w/t, t);
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const { bool Rot3::equals(const Rot3 & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol); return equal_with_abs_tol(matrix(), R.matrix(), tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::operator*(const Point3& p) const { Point3 Rot3::operator*(const Point3& p) const {
return rotate(p); return rotate(p);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Sphere2 Rot3::rotate(const Sphere2& p, Sphere2 Rot3::rotate(const Sphere2& p,
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const { boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
Sphere2 q = rotate(p.point3(Hp)); Sphere2 q = rotate(p.point3(Hp));
if (Hp) if (Hp)
(*Hp) = q.basis().transpose() * matrix() * (*Hp); (*Hp) = q.basis().transpose() * matrix() * (*Hp);
if (HR) if (HR)
(*HR) = -q.basis().transpose() * matrix() * p.skew(); (*HR) = -q.basis().transpose() * matrix() * p.skew();
return q; return q;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Sphere2 Rot3::operator*(const Sphere2& p) const { Sphere2 Rot3::unrotate(const Sphere2& p,
return rotate(p); boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
} Sphere2 q = unrotate(p.point3(Hp));
if (Hp)
/* ************************************************************************* */ (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
// see doc/math.lyx, SO(3) section if (HR)
Point3 Rot3::unrotate(const Point3& p, (*HR) = q.basis().transpose() * q.skew();
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { return q;
const Matrix Rt(transpose()); }
Point3 q(Rt*p.vector()); // q = Rt*p
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); /* ************************************************************************* */
if (H2) *H2 = Rt; Sphere2 Rot3::operator*(const Sphere2& p) const {
return q; return rotate(p);
} }
/* ************************************************************************* */ /* ************************************************************************* */
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) // see doc/math.lyx, SO(3) section
Matrix3 Rot3::dexpL(const Vector3& v) { Point3 Rot3::unrotate(const Point3& p,
if(zero(v)) return eye(3); boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Matrix x = skewSymmetric(v); const Matrix Rt(transpose());
Matrix x2 = x*x; Point3 q(Rt*p.vector()); // q = Rt*p
double theta = v.norm(), vi = theta/2.0; if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
double s1 = sin(vi)/vi; if (H2) *H2 = Rt;
double s2 = (theta - sin(theta))/(theta*theta*theta); return q;
Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2; }
return res;
} /* ************************************************************************* */
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
/* ************************************************************************* */ Matrix3 Rot3::dexpL(const Vector3& v) {
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) if(zero(v)) return eye(3);
Matrix3 Rot3::dexpInvL(const Vector3& v) { Matrix x = skewSymmetric(v);
if(zero(v)) return eye(3); Matrix x2 = x*x;
Matrix x = skewSymmetric(v); double theta = v.norm(), vi = theta/2.0;
Matrix x2 = x*x; double s1 = sin(vi)/vi;
double theta = v.norm(), vi = theta/2.0; double s2 = (theta - sin(theta))/(theta*theta*theta);
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
Matrix res = eye(3) + 0.5*x - s2*x2; return res;
return res; }
}
/* ************************************************************************* */
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
/* ************************************************************************* */ Matrix3 Rot3::dexpInvL(const Vector3& v) {
Point3 Rot3::column(int index) const{ if(zero(v)) return eye(3);
if(index == 3) Matrix x = skewSymmetric(v);
return r3(); Matrix x2 = x*x;
else if(index == 2) double theta = v.norm(), vi = theta/2.0;
return r2(); double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
else if(index == 1) Matrix res = eye(3) + 0.5*x - s2*x2;
return r1(); // default returns r1 return res;
else }
throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
}
/* ************************************************************************* */
/* ************************************************************************* */ Point3 Rot3::column(int index) const{
Vector3 Rot3::xyz() const { if(index == 3)
Matrix I;Vector3 q; return r3();
boost::tie(I,q)=RQ(matrix()); else if(index == 2)
return q; return r2();
} else if(index == 1)
return r1(); // default returns r1
/* ************************************************************************* */ else
Vector3 Rot3::ypr() const { throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
Vector3 q = xyz(); }
return Vector3(q(2),q(1),q(0));
} /* ************************************************************************* */
Vector3 Rot3::xyz() const {
/* ************************************************************************* */ Matrix I;Vector3 q;
Vector3 Rot3::rpy() const { boost::tie(I,q)=RQ(matrix());
return xyz(); return q;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Rot3::quaternion() const { Vector3 Rot3::ypr() const {
Quaternion q = toQuaternion(); Vector3 q = xyz();
Vector v(4); return Vector3(q(2),q(1),q(0));
v(0) = q.w(); }
v(1) = q.x();
v(2) = q.y(); /* ************************************************************************* */
v(3) = q.z(); Vector3 Rot3::rpy() const {
return v; return xyz();
} }
/* ************************************************************************* */ /* ************************************************************************* */
pair<Matrix3, Vector3> RQ(const Matrix3& A) { Vector Rot3::quaternion() const {
Quaternion q = toQuaternion();
double x = -atan2(-A(2, 1), A(2, 2)); Vector v(4);
Rot3 Qx = Rot3::Rx(-x); v(0) = q.w();
Matrix3 B = A * Qx.matrix(); v(1) = q.x();
v(2) = q.y();
double y = -atan2(B(2, 0), B(2, 2)); v(3) = q.z();
Rot3 Qy = Rot3::Ry(-y); return v;
Matrix3 C = B * Qy.matrix(); }
double z = -atan2(-C(1, 0), C(1, 1)); /* ************************************************************************* */
Rot3 Qz = Rot3::Rz(-z); pair<Matrix3, Vector3> RQ(const Matrix3& A) {
Matrix3 R = C * Qz.matrix();
double x = -atan2(-A(2, 1), A(2, 2));
Vector xyz = Vector3(x, y, z); Rot3 Qx = Rot3::Rx(-x);
return make_pair(R, xyz); Matrix3 B = A * Qx.matrix();
}
double y = -atan2(B(2, 0), B(2, 2));
/* ************************************************************************* */ Rot3 Qy = Rot3::Ry(-y);
ostream &operator<<(ostream &os, const Rot3& R) { Matrix3 C = B * Qy.matrix();
os << "\n";
os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n"; double z = -atan2(-C(1, 0), C(1, 1));
os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n"; Rot3 Qz = Rot3::Rz(-z);
os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n"; Matrix3 R = C * Qz.matrix();
return os;
} Vector xyz = Vector3(x, y, z);
return make_pair(R, xyz);
/* ************************************************************************* */ }
} // namespace gtsam /* ************************************************************************* */
ostream &operator<<(ostream &os, const Rot3& R) {
os << "\n";
os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
return os;
}
/* ************************************************************************* */
} // namespace gtsam

4
gtsam/geometry/Rot3.h
View File

@ -331,6 +331,10 @@ namespace gtsam {
Sphere2 rotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none, Sphere2 rotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none,
boost::optional<Matrix&> Hp = boost::none) const; boost::optional<Matrix&> Hp = boost::none) const;
/// unrotate 3D direction from world frame to rotated coordinate frame
Sphere2 unrotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none,
boost::optional<Matrix&> Hp = boost::none) const;
/// rotate 3D direction from rotated coordinate frame to world frame /// rotate 3D direction from rotated coordinate frame to world frame
Sphere2 operator*(const Sphere2& p) const; Sphere2 operator*(const Sphere2& p) const;

616
gtsam/geometry/Rot3M.cpp
View File

@ -1,308 +1,308 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information * See LICENSE for the license information
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file Rot3M.cpp * @file Rot3M.cpp
* @brief Rotation (internal: 3*3 matrix representation*) * @brief Rotation (internal: 3*3 matrix representation*)
* @author Alireza Fathi * @author Alireza Fathi
* @author Christian Potthast * @author Christian Potthast
* @author Frank Dellaert * @author Frank Dellaert
* @author Richard Roberts * @author Richard Roberts
*/ */
#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
#ifndef GTSAM_USE_QUATERNIONS #ifndef GTSAM_USE_QUATERNIONS
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
#include <cmath> #include <cmath>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
static const Matrix3 I3 = Matrix3::Identity(); static const Matrix3 I3 = Matrix3::Identity();
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3() : rot_(Matrix3::Identity()) {} Rot3::Rot3() : rot_(Matrix3::Identity()) {}
/* ************************************************************************* */ /* ************************************************************************* */
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) = col1.vector();
rot_.col(1) = col2.vector(); rot_.col(1) = col2.vector();
rot_.col(2) = col3.vector(); rot_.col(2) = col3.vector();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(double R11, double R12, double R13, Rot3::Rot3(double R11, double R12, double R13,
double R21, double R22, double R23, double R21, double R22, double R23,
double R31, double R32, double R33) { double R31, double R32, double R33) {
rot_ << R11, R12, R13, rot_ << R11, R12, R13,
R21, R22, R23, R21, R22, R23,
R31, R32, R33; R31, R32, R33;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Matrix3& R) { Rot3::Rot3(const Matrix3& R) {
rot_ = R; rot_ = R;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Matrix& R) { Rot3::Rot3(const Matrix& R) {
if (R.rows()!=3 || R.cols()!=3) if (R.rows()!=3 || R.cols()!=3)
throw invalid_argument("Rot3 constructor expects 3*3 matrix"); throw invalid_argument("Rot3 constructor expects 3*3 matrix");
rot_ = R; rot_ = R;
} }
///* ************************************************************************* */ ///* ************************************************************************* */
//Rot3::Rot3(const Matrix3& R) : rot_(R) {} //Rot3::Rot3(const Matrix3& R) : rot_(R) {}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {} Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Rx(double t) { Rot3 Rot3::Rx(double t) {
double st = sin(t), ct = cos(t); double st = sin(t), ct = cos(t);
return Rot3( return Rot3(
1, 0, 0, 1, 0, 0,
0, ct,-st, 0, ct,-st,
0, st, ct); 0, st, ct);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Ry(double t) { Rot3 Rot3::Ry(double t) {
double st = sin(t), ct = cos(t); double st = sin(t), ct = cos(t);
return Rot3( return Rot3(
ct, 0, st, ct, 0, st,
0, 1, 0, 0, 1, 0,
-st, 0, ct); -st, 0, ct);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Rz(double t) { Rot3 Rot3::Rz(double t) {
double st = sin(t), ct = cos(t); double st = sin(t), ct = cos(t);
return Rot3( return Rot3(
ct,-st, 0, ct,-st, 0,
st, ct, 0, st, ct, 0,
0, 0, 1); 0, 0, 1);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Considerably faster than composing matrices above ! // Considerably faster than composing matrices above !
Rot3 Rot3::RzRyRx(double x, double y, double z) { Rot3 Rot3::RzRyRx(double x, double y, double z) {
double cx=cos(x),sx=sin(x); double cx=cos(x),sx=sin(x);
double cy=cos(y),sy=sin(y); double cy=cos(y),sy=sin(y);
double cz=cos(z),sz=sin(z); double cz=cos(z),sz=sin(z);
double ss_ = sx * sy; double ss_ = sx * sy;
double cs_ = cx * sy; double cs_ = cx * sy;
double sc_ = sx * cy; double sc_ = sx * cy;
double cc_ = cx * cy; double cc_ = cx * cy;
double c_s = cx * sz; double c_s = cx * sz;
double s_s = sx * sz; double s_s = sx * sz;
double _cs = cy * sz; double _cs = cy * sz;
double _cc = cy * cz; double _cc = cy * cz;
double s_c = sx * cz; double s_c = sx * cz;
double c_c = cx * cz; double c_c = cx * cz;
double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
return Rot3( return Rot3(
_cc,- c_s + ssc, s_s + csc, _cc,- c_s + ssc, s_s + csc,
_cs, c_c + sss, -s_c + css, _cs, c_c + sss, -s_c + css,
-sy, sc_, cc_ -sy, sc_, cc_
); );
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w, double theta) { Rot3 Rot3::rodriguez(const Vector& w, double theta) {
// get components of axis \omega // get components of axis \omega
double wx = w(0), wy=w(1), wz=w(2); double wx = w(0), wy=w(1), wz=w(2);
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
#ifndef NDEBUG #ifndef NDEBUG
double l_n = wwTxx + wwTyy + wwTzz; double l_n = wwTxx + wwTyy + wwTzz;
if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
#endif #endif
double c = cos(theta), s = sin(theta), c_1 = 1 - c; double c = cos(theta), s = sin(theta), c_1 = 1 - c;
double swx = wx * s, swy = wy * s, swz = wz * s; double swx = wx * s, swy = wy * s, swz = wz * s;
double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
double C11 = c_1*wwTyy, C12 = c_1*wy*wz; double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
double C22 = c_1*wwTzz; double C22 = c_1*wwTzz;
return Rot3( return Rot3(
c + C00, -swz + C01, swy + C02, c + C00, -swz + C01, swy + C02,
swz + C01, c + C11, -swx + C12, swz + C01, c + C11, -swx + C12,
-swy + C02, swx + C12, c + C22); -swy + C02, swx + C12, c + C22);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2, Rot3 Rot3::compose (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = R2.transpose(); if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3; if (H2) *H2 = I3;
return *this * R2; return *this * R2;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const { Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(Matrix3(rot_*R2.rot_)); return Rot3(Matrix3(rot_*R2.rot_));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const { Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -rot_; if (H1) *H1 = -rot_;
return Rot3(Matrix3(rot_.transpose())); return Rot3(Matrix3(rot_.transpose()));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::between (const Rot3& R2, Rot3 Rot3::between (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = -(R2.transpose()*rot_); if (H1) *H1 = -(R2.transpose()*rot_);
if (H2) *H2 = I3; if (H2) *H2 = I3;
return Rot3(Matrix3(rot_.transpose()*R2.rot_)); return Rot3(Matrix3(rot_.transpose()*R2.rot_));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p, Point3 Rot3::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1 || H2) { if (H1 || H2) {
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 Point3(rot_ * p.vector());
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation // Log map at identity - return the canonical coordinates of this rotation
Vector3 Rot3::Logmap(const Rot3& R) { Vector3 Rot3::Logmap(const Rot3& R) {
static const double PI = boost::math::constants::pi<double>(); static const double PI = boost::math::constants::pi<double>();
const Matrix3& rot = R.rot_; const Matrix3& rot = R.rot_;
// Get trace(R) // Get trace(R)
double tr = rot.trace(); double tr = rot.trace();
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special // we do something special
if (std::abs(tr+1.0) < 1e-10) { if (std::abs(tr+1.0) < 1e-10) {
if(std::abs(rot(2,2)+1.0) > 1e-10) if(std::abs(rot(2,2)+1.0) > 1e-10)
return (PI / sqrt(2.0+2.0*rot(2,2) )) * return (PI / sqrt(2.0+2.0*rot(2,2) )) *
Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2)); Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2));
else if(std::abs(rot(1,1)+1.0) > 1e-10) else if(std::abs(rot(1,1)+1.0) > 1e-10)
return (PI / sqrt(2.0+2.0*rot(1,1))) * return (PI / sqrt(2.0+2.0*rot(1,1))) *
Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1)); Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1));
else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
return (PI / sqrt(2.0+2.0*rot(0,0))) * return (PI / sqrt(2.0+2.0*rot(0,0))) *
Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); Vector3(1.0+rot(0,0), rot(1,0), rot(2,0));
} else { } else {
double magnitude; double magnitude;
double tr_3 = tr-3.0; // always negative double tr_3 = tr-3.0; // always negative
if (tr_3<-1e-7) { if (tr_3<-1e-7) {
double theta = acos((tr-1.0)/2.0); double theta = acos((tr-1.0)/2.0);
magnitude = theta/(2.0*sin(theta)); magnitude = theta/(2.0*sin(theta));
} else { } else {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3*tr_3/12.0; magnitude = 0.5 - tr_3*tr_3/12.0;
} }
return magnitude*Vector3( return magnitude*Vector3(
rot(2,1)-rot(1,2), rot(2,1)-rot(1,2),
rot(0,2)-rot(2,0), rot(0,2)-rot(2,0),
rot(1,0)-rot(0,1)); rot(1,0)-rot(0,1));
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::retractCayley(const Vector& omega) const { Rot3 Rot3::retractCayley(const Vector& omega) const {
const double x = omega(0), y = omega(1), z = omega(2); const double x = omega(0), y = omega(1), z = omega(2);
const double x2 = x * x, y2 = y * y, z2 = z * z; const double x2 = x * x, y2 = y * y, z2 = z * z;
const double xy = x * y, xz = x * z, yz = y * z; const double xy = x * y, xz = x * z, yz = y * z;
const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f; const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f;
return (*this) return (*this)
* Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
(xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f, (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f,
(xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f); (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
if(mode == Rot3::EXPMAP) { if(mode == Rot3::EXPMAP) {
return (*this)*Expmap(omega); return (*this)*Expmap(omega);
} else if(mode == Rot3::CAYLEY) { } else if(mode == Rot3::CAYLEY) {
return retractCayley(omega); return retractCayley(omega);
} else if(mode == Rot3::SLOW_CAYLEY) { } else if(mode == Rot3::SLOW_CAYLEY) {
Matrix Omega = skewSymmetric(omega); Matrix Omega = skewSymmetric(omega);
return (*this)*Cayley<3>(-Omega/2); return (*this)*Cayley<3>(-Omega/2);
} else { } else {
assert(false); assert(false);
exit(1); exit(1);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
if(mode == Rot3::EXPMAP) { if(mode == Rot3::EXPMAP) {
return Logmap(between(T)); return Logmap(between(T));
} else if(mode == Rot3::CAYLEY) { } else if(mode == Rot3::CAYLEY) {
// Create a fixed-size matrix // Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix()); Eigen::Matrix3d A(between(T).matrix());
// Mathematica closed form optimization (procrastination?) gone wild: // Mathematica closed form optimization (procrastination?) gone wild:
const double a=A(0,0),b=A(0,1),c=A(0,2); const double a=A(0,0),b=A(0,1),c=A(0,2);
const double d=A(1,0),e=A(1,1),f=A(1,2); const double d=A(1,0),e=A(1,1),f=A(1,2);
const double g=A(2,0),h=A(2,1),i=A(2,2); const double g=A(2,0),h=A(2,1),i=A(2,2);
const double di = d*i, ce = c*e, cd = c*d, fg=f*g; const double di = d*i, ce = c*e, cd = c*d, fg=f*g;
const double M = 1 + e - f*h + i + e*i; const double M = 1 + e - f*h + i + e*i;
const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg));
const double x = (a * f - cd + f) * K; const double x = (a * f - cd + f) * K;
const double y = (b * f - ce - c) * K; const double y = (b * f - ce - c) * K;
const double z = (fg - di - d) * K; const double z = (fg - di - d) * K;
return -2 * Vector3(x, y, z); return -2 * Vector3(x, y, z);
} else if(mode == Rot3::SLOW_CAYLEY) { } else if(mode == Rot3::SLOW_CAYLEY) {
// Create a fixed-size matrix // Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix()); Eigen::Matrix3d A(between(T).matrix());
// using templated version of Cayley // using templated version of Cayley
Eigen::Matrix3d Omega = Cayley<3>(A); Eigen::Matrix3d Omega = Cayley<3>(A);
return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
} else { } else {
assert(false); assert(false);
exit(1); exit(1);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::matrix() const { Matrix3 Rot3::matrix() const {
return rot_; return rot_;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::transpose() const { Matrix3 Rot3::transpose() const {
return rot_.transpose(); return rot_.transpose();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::r1() const { return Point3(rot_.col(0)); } Point3 Rot3::r1() const { return Point3(rot_.col(0)); }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::r2() const { return Point3(rot_.col(1)); } Point3 Rot3::r2() const { return Point3(rot_.col(1)); }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::r3() const { return Point3(rot_.col(2)); } Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
/* ************************************************************************* */ /* ************************************************************************* */
Quaternion Rot3::toQuaternion() const { Quaternion Rot3::toQuaternion() const {
return Quaternion(rot_); return Quaternion(rot_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace gtsam } // namespace gtsam
#endif #endif

5
gtsam/geometry/Rot3Q.cpp
View File

@ -62,11 +62,6 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) : quaternion_(q) {} Rot3::Rot3(const Quaternion& q) : quaternion_(q) {}
/* ************************************************************************* */
void Rot3::print(const std::string& s) const {
gtsam::print((Matrix)matrix(), s);
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); } Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); }

87
gtsam/geometry/Sphere2.cpp
View File

@ -14,6 +14,7 @@
* @date Feb 02, 2011 * @date Feb 02, 2011
* @author Can Erdogan * @author Can Erdogan
* @author Frank Dellaert * @author Frank Dellaert
* @author Alex Trevor
* @brief The Sphere2 class - basically a point on a unit sphere * @brief The Sphere2 class - basically a point on a unit sphere
*/ */
@ -113,7 +114,7 @@ double Sphere2::distance(const Sphere2& q, boost::optional<Matrix&> H) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Sphere2 Sphere2::retract(const Vector& v) const { Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const {
// Get the vector form of the point and the basis matrix // Get the vector form of the point and the basis matrix
Vector p = Point3::Logmap(p_); Vector p = Point3::Logmap(p_);
@ -121,35 +122,75 @@ Sphere2 Sphere2::retract(const Vector& v) const {
// Compute the 3D xi_hat vector // Compute the 3D xi_hat vector
Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1); Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
Vector newPoint = p + xi_hat;
if (mode == Sphere2::EXPMAP) {
double xi_hat_norm = xi_hat.norm();
// Project onto the manifold, i.e. the closest point on the circle to the new location; // Avoid nan
// same as putting it onto the unit circle if (xi_hat_norm == 0.0) {
Vector projected = newPoint / newPoint.norm(); if (v.norm () == 0.0)
return Sphere2 (point3 ());
else
return Sphere2 (-point3 ());
}
Vector exp_p_xi_hat = cos (xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
return Sphere2(exp_p_xi_hat);
} else if (mode == Sphere2::RENORM) {
// Project onto the manifold, i.e. the closest point on the circle to the new location;
// same as putting it onto the unit circle
Vector newPoint = p + xi_hat;
Vector projected = newPoint / newPoint.norm();
return Sphere2(Point3::Expmap(projected)); return Sphere2(Point3::Expmap(projected));
} else {
assert (false);
exit (1);
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Sphere2::localCoordinates(const Sphere2& y) const { Vector Sphere2::localCoordinates(const Sphere2& y, Sphere2::CoordinatesMode mode) const {
// Make sure that the angle different between x and y is less than 90. Otherwise, if (mode == Sphere2::EXPMAP) {
// we can project x + xi_hat from the tangent space at x to y. Matrix B = basis();
assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y.");
Vector p = Point3::Logmap(p_);
Vector q = Point3::Logmap(y.p_);
double theta = acos(p.transpose() * q);
// Get the basis matrix // the below will be nan if theta == 0.0
Matrix B = basis(); if (p == q)
return (Vector (2) << 0, 0);
// Create the vector forms of p and q (the Point3 of y). else if (p == (Vector)-q)
Vector p = Point3::Logmap(p_); return (Vector (2) << M_PI, 0);
Vector q = Point3::Logmap(y.p_);
Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta));
// Compute the basis coefficients [v0,v1] = (B'q)/(p'q). Vector result = B.transpose() * result_hat;
double alpha = p.transpose() * q;
assert(alpha != 0.0); return result;
Matrix coeffs = (B.transpose() * q) / alpha; } else if (mode == Sphere2::RENORM) {
Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0)); // Make sure that the angle different between x and y is less than 90. Otherwise,
return result; // we can project x + xi_hat from the tangent space at x to y.
assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y.");
// Get the basis matrix
Matrix B = basis();
// Create the vector forms of p and q (the Point3 of y).
Vector p = Point3::Logmap(p_);
Vector q = Point3::Logmap(y.p_);
// Compute the basis coefficients [v0,v1] = (B'q)/(p'q).
double alpha = p.transpose() * q;
assert(alpha != 0.0);
Matrix coeffs = (B.transpose() * q) / alpha;
Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0));
return result;
} else {
assert (false);
exit (1);
}
} }
/* ************************************************************************* */ /* ************************************************************************* */

21
gtsam/geometry/Sphere2.h
View File

@ -14,6 +14,7 @@
* @date Feb 02, 2011 * @date Feb 02, 2011
* @author Can Erdogan * @author Can Erdogan
* @author Frank Dellaert * @author Frank Dellaert
* @author Alex Trevor
* @brief Develop a Sphere2 class - basically a point on a unit sphere * @brief Develop a Sphere2 class - basically a point on a unit sphere
*/ */
@ -22,6 +23,10 @@
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/base/DerivedValue.h> #include <gtsam/base/DerivedValue.h>
#ifndef SPHERE2_DEFAULT_COORDINATES_MODE
#define SPHERE2_DEFAULT_COORDINATES_MODE Sphere2::RENORM
#endif
// (Cumbersome) forward declaration for random generator // (Cumbersome) forward declaration for random generator
namespace boost { namespace boost {
namespace random { namespace random {
@ -106,6 +111,13 @@ public:
return p_; return p_;
} }
/// Return unit-norm Vector
Vector unitVector(boost::optional<Matrix&> H = boost::none) const {
if (H)
*H = basis();
return (p_.vector ());
}
/// Signed, vector-valued error between two directions /// Signed, vector-valued error between two directions
Vector error(const Sphere2& q, Vector error(const Sphere2& q,
boost::optional<Matrix&> H = boost::none) const; boost::optional<Matrix&> H = boost::none) const;
@ -129,11 +141,16 @@ public:
return 2; return 2;
} }
enum CoordinatesMode {
EXPMAP, ///< Use the exponential map to retract
RENORM ///< Retract with vector addtion and renormalize.
};
/// The retract function /// The retract function
Sphere2 retract(const Vector& v) const; Sphere2 retract(const Vector& v, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const;
/// The local coordinates function /// The local coordinates function
Vector localCoordinates(const Sphere2& s) const; Vector localCoordinates(const Sphere2& s, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const;
/// @} /// @}
}; };

5
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -89,10 +89,7 @@ TEST (EssentialMatrix, rotate) {
// Derivatives // Derivatives
Matrix actH1, actH2; Matrix actH1, actH2;
try { try { bodyE.rotate(cRb, actH1, actH2);} catch(exception e) {} // avoid exception
bodyE.rotate(cRb, actH1, actH2);
} catch (exception e) {
} // avoid exception
Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), // Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), //
expH2 = numericalDerivative22(rotate_, bodyE, cRb); expH2 = numericalDerivative22(rotate_, bodyE, cRb);
EXPECT(assert_equal(expH1, actH1, 1e-8)); EXPECT(assert_equal(expH1, actH1, 1e-8));

89
gtsam/geometry/tests/testSphere2.cpp
View File

@ -13,6 +13,8 @@
* @file testSphere2.cpp * @file testSphere2.cpp
* @date Feb 03, 2012 * @date Feb 03, 2012
* @author Can Erdogan * @author Can Erdogan
* @author Frank Dellaert
* @author Alex Trevor
* @brief Tests the Sphere2 class * @brief Tests the Sphere2 class
*/ */
@ -76,10 +78,35 @@ TEST(Sphere2, rotate) {
} }
} }
//*******************************************************************************
static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) {
return R.unrotate (p);
}
TEST(Sphere2, unrotate) {
Rot3 R = Rot3::yaw(-M_PI/4.0);
Sphere2 p(1, 0, 0);
Sphere2 expected = Sphere2(1, 1, 0);
Sphere2 actual = R.unrotate (p);
EXPECT(assert_equal(expected, actual, 1e-8));
Matrix actualH, expectedH;
// Use numerical derivatives to calculate the expected Jacobian
{
expectedH = numericalDerivative21(unrotate_, R, p);
R.unrotate(p, actualH, boost::none);
EXPECT(assert_equal(expectedH, actualH, 1e-9));
}
{
expectedH = numericalDerivative22(unrotate_, R, p);
R.unrotate(p, boost::none, actualH);
EXPECT(assert_equal(expectedH, actualH, 1e-9));
}
}
//******************************************************************************* //*******************************************************************************
TEST(Sphere2, error) { TEST(Sphere2, error) {
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
r = p.retract((Vector(2) << 0.8, 0)); r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM);
EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8)); EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8));
EXPECT(assert_equal((Vector(2) << 0.447214, 0), p.error(q), 1e-5)); EXPECT(assert_equal((Vector(2) << 0.447214, 0), p.error(q), 1e-5));
EXPECT(assert_equal((Vector(2) << 0.624695, 0), p.error(r), 1e-5)); EXPECT(assert_equal((Vector(2) << 0.624695, 0), p.error(r), 1e-5));
@ -102,8 +129,8 @@ TEST(Sphere2, error) {
//******************************************************************************* //*******************************************************************************
TEST(Sphere2, distance) { TEST(Sphere2, distance) {
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
r = p.retract((Vector(2) << 0.8, 0)); r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM);
EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8); EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8);
EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8); EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8);
EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8); EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8);
@ -147,9 +174,20 @@ TEST(Sphere2, retract) {
Vector v(2); Vector v(2);
v << 0.5, 0; v << 0.5, 0;
Sphere2 expected(Point3(1, 0, 0.5)); Sphere2 expected(Point3(1, 0, 0.5));
Sphere2 actual = p.retract(v); Sphere2 actual = p.retract(v, Sphere2::RENORM);
EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(expected, actual, 1e-8));
EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::RENORM), 1e-8));
}
//*******************************************************************************
TEST(Sphere2, retract_expmap) {
Sphere2 p;
Vector v(2);
v << (M_PI/2.0), 0;
Sphere2 expected(Point3(0, 0, 1));
Sphere2 actual = p.retract(v, Sphere2::EXPMAP);
EXPECT(assert_equal(expected, actual, 1e-8));
EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::EXPMAP), 1e-8));
} }
//******************************************************************************* //*******************************************************************************
@ -199,6 +237,39 @@ TEST(Sphere2, localCoordinates_retract) {
} }
} }
//*******************************************************************************
// Let x and y be two Sphere2's.
// The equality x.localCoordinates(x.retract(v)) == v should hold.
TEST(Sphere2, localCoordinates_retract_expmap) {
size_t numIterations = 10000;
Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit =
Vector_(3, 1.0, 1.0, 1.0);
Vector minXiLimit = Vector_(2, -M_PI, -M_PI), maxXiLimit = Vector_(2, M_PI, M_PI);
for (size_t i = 0; i < numIterations; i++) {
// Sleep for the random number generator (TODO?: Better create all of them first).
sleep(0);
// Create the two Sphere2s.
// Unlike the above case, we can use any two sphers.
Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit)));
// Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit)));
Vector v12 = randomVector(minXiLimit, maxXiLimit);
// Magnitude of the rotation can be at most pi
if (v12.norm () > M_PI)
v12 = v12 / M_PI;
Sphere2 s2 = s1.retract(v12);
// Check if the local coordinates and retract return the same results.
Vector actual_v12 = s1.localCoordinates(s2);
EXPECT(assert_equal(v12, actual_v12, 1e-3));
Sphere2 actual_s2 = s1.retract(actual_v12);
EXPECT(assert_equal(s2, actual_s2, 1e-3));
}
}
//******************************************************************************* //*******************************************************************************
//TEST( Pose2, between ) //TEST( Pose2, between )
//{ //{
@ -245,13 +316,13 @@ TEST(Sphere2, Random) {
// Check that is deterministic given same random seed // Check that is deterministic given same random seed
Point3 expected(-0.667578, 0.671447, 0.321713); Point3 expected(-0.667578, 0.671447, 0.321713);
Point3 actual = Sphere2::Random(rng).point3(); Point3 actual = Sphere2::Random(rng).point3();
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected,actual,1e-5));
// Check that means are all zero at least // Check that means are all zero at least
Point3 expectedMean, actualMean; Point3 expectedMean, actualMean;
for (size_t i = 0; i < 100; i++) for (size_t i = 0; i < 100; i++)
actualMean = actualMean + Sphere2::Random(rng).point3(); actualMean = actualMean + Sphere2::Random(rng).point3();
actualMean = actualMean / 100; actualMean = actualMean/100;
EXPECT(assert_equal(expectedMean, actualMean, 0.1)); EXPECT(assert_equal(expectedMean,actualMean,0.1));
} }
//************************************************************************* //*************************************************************************

22
gtsam/geometry/tests/timePose3.cpp
View File

@ -17,23 +17,23 @@
#include <iostream> #include <iostream>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
#define TEST(TITLE,STATEMENT) \ #define TEST(TITLE,STATEMENT) \
gttic_(TITLE); \ gttic_(TITLE); \
for(int i = 0; i < n; i++) \ for(int i = 0; i < n; i++) \
STATEMENT; \ STATEMENT; \
gttoc_(TITLE); gttoc_(TITLE);
int main() int main()
{ {
int n = 5000000; int n = 5000000;
cout << "NOTE: Times are reported for " << n << " calls" << endl; cout << "NOTE: Times are reported for " << n << " calls" << endl;
double norm=sqrt(1.0+16.0+4.0); double norm=sqrt(1.0+16.0+4.0);
double x=1.0/norm, y=4.0/norm, z=2.0/norm; double x=1.0/norm, y=4.0/norm, z=2.0/norm;
@ -47,9 +47,9 @@ int main()
TEST(between, T.between(T2)) TEST(between, T.between(T2))
TEST(between_derivatives, T.between(T2,H1,H2)) TEST(between_derivatives, T.between(T2,H1,H2))
TEST(Logmap, Pose3::Logmap(T.between(T2))) TEST(Logmap, Pose3::Logmap(T.between(T2)))
// Print timings // Print timings
tictoc_print_(); tictoc_print_();
return 0; return 0;
} }

10
matlab/+gtsam/EXPECT.m Normal file
View File

@ -0,0 +1,10 @@
function EXPECT(name,assertion)
% EXPECT throw a warning if an assertion fails
%
% EXPECT(name,assertion)
% - name of test
% - assertion
if (assertion~=1)
warning(['EXPECT ' name ' fails']);
end

7
matlab/+gtsam/covarianceEllipse.m
View File

@ -1,16 +1,17 @@
function covarianceEllipse(x,P,color) function covarianceEllipse(x,P,color, k)
% covarianceEllipse plots a Gaussian as an uncertainty ellipse % covarianceEllipse plots a Gaussian as an uncertainty ellipse
% Based on Maybeck Vol 1, page 366 % Based on Maybeck Vol 1, page 366
% k=2.296 corresponds to 1 std, 68.26% of all probability % k=2.296 corresponds to 1 std, 68.26% of all probability
% k=11.82 corresponds to 3 std, 99.74% of all probability % k=11.82 corresponds to 3 std, 99.74% of all probability
% %
% covarianceEllipse(x,P,color) % covarianceEllipse(x,P,color,k)
% it is assumed x and y are the first two components of state x % it is assumed x and y are the first two components of state x
% k is scaling for std deviations, defaults to 1 std
[e,s] = eig(P(1:2,1:2)); [e,s] = eig(P(1:2,1:2));
s1 = s(1,1); s1 = s(1,1);
s2 = s(2,2); s2 = s(2,2);
k = 2.296; if nargin<4, k = 2.296; end;
[ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) ); [ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) );
line(ex,ey,'color',color); line(ex,ey,'color',color);

56
matlab/gtsam_examples/MonocularVOExample.m Normal file
View File

@ -0,0 +1,56 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GTSAM Copyright 2010-2013, Georgia Tech Research Corporation,
% Atlanta, Georgia 30332-0415
% All Rights Reserved
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
%
% See LICENSE for the license information
%
% @brief Monocular VO Example with 5 landmarks and two cameras
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% import
import gtsam.*
%% Create two cameras and corresponding essential matrix E
aRb = Rot3.Yaw(pi/2);
aTb = Point3 (0.1, 0, 0);
identity=Pose3;
aPb = Pose3(aRb, aTb);
cameraA = CalibratedCamera(identity);
cameraB = CalibratedCamera(aPb);
%% Create 5 points
P = { Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5) };
%% Project points in both cameras
for i=1:5
pA{i}= cameraA.project(P{i});
pB{i}= cameraB.project(P{i});
end
%% We start with a factor graph and add epipolar constraints to it
% Noise sigma is 1cm, assuming metric measurements
graph = NonlinearFactorGraph;
model = noiseModel.Isotropic.Sigma(1,0.01);
for i=1:5
graph.add(EssentialMatrixFactor(1, pA{i}, pB{i}, model));
end
%% Create initial estimate
initial = Values;
trueE = EssentialMatrix(aRb,Sphere2(aTb));
initialE = trueE.retract([0.1, -0.1, 0.1, 0.1, -0.1]');
initial.insert(1, initialE);
%% Optimize
parameters = LevenbergMarquardtParams;
% parameters.setVerbosity('ERROR');
optimizer = LevenbergMarquardtOptimizer(graph, initial, parameters);
result = optimizer.optimize();
%% Print result (as essentialMatrix) and error
error = graph.error(result)
E = result.at(1)

2
matlab/gtsam_examples/Pose2SLAMExample_graph.m
View File

@ -13,7 +13,7 @@
import gtsam.* import gtsam.*
%% Find data file %% Find data file
datafile = findExampleDataFile('w100-odom.graph'); datafile = findExampleDataFile('w100.graph');
%% Initialize graph, initial estimate, and odometry noise %% Initialize graph, initial estimate, and odometry noise
model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]); model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 5*pi/180]);

2
matlab/gtsam_examples/StereoVOExample.m
View File

@ -33,7 +33,7 @@ graph.add(NonlinearEqualityPose3(x1, first_pose));
%% Create realistic calibration and measurement noise model %% Create realistic calibration and measurement noise model
% format: fx fy skew cx cy baseline % format: fx fy skew cx cy baseline
K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2); K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]); stereo_model = noiseModel.Isotropic.Sigma(3,1);
%% Add measurements %% Add measurements
% pose 1 % pose 1