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;
};
#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 {
// Standard Constructors
Cal3_S2();
@ -2273,6 +2316,12 @@ virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D;
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>
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
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,
* 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
* -------------------------------------------------------------------------- */
/**
* @file Rot3.cpp
* @brief Rotation, common code between Rotation matrix and Quaternion
* @author Alireza Fathi
* @author Christian Potthast
* @author Frank Dellaert
* @author Richard Roberts
*/
#include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <boost/random.hpp>
#include <cmath>
using namespace std;
namespace gtsam {
static const Matrix3 I3 = Matrix3::Identity();
/* ************************************************************************* */
void Rot3::print(const std::string& s) const {
gtsam::print((Matrix)matrix(), s);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Point3& w, double theta) {
return rodriguez((Vector)w.vector(),theta);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Sphere2& w, double theta) {
return rodriguez(w.point3(),theta);
}
/* ************************************************************************* */
Rot3 Rot3::Random(boost::random::mt19937 & rng) {
// TODO allow any engine without including all of boost :-(
Sphere2 w = Sphere2::Random(rng);
boost::random::uniform_real_distribution<double> randomAngle(-M_PI,M_PI);
double angle = randomAngle(rng);
return rodriguez(w,angle);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w) {
double t = w.norm();
if (t < 1e-10) return Rot3();
return rodriguez(w/t, t);
}
/* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
}
/* ************************************************************************* */
Point3 Rot3::operator*(const Point3& p) const {
return rotate(p);
}
/* ************************************************************************* */
Sphere2 Rot3::rotate(const Sphere2& p,
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
Sphere2 q = rotate(p.point3(Hp));
if (Hp)
(*Hp) = q.basis().transpose() * matrix() * (*Hp);
if (HR)
(*HR) = -q.basis().transpose() * matrix() * p.skew();
return q;
}
/* ************************************************************************* */
Sphere2 Rot3::operator*(const Sphere2& p) const {
return rotate(p);
}
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
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;
return q;
}
/* ************************************************************************* */
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpL(const Vector3& v) {
if(zero(v)) return eye(3);
Matrix x = skewSymmetric(v);
Matrix x2 = x*x;
double theta = v.norm(), vi = theta/2.0;
double s1 = sin(vi)/vi;
double s2 = (theta - sin(theta))/(theta*theta*theta);
Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
return res;
}
/* ************************************************************************* */
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpInvL(const Vector3& v) {
if(zero(v)) return eye(3);
Matrix x = skewSymmetric(v);
Matrix x2 = x*x;
double theta = v.norm(), vi = theta/2.0;
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
Matrix res = eye(3) + 0.5*x - s2*x2;
return res;
}
/* ************************************************************************* */
Point3 Rot3::column(int index) const{
if(index == 3)
return r3();
else if(index == 2)
return r2();
else if(index == 1)
return r1(); // default returns r1
else
throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
}
/* ************************************************************************* */
Vector3 Rot3::xyz() const {
Matrix I;Vector3 q;
boost::tie(I,q)=RQ(matrix());
return q;
}
/* ************************************************************************* */
Vector3 Rot3::ypr() const {
Vector3 q = xyz();
return Vector3(q(2),q(1),q(0));
}
/* ************************************************************************* */
Vector3 Rot3::rpy() const {
return xyz();
}
/* ************************************************************************* */
Vector Rot3::quaternion() const {
Quaternion q = toQuaternion();
Vector v(4);
v(0) = q.w();
v(1) = q.x();
v(2) = q.y();
v(3) = q.z();
return v;
}
/* ************************************************************************* */
pair<Matrix3, Vector3> RQ(const Matrix3& A) {
double x = -atan2(-A(2, 1), A(2, 2));
Rot3 Qx = Rot3::Rx(-x);
Matrix3 B = A * Qx.matrix();
double y = -atan2(B(2, 0), B(2, 2));
Rot3 Qy = Rot3::Ry(-y);
Matrix3 C = B * Qy.matrix();
double z = -atan2(-C(1, 0), C(1, 1));
Rot3 Qz = Rot3::Rz(-z);
Matrix3 R = C * Qz.matrix();
Vector xyz = Vector3(x, y, z);
return make_pair(R, xyz);
}
/* ************************************************************************* */
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
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, 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
* -------------------------------------------------------------------------- */
/**
* @file Rot3.cpp
* @brief Rotation, common code between Rotation matrix and Quaternion
* @author Alireza Fathi
* @author Christian Potthast
* @author Frank Dellaert
* @author Richard Roberts
*/
#include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <boost/random.hpp>
#include <cmath>
using namespace std;
namespace gtsam {
static const Matrix3 I3 = Matrix3::Identity();
/* ************************************************************************* */
void Rot3::print(const std::string& s) const {
gtsam::print((Matrix)matrix(), s);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Point3& w, double theta) {
return rodriguez((Vector)w.vector(),theta);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Sphere2& w, double theta) {
return rodriguez(w.point3(),theta);
}
/* ************************************************************************* */
Rot3 Rot3::Random(boost::random::mt19937 & rng) {
// TODO allow any engine without including all of boost :-(
Sphere2 w = Sphere2::Random(rng);
boost::random::uniform_real_distribution<double> randomAngle(-M_PI,M_PI);
double angle = randomAngle(rng);
return rodriguez(w,angle);
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w) {
double t = w.norm();
if (t < 1e-10) return Rot3();
return rodriguez(w/t, t);
}
/* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
}
/* ************************************************************************* */
Point3 Rot3::operator*(const Point3& p) const {
return rotate(p);
}
/* ************************************************************************* */
Sphere2 Rot3::rotate(const Sphere2& p,
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
Sphere2 q = rotate(p.point3(Hp));
if (Hp)
(*Hp) = q.basis().transpose() * matrix() * (*Hp);
if (HR)
(*HR) = -q.basis().transpose() * matrix() * p.skew();
return q;
}
/* ************************************************************************* */
Sphere2 Rot3::unrotate(const Sphere2& 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);
if (HR)
(*HR) = q.basis().transpose() * q.skew();
return q;
}
/* ************************************************************************* */
Sphere2 Rot3::operator*(const Sphere2& p) const {
return rotate(p);
}
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
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;
return q;
}
/* ************************************************************************* */
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpL(const Vector3& v) {
if(zero(v)) return eye(3);
Matrix x = skewSymmetric(v);
Matrix x2 = x*x;
double theta = v.norm(), vi = theta/2.0;
double s1 = sin(vi)/vi;
double s2 = (theta - sin(theta))/(theta*theta*theta);
Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
return res;
}
/* ************************************************************************* */
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpInvL(const Vector3& v) {
if(zero(v)) return eye(3);
Matrix x = skewSymmetric(v);
Matrix x2 = x*x;
double theta = v.norm(), vi = theta/2.0;
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
Matrix res = eye(3) + 0.5*x - s2*x2;
return res;
}
/* ************************************************************************* */
Point3 Rot3::column(int index) const{
if(index == 3)
return r3();
else if(index == 2)
return r2();
else if(index == 1)
return r1(); // default returns r1
else
throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
}
/* ************************************************************************* */
Vector3 Rot3::xyz() const {
Matrix I;Vector3 q;
boost::tie(I,q)=RQ(matrix());
return q;
}
/* ************************************************************************* */
Vector3 Rot3::ypr() const {
Vector3 q = xyz();
return Vector3(q(2),q(1),q(0));
}
/* ************************************************************************* */
Vector3 Rot3::rpy() const {
return xyz();
}
/* ************************************************************************* */
Vector Rot3::quaternion() const {
Quaternion q = toQuaternion();
Vector v(4);
v(0) = q.w();
v(1) = q.x();
v(2) = q.y();
v(3) = q.z();
return v;
}
/* ************************************************************************* */
pair<Matrix3, Vector3> RQ(const Matrix3& A) {
double x = -atan2(-A(2, 1), A(2, 2));
Rot3 Qx = Rot3::Rx(-x);
Matrix3 B = A * Qx.matrix();
double y = -atan2(B(2, 0), B(2, 2));
Rot3 Qy = Rot3::Ry(-y);
Matrix3 C = B * Qy.matrix();
double z = -atan2(-C(1, 0), C(1, 1));
Rot3 Qz = Rot3::Rz(-z);
Matrix3 R = C * Qz.matrix();
Vector xyz = Vector3(x, y, z);
return make_pair(R, xyz);
}
/* ************************************************************************* */
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,
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
Sphere2 operator*(const Sphere2& p) const;

616
gtsam/geometry/Rot3M.cpp
View File

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

5
gtsam/geometry/Rot3Q.cpp
View File

@ -62,11 +62,6 @@ namespace gtsam {
/* ************************************************************************* */
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())); }

87
gtsam/geometry/Sphere2.cpp
View File

@ -14,6 +14,7 @@
* @date Feb 02, 2011
* @author Can Erdogan
* @author Frank Dellaert
* @author Alex Trevor
* @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
Vector p = Point3::Logmap(p_);
@ -121,35 +122,75 @@ Sphere2 Sphere2::retract(const Vector& v) const {
// Compute the 3D xi_hat vector
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;
// same as putting it onto the unit circle
Vector projected = newPoint / newPoint.norm();
// Avoid nan
if (xi_hat_norm == 0.0) {
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,
// 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.");
if (mode == Sphere2::EXPMAP) {
Matrix B = basis();
Vector p = Point3::Logmap(p_);
Vector q = Point3::Logmap(y.p_);
double theta = acos(p.transpose() * q);
// 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;
// the below will be nan if theta == 0.0
if (p == q)
return (Vector (2) << 0, 0);
else if (p == (Vector)-q)
return (Vector (2) << M_PI, 0);
Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta));
Vector result = B.transpose() * result_hat;
return result;
} else if (mode == Sphere2::RENORM) {
// Make sure that the angle different between x and y is less than 90. Otherwise,
// 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
* @author Can Erdogan
* @author Frank Dellaert
* @author Alex Trevor
* @brief Develop a Sphere2 class - basically a point on a unit sphere
*/
@ -22,6 +23,10 @@
#include <gtsam/geometry/Point3.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
namespace boost {
namespace random {
@ -106,6 +111,13 @@ public:
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
Vector error(const Sphere2& q,
boost::optional<Matrix&> H = boost::none) const;
@ -129,11 +141,16 @@ public:
return 2;
}
enum CoordinatesMode {
EXPMAP, ///< Use the exponential map to retract
RENORM ///< Retract with vector addtion and renormalize.
};
/// 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
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
Matrix actH1, actH2;
try {
bodyE.rotate(cRb, actH1, actH2);
} catch (exception e) {
} // avoid exception
try { bodyE.rotate(cRb, actH1, actH2);} catch(exception e) {} // avoid exception
Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), //
expH2 = numericalDerivative22(rotate_, bodyE, cRb);
EXPECT(assert_equal(expH1, actH1, 1e-8));

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

@ -13,6 +13,8 @@
* @file testSphere2.cpp
* @date Feb 03, 2012
* @author Can Erdogan
* @author Frank Dellaert
* @author Alex Trevor
* @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) {
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
r = p.retract((Vector(2) << 0.8, 0));
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
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.447214, 0), p.error(q), 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) {
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
r = p.retract((Vector(2) << 0.8, 0));
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), //
r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM);
EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8);
EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8);
EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8);
@ -147,9 +174,20 @@ TEST(Sphere2, retract) {
Vector v(2);
v << 0.5, 0;
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(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 )
//{
@ -245,13 +316,13 @@ TEST(Sphere2, Random) {
// Check that is deterministic given same random seed
Point3 expected(-0.667578, 0.671447, 0.321713);
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
Point3 expectedMean, actualMean;
for (size_t i = 0; i < 100; i++)
actualMean = actualMean + Sphere2::Random(rng).point3();
actualMean = actualMean / 100;
EXPECT(assert_equal(expectedMean, actualMean, 0.1));
actualMean = actualMean/100;
EXPECT(assert_equal(expectedMean,actualMean,0.1));
}
//*************************************************************************

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

@ -17,23 +17,23 @@
#include <iostream>
#include <gtsam/base/timing.h>
#include <gtsam/base/timing.h>
#include <gtsam/geometry/Pose3.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
/* ************************************************************************* */
#define TEST(TITLE,STATEMENT) \
gttic_(TITLE); \
for(int i = 0; i < n; i++) \
STATEMENT; \
gttoc_(TITLE);
gttic_(TITLE); \
for(int i = 0; i < n; i++) \
STATEMENT; \
gttoc_(TITLE);
int main()
{
int n = 5000000;
cout << "NOTE: Times are reported for " << n << " calls" << endl;
int n = 5000000;
cout << "NOTE: Times are reported for " << n << " calls" << endl;
double norm=sqrt(1.0+16.0+4.0);
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_derivatives, T.between(T2,H1,H2))
TEST(Logmap, Pose3::Logmap(T.between(T2)))
// Print timings
tictoc_print_();
// Print timings
tictoc_print_();
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
% Based on Maybeck Vol 1, page 366
% k=2.296 corresponds to 1 std, 68.26% 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
% k is scaling for std deviations, defaults to 1 std
[e,s] = eig(P(1:2,1:2));
s1 = s(1,1);
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) );
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.*
%% Find data file
datafile = findExampleDataFile('w100-odom.graph');
datafile = findExampleDataFile('w100.graph');
%% Initialize graph, initial estimate, and odometry noise
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
% format: fx fy skew cx cy baseline
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
% pose 1