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

Large refactoring - made several Lie group functions global, which used to be member functions, to treat Lie groups more uniformly. Also created Lie.h, and a preprocessor flag in numericalDerivative to change the coordinate frame derivatives are reported in. gtsam and easylib build and pass unit tests, but this will probably break other projects, which will require a few small changes to work again. Email coming in a few minutes to describe the changes.

release/4.3a0
Richard Roberts 2010-01-08 00:40:17 +00:00
parent 1bc9727e9e
commit 19a3e228d7
41 changed files with 1182 additions and 1009 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
cpp/Cal3_S2.h
View File

@ -54,25 +54,11 @@ namespace gtsam {
*/
bool equals(const Cal3_S2& K, double tol = 10e-9) const;
/**
* return DOF, dimensionality of tangent space
*/
size_t dim() const {
return 5;
}
/**
* load calibration from location (default name is calibration_info.txt)
*/
Cal3_S2(const std::string &path);
/**
* Given 5-dim tangent vector, create new calibration
*/
Cal3_S2 exmap(const Vector& d) const {
return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4));
}
/**
* return the principal point
*/
@ -107,6 +93,7 @@ namespace gtsam {
/** friends */
friend Matrix Duncalibrate2(const Cal3_S2& K, const Point2& p);
friend Cal3_S2 expmap(const Cal3_S2& cal, const Vector& d);
private:
/** Serialization function */
@ -124,6 +111,19 @@ namespace gtsam {
typedef boost::shared_ptr<Cal3_S2> shared_ptrK;
/**
* return DOF, dimensionality of tangent space
*/
inline size_t dim(const Cal3_S2&) { return 5; }
/**
* Given 5-dim tangent vector, create new calibration
*/
inline Cal3_S2 expmap(const Cal3_S2& cal, const Vector& d) {
return Cal3_S2(cal.fx_ + d(0), cal.fy_ + d(1),
cal.s_ + d(2), cal.u0_ + d(3), cal.v0_ + d(4));
}
/**
* convert intrinsic coordinates xy to image coordinates uv
*/

3
cpp/FactorGraph-inl.h
View File

@ -25,9 +25,6 @@
#include "Ordering.h"
#include "FactorGraph.h"
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)

3
cpp/GaussianFactorGraph.cpp
View File

@ -286,7 +286,7 @@ VectorConfig GaussianFactorGraph::optimalUpdate(const VectorConfig& x,
cout << alpha << endl;
// return updated estimate by stepping in direction d
return x.exmap(d.scale(alpha));
return expmap(x, d.scale(alpha));
}
/* ************************************************************************* */
@ -319,3 +319,4 @@ boost::shared_ptr<VectorConfig> GaussianFactorGraph::conjugateGradientDescent_(
}
/* ************************************************************************* */

1
cpp/GaussianFactorGraph.h
View File

@ -218,7 +218,6 @@ namespace gtsam {
boost::shared_ptr<VectorConfig> conjugateGradientDescent_(
const VectorConfig& x0, bool verbose = false, double epsilon = 1e-3,
size_t maxIterations = 0) const;
};
}

44
cpp/Lie.h Normal file
View File

@ -0,0 +1,44 @@
/*
* Lie.h
*
* Created on: Jan 5, 2010
* Author: Richard Roberts
*/
#ifndef LIE_H_
#define LIE_H_
#include "Vector.h"
#include "VectorConfig.h"
#include "Matrix.h"
namespace gtsam {
template<class T>
T expmap(const Vector& v); /* Exponential map about identity */
// Syntactic sugar
template<class T>
inline T operator*(const T& l1, const T& l0) { return compose(l1, l0); }
// The following functions may be overridden in your own class file
// with more efficient versions if possible.
// Compute l1 s.t. l2=l1*l0
template<class T>
inline T between(const T& l0, const T& l2) { return l2*inverse(l0); }
// Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp
template<class T>
inline Vector logmap(const T& l0, const T& lp) { return logmap(between(l0,lp)); }
/* Exponential map centered at l0, s.t. exp(l0,v) = exp(v)*l0 */
template<class T>
inline T expmap(const T& l0, const Vector& v) { return expmap<T>(v)*l0; }
}
#endif /* LIE_H_ */

1
cpp/Makefile.am
View File

@ -157,6 +157,7 @@ testSQPOptimizer_SOURCES = $(example) testSQPOptimizer.cpp
testSQPOptimizer_LDADD = libgtsam.la
# geometry
headers += Lie.h
sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
check_PROGRAMS += testPoint2 testRot2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2
testPoint2_SOURCES = testPoint2.cpp

2
cpp/NonlinearFactorGraph.h
View File

@ -10,6 +10,8 @@
#pragma once
#include <math.h>
#include "NonlinearFactor.h"
#include "GaussianFactorGraph.h"

4
cpp/NonlinearOptimizer-inl.h
View File

@ -82,7 +82,7 @@ namespace gtsam {
delta.print("delta");
// take old config and update it
shared_config newConfig(new C(config_->exmap(delta)));
shared_config newConfig(new C(expmap(*config_,delta)));
// maybe show output
if (verbosity >= CONFIG)
@ -134,7 +134,7 @@ namespace gtsam {
delta.print("delta");
// update config
shared_config newConfig(new C(config_->exmap(delta))); // TODO: updateConfig
shared_config newConfig(new C(expmap(*config_,delta))); // TODO: updateConfig
if (verbosity >= TRYCONFIG)
newConfig->print("config");

26
cpp/Point2.cpp
View File

@ -10,21 +10,21 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
void Point2::print(const string& s) const {
cout << s << "(" << x_ << ", " << y_ << ")" << endl;
}
/* ************************************************************************* */
void Point2::print(const string& s) const {
cout << s << "(" << x_ << ", " << y_ << ")" << endl;
}
/* ************************************************************************* */
bool Point2::equals(const Point2& q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol);
}
/* ************************************************************************* */
bool Point2::equals(const Point2& q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol);
}
/* ************************************************************************* */
double Point2::dist(const Point2& p2) const {
return sqrt(pow(x() - p2.x(), 2.0) + pow(y() - p2.y(), 2.0));
}
/* ************************************************************************* */
double Point2::dist(const Point2& p2) const {
return sqrt(pow(x() - p2.x(), 2.0) + pow(y() - p2.y(), 2.0));
}
/* ************************************************************************* */
/* ************************************************************************* */
} // namespace gtsam

41
cpp/Point2.h
View File

@ -9,6 +9,7 @@
#include <boost/serialization/nvp.hpp>
#include "Vector.h"
#include "Testable.h"
#include "Lie.h"
namespace gtsam {
@ -37,20 +38,8 @@ namespace gtsam {
double x() const {return x_;}
double y() const {return y_;}
/** return DOF, dimensionality of tangent space */
size_t dim() const { return 2;}
/** Given 2-dim tangent vector, create new point */
Point2 exmap(const Vector& d) const {
return Point2(x_+d(0),y_+d(1));
}
/** return vectorized form (column-wise) */
Vector vector() const {
Vector v(2);
v(0)=x_;v(1)=y_;
return v;
}
Vector vector() const { return Vector_(2, x_, y_); }
/** operators */
inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;}
@ -71,5 +60,31 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_NVP(y_);
}
};
// Lie group functions
// Dimensionality of the tangent space
inline size_t dim(const Point2& obj) { return 2; }
// Exponential map around identity - just create a Point2 from a vector
template<> inline Point2 expmap(const Vector& dp) { return Point2(dp); }
// Log map around identity - just return the Point2 as a vector
inline Vector logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); }
// "Compose", just adds the coordinates of two points.
inline Point2 compose(const Point2& p1, const Point2& p0) { return p0+p1; }
inline Matrix Dcompose1(const Point2& p1, const Point2& p0) {
return Matrix_(2,2,
1.0, 0.0,
0.0, 1.0); }
inline Matrix Dcompose2(const Point2& p1, const Point2& p0) {
return Matrix_(2,2,
1.0, 0.0,
0.0, 1.0); }
// "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2()
inline Point2 inverse(const Point2& p) { return Point2(-p.x(), -p.y()); }
}

139
cpp/Point3.cpp
View File

@ -8,78 +8,81 @@
namespace gtsam {
/* ************************************************************************* */
bool Point3::equals(const Point3 & q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol);
}
/* ************************************************************************* */
/* ************************************************************************* */
bool Point3::equals(const Point3 & q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol && fabs(z_ - q.z()) < tol);
}
bool Point3::operator== (const Point3& q) const {
return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
}
void Point3::print(const std::string& s) const {
std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl;
}
/* ************************************************************************* */
Point3 Point3::operator+(const Point3& q) const {
return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ );
}
/* ************************************************************************* */
/* ************************************************************************* */
Point3 Point3::operator- (const Point3& q) const {
return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ );
}
/* ************************************************************************* */
Point3 Point3::operator*(double s) const {
return Point3(x_ * s, y_ * s, z_ * s);
}
/* ************************************************************************* */
Point3 Point3::operator/(double s) const {
return Point3(x_ / s, y_ / s, z_ / s);
}
/* ************************************************************************* */
Point3 operator*(double s, const Point3& p) { return p*s;}
bool Point3::operator== (const Point3& q) const {
return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
}
/* ************************************************************************* */
Point3 add(const Point3 &p, const Point3 &q) {
return p+q;
}
/* ************************************************************************* */
Matrix Dadd1(const Point3 &p, const Point3 &q) {
return eye(3,3);
}
/* ************************************************************************* */
Matrix Dadd2(const Point3 &p, const Point3 &q) {
return eye(3,3);
}
/* ************************************************************************* */
Point3 sub(const Point3 &p, const Point3 &q) {
return p+q;
}
/* ************************************************************************* */
Matrix Dsub1(const Point3 &p, const Point3 &q) {
return eye(3,3);
}
/* ************************************************************************* */
Matrix Dsub2(const Point3 &p, const Point3 &q) {
return -eye(3,3);
}
/* ************************************************************************* */
Point3 cross(const Point3 &p, const Point3 &q)
{
return Point3( p.y_*q.z_ - p.z_*q.y_,
p.z_*q.x_ - p.x_*q.z_,
p.x_*q.y_ - p.y_*q.x_ );
}
/* ************************************************************************* */
double dot(const Point3 &p, const Point3 &q)
{
return ( p.x_*q.x_ + p.y_*q.y_ + p.z_*q.z_ );
}
/* ************************************************************************* */
double norm(const Point3 &p)
{
return sqrt( p.x_*p.x_ + p.y_*p.y_ + p.z_*p.z_ );
}
/* ************************************************************************* */
/* ************************************************************************* */
Point3 Point3::operator+(const Point3& q) const {
return Point3( x_ + q.x_, y_ + q.y_, z_ + q.z_ );
}
/* ************************************************************************* */
Point3 Point3::operator- (const Point3& q) const {
return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ );
}
/* ************************************************************************* */
Point3 Point3::operator*(double s) const {
return Point3(x_ * s, y_ * s, z_ * s);
}
/* ************************************************************************* */
Point3 Point3::operator/(double s) const {
return Point3(x_ / s, y_ / s, z_ / s);
}
/* ************************************************************************* */
Point3 add(const Point3 &p, const Point3 &q) {
return p+q;
}
/* ************************************************************************* */
Matrix Dadd1(const Point3 &p, const Point3 &q) {
return eye(3,3);
}
/* ************************************************************************* */
Matrix Dadd2(const Point3 &p, const Point3 &q) {
return eye(3,3);
}
/* ************************************************************************* */
Point3 sub(const Point3 &p, const Point3 &q) {
return p+q;
}
/* ************************************************************************* */
Matrix Dsub1(const Point3 &p, const Point3 &q) {
return eye(3,3);
}
/* ************************************************************************* */
Matrix Dsub2(const Point3 &p, const Point3 &q) {
return -eye(3,3);
}
/* ************************************************************************* */
Point3 cross(const Point3 &p, const Point3 &q)
{
return Point3( p.y_*q.z_ - p.z_*q.y_,
p.z_*q.x_ - p.x_*q.z_,
p.x_*q.y_ - p.y_*q.x_ );
}
/* ************************************************************************* */
double dot(const Point3 &p, const Point3 &q)
{
return ( p.x_*q.x_ + p.y_*q.y_ + p.z_*q.z_ );
}
/* ************************************************************************* */
double norm(const Point3 &p)
{
return sqrt( p.x_*p.x_ + p.y_*p.y_ + p.z_*p.z_ );
}
/* ************************************************************************* */
} // namespace gtsam

45
cpp/Point3.h
View File

@ -14,6 +14,7 @@
#include "Matrix.h"
#include "Testable.h"
#include "Lie.h"
namespace gtsam {
@ -29,19 +30,11 @@ namespace gtsam {
Point3(const Vector& v) : x_(v(0)), y_(v(1)), z_(v(2)) {}
/** print with optional string */
void print(const std::string& s = "") const {
std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl;
}
void print(const std::string& s = "") const;
/** equals with an tolerance */
bool equals(const Point3& p, double tol = 1e-9) const;
/** return DOF, dimensionality of tangent space */
size_t dim() const { return 3;}
/** Given 3-dim tangent vector, create new rotation*/
Point3 exmap(const Vector& d) const { return *this + d; }
/** return vectorized form (column-wise)*/
Vector vector() const {
//double r[] = { x_, y_, z_ };
@ -84,7 +77,39 @@ namespace gtsam {
}
};
Point3 operator*(double s, const Point3& p);
/** return DOF, dimensionality of tangent space */
// Dimensionality of the tangent space
inline size_t dim(const Point3&) { return 3; }
// Exponential map at identity - just create a Point3 from x,y,z
template<> inline Point3 expmap(const Vector& dp) { return Point3(dp); }
// Log map at identity - return the x,y,z of this point
inline Vector logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); }
// "Compose" - just adds coordinates of two points
inline Point3 compose(const Point3& p1, const Point3& p0) { return p0+p1; }
inline Matrix Dcompose1(const Point3& p1, const Point3& p0) {
return Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
}
inline Matrix Dcompose2(const Point3& p1, const Point3& p0) {
return Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
}
// "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3()
inline Point3 inverse(const Point3& p) { return Point3(-p.x(), -p.y(), -p.z()); }
// Syntactic sugar for multiplying coordinates by a scalar s*p
inline Point3 operator*(double s, const Point3& p) { return p*s;}
/** add two points, add(p,q) is same as p+q */
Point3 add (const Point3 &p, const Point3 &q);

28
cpp/Pose2.cpp
View File

@ -20,16 +20,6 @@ namespace gtsam {
}
/* ************************************************************************* */
// Pose2 Pose2::rotate(double theta) const {
// //return Pose2(t_, Rot2(theta)*r_);
// return Pose2(Point2(0.0,0.0),-theta)*(*this);
// }
/* ************************************************************************* */
Point2 transform_to(const Pose2& pose, const Point2& point) {
return pose*point;
}
// TODO, have a combined function that returns both function value and derivative
Matrix Dtransform_to1(const Pose2& pose, const Point2& point) {
Matrix dx_dt = Matrix_(2,2, -1.0, 0.0, 0.0, -1.0);
@ -42,24 +32,18 @@ namespace gtsam {
}
/* ************************************************************************* */
Pose2 between(const Pose2& p1, const Pose2& p2) {
return Pose2(
p1.r().invcompose(p2.r()),
p1.r().unrotate(p2.t() - p1.t()));
}
Matrix Dbetween1(const Pose2& p1, const Pose2& p2) {
Matrix dt_dr = Dunrotate1(p1.r(), p2.t()-p1.t());
Matrix dt_dt1 = -p2.r().invcompose(p1.r()).matrix();
Matrix dt_dr1 = Dunrotate1(p2.r(), p2.t()-p1.t());
Matrix Dbetween1(const Pose2& p0, const Pose2& p2) {
Matrix dt_dr = Dunrotate1(p0.r(), p2.t()-p0.t());
Matrix dt_dt1 = -invcompose(p2.r(), p0.r()).matrix();
Matrix dt_dr1 = Dunrotate1(p2.r(), p2.t()-p0.t());
return Matrix_(3,3,
dt_dt1(0,0), dt_dt1(0,1), dt_dr1(0,0),
dt_dt1(1,0), dt_dt1(1,1), dt_dr1(1,0),
0.0, 0.0, -1.0);
}
Matrix Dbetween2(const Pose2& p1, const Pose2& p2) {
Matrix db_dt2 = p1.r().transpose();
Matrix Dbetween2(const Pose2& p0, const Pose2& p2) {
Matrix db_dt2 = p0.r().transpose();
return Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,

110
cpp/Pose2.h
View File

@ -13,29 +13,14 @@
#include "Rot2.h"
#include "Matrix.h"
#include "Testable.h"
#include "Lie.h"
namespace gtsam {
/**
* Return point coordinates in pose coordinate frame
*/
class Pose2;
Point2 transform_to(const Pose2& pose, const Point2& point);
Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p1, const Pose2& p2);
Matrix Dbetween1(const Pose2& p1, const Pose2& p2);
Matrix Dbetween2(const Pose2& p1, const Pose2& p2);
/**
* A 2D pose (Point2,Rot2)
*/
class Pose2: Testable<Pose2> {
private:
Point2 t_;
Rot2 r_;
@ -43,12 +28,10 @@ namespace gtsam {
public:
/** default constructor = origin */
Pose2() :
t_(0.0, 0.0), r_(0) { } // default is origin
Pose2() : t_(0.0, 0.0), r_(0) {} // default is origin
/** copy constructor */
Pose2(const Pose2& pose) :
t_(pose.t_), r_(pose.r_) { }
Pose2(const Pose2& pose) : t_(pose.t_), r_(pose.r_) {}
/**
* construct from (x,y,theta)
@ -56,14 +39,11 @@ namespace gtsam {
* @param y y coordinate
* @param theta angle with positive X-axis
*/
Pose2(double x, double y, double theta) :
t_(x,y), r_(theta) { }
Pose2(double x, double y, double theta) : t_(x,y), r_(theta) {}
/** construct from rotation and translation */
Pose2(double theta, const Point2& t) :
t_(t), r_(theta) { }
Pose2(const Rot2& r, const Point2& t) :
t_(t), r_(r) { }
Pose2(double theta, const Point2& t) : t_(t), r_(theta) {}
Pose2(const Rot2& r, const Point2& t) : t_(t), r_(r) {}
/** print with optional string */
void print(const std::string& s = "") const;
@ -72,39 +52,57 @@ namespace gtsam {
bool equals(const Pose2& pose, double tol = 1e-9) const;
/** get functions for x, y, theta */
double x() const { return t_.x();}
double y() const { return t_.y();}
double theta() const { return r_.theta();}
double x() const { return t_.x(); }
double y() const { return t_.y(); }
double theta() const { return r_.theta(); }
Point2 t() const { return t_; }
Rot2 r() const { return r_; }
/** return this pose2 as a vector (x,y,r) */
Vector vector() const { return Vector_(3, t_.x(), t_.y(), r_.theta()); }
/** return DOF, dimensionality of tangent space = 3 */
size_t dim() const { return 3; }
/** exponential map */
Pose2 exmap(const Vector& v) const { return Pose2(v[0], v[1], v[2]) * (*this); }
/** log map */
Vector log(const Pose2 &pose) const { return between(*this, pose).vector(); }
/** rotate pose by theta */
// Pose2 rotate(double theta) const;
/** inverse transformation */
Pose2 inverse() const { return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); }
/** compose this transformation onto another (pre-multiply this*p1) */
Pose2 compose(const Pose2& p1) const { return Pose2(p1.r_ * r_, p1.r_ * t_ + p1.t_); }
/** same as compose (pre-multiply this*p1) */
Pose2 operator*(const Pose2& p1) const { return compose(p1); }
/** Return point coordinates in pose coordinate frame, same as transform_to */
Point2 operator*(const Point2& point) const { return r_.unrotate(point-t_); }
}; // Pose2
/** return DOF, dimensionality of tangent space = 3 */
inline size_t dim(const Pose2&) { return 3; }
/** inverse transformation */
inline Pose2 inverse(const Pose2& pose) {
return Pose2(inverse(pose.r()),
pose.r().unrotate(Point2(-pose.t().x(), -pose.t().y()))); }
/** compose this transformation onto another (pre-multiply this*p1) */
inline Pose2 compose(const Pose2& p1, const Pose2& p0) {
return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t()); }
/* exponential and log maps around identity */
// Create an incremental pose from x,y,theta
template<> inline Pose2 expmap(const Vector& v) { return Pose2(v[0], v[1], v[2]); }
// Return the x,y,theta of this pose
inline Vector logmap(const Pose2& p) { return Vector_(3, p.x(), p.y(), p.theta()); }
/** Return point coordinates in pose coordinate frame */
inline Point2 transform_to(const Pose2& pose, const Point2& point) {
return unrotate(pose.r(), point-pose.t()); }
Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
/** Return point coordinates in global frame */
inline Point2 transform_from(const Pose2& pose, const Point2& point) {
return rotate(pose.r(), point)+pose.t(); }
/** Return relative pose between p1 and p2, in p1 coordinate frame */
// todo: make sure compiler finds this version of between.
//inline Pose2 between(const Pose2& p0, const Pose2& p2) {
// return Pose2(p0.r().invcompose(p2.r()), p0.r().unrotate(p2.t()-p0.t())); }
Matrix Dbetween1(const Pose2& p0, const Pose2& p2);
Matrix Dbetween2(const Pose2& p0, const Pose2& p2);
/** same as compose (pre-multiply this*p1) */
inline Pose2 operator*(const Pose2& p1, const Pose2& p0) { return compose(p1, p0); }
/** Transform a point in this coordinate frame to global coordinates,
* same as transform_from */
inline Point2 operator*(const Pose2& pose, const Point2& point) {
return transform_from(pose, point); }
} // namespace gtsam

6
cpp/Pose2Config.cpp
View File

@ -49,14 +49,14 @@ namespace gtsam {
}
/* ************************************************************************* */
Pose2Config Pose2Config::exmap(const VectorConfig& delta) const {
Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta) {
Pose2Config newConfig;
std::string j; Pose2 pj;
FOREACH_PAIR(j, pj, values_) {
FOREACH_PAIR(j, pj, c.values_) {
if (delta.contains(j)) {
const Vector& dj = delta[j];
//check_size(j,vj,dj);
newConfig.insert(j, pj.exmap(dj));
newConfig.insert(j, expmap(pj,dj));
} else
newConfig.insert(j, pj);
}

61
cpp/Pose2Config.h
View File

@ -13,49 +13,52 @@
namespace gtsam {
class VectorConfig;
class VectorConfig;
class Pose2Config: public Testable<Pose2Config> {
class Pose2Config: public Testable<Pose2Config> {
private:
std::map<std::string, Pose2> values_;
private:
std::map<std::string, Pose2> values_;
public:
public:
typedef std::map<std::string, Pose2>::const_iterator iterator;
typedef iterator const_iterator;
typedef std::map<std::string, Pose2>::const_iterator iterator;
typedef iterator const_iterator;
Pose2Config() {}
Pose2Config() {}
Pose2Config(const Pose2Config &config) :values_(config.values_) {}
Pose2Config(const Pose2Config &config) :values_(config.values_) {}
virtual ~Pose2Config() {}
virtual ~Pose2Config() {}
const Pose2& get(std::string key) const;
const Pose2& get(std::string key) const;
void insert(const std::string& name, const Pose2& val);
void insert(const std::string& name, const Pose2& val);
inline Pose2Config& operator=(const Pose2Config& rhs) {
values_ = rhs.values_;
return (*this);
}
inline Pose2Config& operator=(const Pose2Config& rhs) {
values_ = rhs.values_;
return (*this);
}
bool equals(const Pose2Config& expected, double tol) const;
bool equals(const Pose2Config& expected, double tol) const;
void print(const std::string &s) const;
void print(const std::string &s) const;
void clear() { values_.clear(); }
void clear() { values_.clear(); }
int size() { return values_.size(); };
int size() { return values_.size(); };
/**
* Add a delta config, needed for use in NonlinearOptimizer
*/
Pose2Config exmap(const VectorConfig& delta) const;
inline const_iterator begin() const {return values_.begin();}
inline const_iterator end() const {return values_.end();}
inline iterator begin() {return values_.begin();}
inline iterator end() {return values_.end();}
friend Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta);
};
/**
* Add a delta config, needed for use in NonlinearOptimizer
*/
Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta);
inline const_iterator begin() const {return values_.begin();}
inline const_iterator end() const {return values_.end();}
inline iterator begin() {return values_.begin();}
inline iterator end() {return values_.end();}
};
} // namespace

5
cpp/Pose2Factor.h
View File

@ -49,7 +49,7 @@ public:
Vector error_vector(const Pose2Config& config) const {
//z-h
Pose2 p1 = config.get(key1_), p2 = config.get(key2_);
return -between(p1,p2).log(measured_);
return -logmap(between(p1,p2), measured_);
}
std::list<std::string> keys() const { return keys_; }
@ -58,7 +58,7 @@ public:
/** linearize */
boost::shared_ptr<GaussianFactor> linearize(const Pose2Config& config) const {
Pose2 p1 = config.get(key1_), p2 = config.get(key2_);
Vector b = -between(p1,p2).log(measured_);
Vector b = -logmap(between(p1,p2), measured_);
Matrix H1 = Dbetween1(p1,p2);
Matrix H2 = Dbetween2(p1,p2);
boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(
@ -68,4 +68,5 @@ public:
return linearized;
}
};
} /// namespace gtsam

261
cpp/Pose3.cpp
View File

@ -3,135 +3,184 @@
* @brief 3D Pose
*/
#include <iostream>
#include "Pose3.h"
using namespace std;
using namespace boost::numeric::ublas;
namespace gtsam {
/* ************************************************************************* */
void Pose3::print(const string& s) const {
R_.print(s + ".R");
t_.print(s + ".t");
}
/* ************************************************************************* */
void Pose3::print(const string& s) const {
R_.print(s + ".R");
t_.print(s + ".t");
}
/* ************************************************************************* */
bool Pose3::equals(const Pose3& pose, double tol) const
{
return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol);
}
/* ************************************************************************* */
bool Pose3::equals(const Pose3& pose, double tol) const {
return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol);
}
/* ************************************************************************* */
// Agrawal06iros, formula (6), seems to suggest this could be wrong:
Pose3 Pose3::exmap(const Vector& v) const {
return Pose3(R_.exmap(sub(v,0,3)), t_.exmap(sub(v,3,6)));
}
/** Agrawal06iros versions
Pose3 expmap(const Vector& d) {
/* ************************************************************************* */
Vector Pose3::vector() const {
Vector r = R_.vector(), t = t_.vector();
return concatVectors(2, &r, &t);
}
// From
Vector w = vector_range<const Vector>(d, range(0,3));
Vector u = vector_range<const Vector>(d, range(3,6));
double t = norm_2(w);
if (t < 1e-5)
return Pose3(expmap<Rot3> (w), expmap<Point3> (u));
else {
Matrix W = skewSymmetric(w/t);
Matrix A = eye(3, 3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
return Pose3(expmap<Rot3> (w), expmap<Point3> (A * u));
}
}
/* ************************************************************************* */
Matrix Pose3::matrix() const {
const double row4[] = { 0, 0, 0, 1 };
Matrix A34 = Matrix_(3, 4, vector()), A14 = Matrix_(1, 4, row4);
return stack(2, &A34, &A14);
}
Vector logmap(const Pose3& p) {
Vector w = logmap(p.rotation()), T = p.translation().vector();
double t = norm_2(w);
if (t < 1e-5)
return concatVectors(2, &w, &T);
else {
Matrix W = skewSymmetric(w/t);
Matrix Ainv = eye(3, 3) - 0.5*t* W + ((2*sin(t)-t*(1+cos(t)))/2*sin(t)) * (W * W);
Vector u = Ainv*T;
return concatVectors(2, &w, &u);
}
}
*/
/* ************************************************************************* */
Point3 transform_from(const Pose3& pose, const Point3& p) {
return pose.R_ * p + pose.t_;
}
/* ************************************************************************* */
Matrix Pose3::matrix() const {
const Matrix R = R_.matrix(), T = Matrix_(3,1, t_.vector());
const Matrix A34 = collect(2, &R, &T);
const Matrix A14 = Matrix_(1,4, 0.0, 0.0, 0.0, 1.0);
return stack(2, &A34, &A14);
}
/* ************************************************************************* */
Matrix Dtransform_from1(const Pose3& pose, const Point3& p) {
Matrix DR = Drotate1(pose.rotation(), p);
Matrix Dt = Dadd1(pose.translation(), p);
return collect(2,&DR,&Dt);
}
/* ************************************************************************* */
Pose3 Pose3::transform_to(const Pose3& pose) const {
Rot3 cRv = R_ * Rot3(inverse(pose.R_));
Point3 t = gtsam::transform_to(pose, t_);
return Pose3(cRv, t);
}
/* ************************************************************************* */
Matrix Dtransform_from2(const Pose3& pose) {
return Drotate2(pose.rotation());
}
/* ************************************************************************* */
Point3 transform_from(const Pose3& pose, const Point3& p) {
return pose.rotation() * p + pose.translation();
}
/* ************************************************************************* */
Point3 transform_to(const Pose3& pose, const Point3& p) {
Point3 sub = p - pose.t_;
Point3 r = unrotate(pose.R_, sub);
return r;
}
/* ************************************************************************* */
Matrix Dtransform_from1(const Pose3& pose, const Point3& p) {
#ifdef NEW_EXMAP
Point3 q = transform_from(pose,p);
Matrix DR = skewSymmetric(-q.x(), -q.y(), -q.z());
#else
Matrix DR = Drotate1(pose.rotation(), p);
#endif
Matrix Dt = eye(3);
return collect(2,&DR,&Dt);
}
/* ************************************************************************* */
Matrix Dtransform_to1(const Pose3& pose, const Point3& p) {
Point3 q = p - pose.translation();
Matrix D_r_R = Dunrotate1(pose.rotation(),q);
Matrix D_r_t = - Dunrotate2(pose.rotation()); // negative because of sub
/* ************************************************************************* */
Matrix Dtransform_from2(const Pose3& pose) {
return pose.rotation().matrix();
}
Matrix D_r_pose = collect(2,&D_r_R,&D_r_t);
return D_r_pose;
}
/* ************************************************************************* */
Point3 transform_to(const Pose3& pose, const Point3& p) {
Point3 sub = p - pose.translation();
return unrotate(pose.rotation(), sub);
}
/* ************************************************************************* */
Matrix Dtransform_to2(const Pose3& pose, const Point3& p) {
return Dunrotate2(pose.rotation());
}
/* ************************************************************************* */
Matrix Dtransform_to1(const Pose3& pose, const Point3& p) {
Point3 q = transform_to(pose,p);
Matrix Rt = pose.rotation().transpose();
Matrix DR = skewSymmetric(q.x(), q.y(), q.z()) * Rt;
Matrix DT = - Rt; // negative because of sub
return collect(2,&DR,&DT);
}
/* ************************************************************************* */
// direct measurement of the deviation of a pose from the origin
// used as soft prior
/* ************************************************************************* */
Vector hPose (const Vector& x) {
Pose3 pose(x); // transform from vector to Pose3
Vector w = pose.rotation().ypr(); // get angle differences
Vector d = pose.translation().vector(); // get translation differences
return concatVectors(2,&w,&d);
}
/* ************************************************************************* */
Matrix Dtransform_to2(const Pose3& pose, const Point3& p) {
return pose.rotation().transpose();
}
/* ************************************************************************* */
// derivative of direct measurement
// 6*6, entry i,j is how measurement error will change
/* ************************************************************************* */
Matrix DhPose(const Vector& x) {
Matrix H = eye(6,6);
return H;
}
/* ************************************************************************* */
// direct measurement of the deviation of a pose from the origin
// used as soft prior
/* ************************************************************************* */
Vector hPose (const Vector& x) {
Pose3 pose(x); // transform from vector to Pose3
Vector w = pose.rotation().ypr(); // get angle differences
Vector d = pose.translation().vector(); // get translation differences
return concatVectors(2,&w,&d);
}
/* ************************************************************************* */
Pose3 Pose3::inverse() const {
Rot3 Rt = R_.inverse();
return Pose3(Rt, -(Rt * t_));
}
/* ************************************************************************* */
// derivative of direct measurement
// 6*6, entry i,j is how measurement error will change
/* ************************************************************************* */
Matrix DhPose(const Vector& x) {
Matrix H = eye(6,6);
return H;
}
/* ************************************************************************* */
Pose3 Pose3::transform_to(const Pose3& pose) const {
Rot3 cRv = R_ * Rot3(pose.R_.inverse());
Point3 t = gtsam::transform_to(pose, t_);
return Pose3(cRv, t);
}
/* ************************************************************************* */
/*
Pose3 compose(const Pose3& p1, const Pose3& p2) {
return Pose3(compose(p1.rotation(),p2.rotation()),transform_from(p1,p2.translation());
}
*/
/* ************************************************************************* */
Pose3 between(const Pose3& p1, const Pose3& p2){
Pose3 p;
return p;
// TODO: implement
}
/* ************************************************************************* */
Matrix Dcompose1(const Pose3& p1, const Pose3& p2){
Matrix DR_R1 = eye(3);
Matrix DR_t1 = zeros(3,3);
Matrix DR = collect(2,&DR_R1,&DR_t1);
Matrix Dt = Dtransform_from1(p1,p2.translation());
return stack(2,&DR,&Dt);
}
/* ************************************************************************* */
Matrix Dbetween1(const Pose3& p1, const Pose3& p2){
Matrix m;
return m;
// TODO: implement
}
/* ************************************************************************* */
Matrix Dcompose2(const Pose3& p1, const Pose3& p2){
Matrix DR_R2 = p1.rotation().matrix();
Matrix DR_t2 = zeros(3,3);
Matrix DR = collect(2,&DR_R2,&DR_t2);
Matrix Dt_R2 = zeros(3,3);
Matrix Dt_t2 = Dtransform_from2(p1);
Matrix Dt = collect(2,&Dt_R2,&Dt_t2);
return stack(2,&DR,&Dt);
}
/* ************************************************************************* */
Matrix Dbetween2(const Pose3& p1, const Pose3& p2){
Matrix m;
return m;
// TODO: implement
}
/* ************************************************************************* */
// inverse = Pose3(inverse(p.rotation()),-unrotate(p.rotation(),p.translation()));
Matrix Dinverse(const Pose3& p){
Matrix Rt = p.rotation().transpose();
Matrix DR_R1 = -Rt;
Matrix DR_t1 = zeros(3,3);
Matrix DR = collect(2,&DR_R1,&DR_t1);
Matrix Dt_R1 = - (skewSymmetric(Rt*p.translation().vector()) * Rt);
Matrix Dt_t1 = - Rt;
Matrix Dt = collect(2,&Dt_R1,&Dt_t1);
return stack(2,&DR,&Dt);
}
/* ************************************************************************* */
/* ************************************************************************* */
// between = compose(p2,inverse(p1));
Matrix Dbetween1(const Pose3& p1, const Pose3& p2){
Pose3 invp1 = inverse(p1);
return Dinverse(p1) * Dcompose2(p2,invp1);
}
Matrix Dbetween2(const Pose3& p1, const Pose3& p2){
Pose3 invp1 = inverse(p1);
return Dcompose1(p2,invp1);
}
/* ************************************************************************* */
} // namespace gtsam

222
cpp/Pose3.h
View File

@ -7,149 +7,149 @@
#pragma once
#include <boost/numeric/ublas/vector_proxy.hpp>
#include "Point3.h"
#include "Rot3.h"
#include "Testable.h"
#include "Lie.h"
namespace gtsam {
/** A 3D pose (R,t) : (Rot3,Point3) */
class Pose3 : Testable<Pose3> {
private:
Rot3 R_;
Point3 t_;
/** A 3D pose (R,t) : (Rot3,Point3) */
class Pose3 : Testable<Pose3> {
private:
Rot3 R_;
Point3 t_;
public:
public:
/**
* Default constructor is origin
*/
Pose3() {}
/** Default constructor is origin */
Pose3() {}
/**
* Copy constructor
*/
Pose3(const Pose3& pose) :
R_(pose.R_), t_(pose.t_) {
}
/** Copy constructor */
Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {}
/**
* Construct from R,t
*/
Pose3(const Rot3& R, const Point3& t) :
R_(R), t_(t) {
}
/** Construct from R,t */
Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {}
/** Constructor from 4*4 matrix */
Pose3(const Matrix &T) :
R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0),
T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
}
/** Constructor from 4*4 matrix */
Pose3(const Matrix &T) :
R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0),
T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {}
/** Constructor from 12D vector */
Pose3(const Vector &V) :
R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)),
t_(V(9), V(10),V(11)) {
}
/** Constructor from 12D vector */
Pose3(const Vector &V) :
R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)),
t_(V(9), V(10),V(11)) {}
/** print with optional string */
void print(const std::string& s = "") const;
const Rot3& rotation() const { return R_; }
/** assert equality up to a tolerance */
bool equals(const Pose3& pose, double tol = 1e-9) const;
const Point3& translation() const { return t_; }
const Rot3& rotation() const {
return R_;
}
/** convert to 4*4 matrix */
Matrix matrix() const;
const Point3& translation() const {
return t_;
}
/** print with optional string */
void print(const std::string& s = "") const;
/** return DOF, dimensionality of tangent space = 6 */
size_t dim() const {
return 6;
}
/** assert equality up to a tolerance */
bool equals(const Pose3& pose, double tol = 1e-9) const;
/** Given 6-dim tangent vector, create new pose */
Pose3 exmap(const Vector& d) const;
Pose3 transform_to(const Pose3& pose) const;
/** inverse transformation */
Pose3 inverse() const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_);
}
}; // Pose3 class
// operators
Pose3 operator+(const Pose3& p3) const{
// TODO implement the operators "+"
Pose3 p;
return p;
};
// Dimensionality of the tangent space
inline size_t dim(const Pose3&) { return 6; }
Pose3 operator-(const Pose3& p3) const{
// TODO implement the operators "-"
Pose3 p;
return p;
};
// Compose two poses
inline Pose3 compose(const Pose3& p0, const Pose3& p1) {
return Pose3(p0.rotation()*p1.rotation(),
p0.translation() + p0.rotation()*p1.translation());
}
/** composition */
inline Pose3 operator*(const Pose3& B) const {
return Pose3(R_*B.R_, t_ + R_*B.t_);
}
// Find the inverse pose s.t. inverse(p)*p = Pose3()
inline Pose3 inverse(const Pose3& p) {
Rot3 Rt = inverse(p.rotation());
return Pose3(Rt, Rt*(-p.translation()));
}
/** return 12D vectorized form (column-wise) */
Vector vector() const;
// Exponential map at identity - create a pose with a translation and
// rotation (in canonical coordinates)
template<> inline Pose3 expmap(const Vector& d) {
Vector w = sub(d, 0,3);
Vector u = sub(d, 3,6);
return Pose3(expmap<Rot3> (w), expmap<Point3> (u));
}
/** convert to 4*4 matrix */
Matrix matrix() const;
// Log map at identity - return the translation and canonical rotation
// coordinates of a pose.
inline Vector logmap(const Pose3& p) {
const Vector w = logmap(p.rotation()), u = logmap(p.translation());
return concatVectors(2, &w, &u);
}
/** transforms */
Pose3 transform_to(const Pose3& transform) const;
// todo: these are the "old-style" expmap and logmap about the specified
// pose.
// Increments the offset and rotation independently given a translation and
// canonical rotation coordinates
template<> inline Pose3 expmap<Pose3>(const Pose3& p0, const Vector& d) {
return Pose3(expmap(p0.rotation(), sub(d, 0, 3)),
expmap(p0.translation(), sub(d, 3, 6)));
}
/** friends */
friend Point3 transform_from(const Pose3& pose, const Point3& p);
friend Point3 transform_to(const Pose3& pose, const Point3& p);
friend Pose3 compose(const Pose3& current, const Pose3& target);
// Independently computes the logmap of the translation and rotation.
template<> inline Vector logmap<Pose3>(const Pose3& p0, const Pose3& pp) {
const Vector r(logmap(p0.rotation(), pp.rotation())),
t(logmap(p0.translation(), pp.translation()));
return concatVectors(2, &r, &t);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_);
}
}; // Pose3 class
/**
* Finds a transform from the current frame dTx to the target frame sTx
*/
inline Pose3 compose(const Pose3& dTx, const Pose3& sTx) {
return dTx * sTx.inverse();
}
/** receives the point in Pose coordinates and transforms it to world coordinates */
Point3 transform_from(const Pose3& pose, const Point3& p);
inline Point3 operator*(const Pose3& pose, const Point3& p) { return transform_from(pose, p); }
Matrix Dtransform_from1(const Pose3& pose, const Point3& p);
Matrix Dtransform_from2(const Pose3& pose); // does not depend on p !
/** receives the point in Pose coordinates and transforms it to world coordinates */
Point3 transform_from(const Pose3& pose, const Point3& p);
Matrix Dtransform_from1(const Pose3& pose, const Point3& p);
Matrix Dtransform_from2(const Pose3& pose); // does not depend on p !
/** receives the point in world coordinates and transforms it to Pose coordinates */
Point3 transform_to(const Pose3& pose, const Point3& p);
Matrix Dtransform_to1(const Pose3& pose, const Point3& p);
Matrix Dtransform_to2(const Pose3& pose, const Point3& p);
/** receives the point in world coordinates and transforms it to Pose coordinates */
Point3 transform_to(const Pose3& pose, const Point3& p);
Matrix Dtransform_to1(const Pose3& pose, const Point3& p);
Matrix Dtransform_to2(const Pose3& pose, const Point3& p);
/**
* Derivatives of compose
*/
Matrix Dcompose1(const Pose3& p1, const Pose3& p2);
Matrix Dcompose2(const Pose3& p1, const Pose3& p2);
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose3 between(const Pose3& p1, const Pose3& p2);
Matrix Dbetween1(const Pose3& p1, const Pose3& p2);
Matrix Dbetween2(const Pose3& p1, const Pose3& p2);
/**
* Derivative of inverse
*/
Matrix Dinverse(const Pose3& p);
/** direct measurement of a pose */
Vector hPose(const Vector& x);
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Matrix Dbetween1(const Pose3& p1, const Pose3& p2);
Matrix Dbetween2(const Pose3& p1, const Pose3& p2);
/**
* derivative of direct measurement
* 12*6, entry i,j is how measurement error will change
*/
Matrix DhPose(const Vector& x);
/** direct measurement of a pose */
Vector hPose(const Vector& x);
/**
* derivative of direct measurement
* 12*6, entry i,j is how measurement error will change
*/
Matrix DhPose(const Vector& x);
} // namespace gtsam

12
cpp/Pose3Factor.h
View File

@ -52,7 +52,11 @@ namespace gtsam {
Vector error_vector(const Config& config) const {
//z-h
Pose3 p1 = config.get(key1_), p2 = config.get(key2_);
return (measured_ - between(p1, p2)).vector();
// todo: removed vector() from Pose3, so emulating it here! This is incorrect!
Pose3 betw = between(p1,p2);
Vector r_diff = logmap(measured_.rotation()) - logmap(betw.rotation());
Vector t_diff = logmap(measured_.translation()) - logmap(betw.translation());
return concatVectors(2, &r_diff, &t_diff);
}
std::list<std::string> keys() const {
@ -66,7 +70,11 @@ namespace gtsam {
/** linearize */
boost::shared_ptr<GaussianFactor> linearize(const Config& config) const {
Pose3 p1 = config.get(key1_), p2 = config.get(key2_);
Vector b = (measured_ - between(p1, p2)).vector();
Pose3 betw = between(p1,p2);
// todo: removed vector() from Pose3, so emulating it here! This is incorrect!
Vector r_diff = logmap(measured_.rotation()) - logmap(betw.rotation());
Vector t_diff = logmap(measured_.translation()) - logmap(betw.translation());
Vector b = concatVectors(2, &r_diff, &t_diff);
Matrix H1 = Dbetween1(p1, p2);
Matrix H2 = Dbetween2(p1, p2);
boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(key1_,

41
cpp/Rot2.cpp
View File

@ -21,12 +21,6 @@ namespace gtsam {
return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol;
}
/* ************************************************************************* */
Rot2 Rot2::exmap(const Vector& v) const {
if (zero(v)) return (*this);
return Rot2(v(0)) * (*this); // exponential map then compose
}
/* ************************************************************************* */
Matrix Rot2::matrix() const {
return Matrix_(2, 2, c_, -s_, s_, c_);
@ -42,32 +36,6 @@ namespace gtsam {
return Matrix_(2, 2, -c_, -s_, s_, -c_);
}
/* ************************************************************************* */
Rot2 Rot2::inverse() const { return Rot2(c_, -s_);}
/* ************************************************************************* */
Rot2 Rot2::invcompose(const Rot2& R) const {
return Rot2(
c_ * R.c_ + s_ * R.s_,
-s_ * R.c_ + c_ * R.s_);
}
/* ************************************************************************* */
Rot2 Rot2::operator*(const Rot2& R) const {
return Rot2(
c_ * R.c_ - s_ * R.s_,
s_ * R.c_ + c_ * R.s_
);
}
/* ************************************************************************* */
Point2 Rot2::operator*(const Point2& p) const {
return Point2(
c_ * p.x() - s_ * p.y(),
s_ * p.x() + c_ * p.y()
);
}
/* ************************************************************************* */
Point2 Rot2::unrotate(const Point2& p) const {
return Point2(
@ -76,14 +44,11 @@ namespace gtsam {
);
}
/* ************************************************************************* */
Rot2 exmap(const Rot2& R, const Vector& v) {
return R.exmap(v);
}
/* ************************************************************************* */
Point2 rotate(const Rot2& R, const Point2& p) {
return R * p;
return Point2(
R.c() * p.x() - R.s() * p.y(),
R.s() * p.x() + R.c() * p.y());
}
/* ************************************************************************* */

80
cpp/Rot2.h
View File

@ -11,6 +11,7 @@
#include "Testable.h"
#include "Point2.h"
#include "Matrix.h"
#include "Lie.h"
namespace gtsam {
@ -20,13 +21,13 @@ namespace gtsam {
/** we store cos(theta) and sin(theta) */
double c_, s_;
/** private constructor from cos/sin */
public:
/** constructor from cos/sin */
Rot2(double c, double s) :
c_(c), s_(s) {
}
public:
/** default constructor, zero rotation */
Rot2() : c_(1.0), s_(0.0) {}
@ -48,18 +49,6 @@ namespace gtsam {
/** equals with an tolerance */
bool equals(const Rot2& R, double tol = 1e-9) const;
/** return DOF, dimensionality of tangent space */
inline size_t dim() const { return 1;}
/** Given 1-dim tangent vector, create new rotation */
Rot2 exmap(const Vector& d) const;
/** Return the 1-dim tangent vector of R about this rotation */
Vector log(const Rot2& R) const { return Vector_(1, R.theta() - theta()); }
/** return vectorized form (column-wise)*/
inline Vector vector() const { return Vector_(2,c_,s_);}
/** return 2*2 rotation matrix */
Matrix matrix() const;
@ -69,18 +58,6 @@ namespace gtsam {
/** return 2*2 negative transpose */
Matrix negtranspose() const;
/** inverse transformation */
Rot2 inverse() const;
/** compose with the inverse of this rotation */
Rot2 invcompose(const Rot2& R) const;
/** composition via sum and difference formulas */
Rot2 operator*(const Rot2& R) const;
/** rotate from rotated to world, syntactic sugar = R*p */
Point2 operator*(const Point2& p) const;
/** rotate from world to rotated = R'*p */
Point2 unrotate(const Point2& p) const;
@ -97,19 +74,54 @@ namespace gtsam {
}
};
/**
* Update Rotation with incremental rotation
* @param v a vector of incremental angle
* @param R a 2D rotation
* @return incremental rotation matrix
*/
Rot2 exmap(const Rot2& R, const Vector& v);
// Lie group functions
// Dimensionality of the tangent space
inline size_t dim(const Rot2&) { return 1; }
// Expmap around identity - create a rotation from an angle
template<> inline Rot2 expmap(const Vector& v) {
if (zero(v)) return (Rot2());
else return Rot2(v(0));
}
// Logmap around identity - return the angle of the rotation
inline Vector logmap(const Rot2& r) {
return Vector_(1, r.theta());
}
// Compose - make a new rotation by adding angles
inline Rot2 compose(const Rot2& r0, const Rot2& r1) {
return Rot2(
r0.c() * r1.c() - r0.s() * r1.s(),
r0.s() * r1.c() + r0.c() * r1.s());
}
// Syntactic sugar R1*R2 = compose(R1,R2)
inline Rot2 operator*(const Rot2& r0, const Rot2& r1) {
return compose(r0, r1);
}
// The inverse rotation - negative angle
inline Rot2 inverse(const Rot2& r) { return Rot2(r.c(), -r.s());}
// Shortcut to compose the inverse: invcompose(R0,R1) = inverse(R0)*R1
inline Rot2 invcompose(const Rot2& r0, const Rot2& r1) {
return Rot2(
r0.c() * r1.c() + r0.s() * r1.s(),
-r0.s() * r1.c() + r0.c() * r1.s());
}
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
Point2 rotate(const Rot2& R, const Point2& p);
inline Point2 operator*(const Rot2& R, const Point2& p) {
return rotate(R,p);
}
Matrix Drotate1(const Rot2& R, const Point2& p);
Matrix Drotate2(const Rot2& R); // does not depend on p !

284
cpp/Rot3.cpp
View File

@ -13,202 +13,178 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
}
bool Rot3::equals(const Rot3 & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
}
/* ************************************************************************* */
Rot3 Rot3::exmap(const Vector& v) const {
if (zero(v)) return (*this);
return rodriguez(v) * (*this);
}
/* ************************************************************************* */
Vector Rot3::log() const {
double tr = r1_.x()+r2_.y()+r3_.z();
if (tr==3.0) return ones(3);
if (tr==-1.0) throw domain_error("Rot3::log: trace == -1 not yet handled :-(");;
double theta = acos((tr-1.0)/2.0);
return (theta/2.0/sin(theta))*Vector_(3,
r2_.z()-r3_.y(),
r3_.x()-r1_.z(),
r1_.y()-r2_.x());
}
/* ************************************************************************* */
Vector Rot3::vector() const {
double r[] = { r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z() };
Vector v(9);
copy(r,r+9,v.begin());
return v;
}
// Vector Rot3::vector() const {
// double r[] = { r1_.x(), r1_.y(), r1_.z(),
// r2_.x(), r2_.y(), r2_.z(),
// r3_.x(), r3_.y(), r3_.z() };
// Vector v(9);
// copy(r,r+9,v.begin());
// return v;
// }
/* ************************************************************************* */
Matrix Rot3::matrix() const {
double r[] = { r1_.x(), r2_.x(), r3_.x(),
r1_.y(), r2_.y(), r3_.y(),
r1_.z(), r2_.z(), r3_.z() };
r1_.y(), r2_.y(), r3_.y(),
r1_.z(), r2_.z(), r3_.z() };
return Matrix_(3,3, r);
}
/* ************************************************************************* */
Matrix Rot3::transpose() const {
double r[] = { r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z()};
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z()};
return Matrix_(3,3, r);
}
/* ************************************************************************* */
Point3 Rot3::column(int index) const{
if(index == 3)
return r3_;
else if (index == 2)
return r2_;
else
return r1_; // default returns r1
if(index == 3)
return r3_;
else if (index == 2)
return r2_;
else
return r1_; // default returns r1
}
/* ************************************************************************* */
Rot3 Rot3::inverse() const {
return Rot3(
r1_.x(), r1_.y(), r1_.z(),
r2_.x(), r2_.y(), r2_.z(),
r3_.x(), r3_.y(), r3_.z());
}
/* ************************************************************************* */
Point3 Rot3::unrotate(const Point3& p) const {
return Point3(
r1_.x() * p.x() + r1_.y() * p.y() + r1_.z() * p.z(),
r2_.x() * p.x() + r2_.y() * p.y() + r2_.z() * p.z(),
r3_.x() * p.x() + r3_.y() * p.y() + r3_.z() * p.z()
);
}
/* ************************************************************************* */
Vector Rot3::ypr() const {
Matrix I;Vector q;
boost::tie(I,q)=RQ(matrix());
return q;
Matrix I;Vector q;
boost::tie(I,q)=RQ(matrix());
return q;
}
/* ************************************************************************* */
Rot3 rodriguez(const Vector& n, double t) {
double n0 = n(0), n1=n(1), n2=n(2);
double n00 = n0*n0, n11 = n1*n1, n22 = n2*n2;
/* ************************************************************************* */
Rot3 rodriguez(const Vector& n, double t) {
double n0 = n(0), n1=n(1), n2=n(2);
double n00 = n0*n0, n11 = n1*n1, n22 = n2*n2;
#ifndef NDEBUG
double l_n = n00+n11+n22;
if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
double l_n = n00+n11+n22;
if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
#endif
double ct = cos(t), st = sin(t), ct_1 = 1 - ct;
double ct = cos(t), st = sin(t), ct_1 = 1 - ct;
double s0 = n0 * st, s1 = n1 * st, s2 = n2 * st;
double C01 = ct_1*n0*n1, C02 = ct_1*n0*n2, C12 = ct_1*n1*n2;
double C00 = ct_1*n00, C11 = ct_1*n11, C22 = ct_1*n22;
double s0 = n0 * st, s1 = n1 * st, s2 = n2 * st;
double C01 = ct_1*n0*n1, C02 = ct_1*n0*n2, C12 = ct_1*n1*n2;
double C00 = ct_1*n00, C11 = ct_1*n11, C22 = ct_1*n22;
Point3 r1 = Point3( ct + C00, s2 + C01, -s1 + C02);
Point3 r2 = Point3(-s2 + C01, ct + C11, s0 + C12);
Point3 r3 = Point3( s1 + C02, -s0 + C12, ct + C22);
Point3 r1 = Point3( ct + C00, s2 + C01, -s1 + C02);
Point3 r2 = Point3(-s2 + C01, ct + C11, s0 + C12);
Point3 r3 = Point3( s1 + C02, -s0 + C12, ct + C22);
return Rot3(r1, r2, r3);
}
/* ************************************************************************* */
Rot3 rodriguez(const Vector& w) {
double t = norm_2(w);
if (t < 1e-5) return Rot3();
return rodriguez(w/t, t);
}
/* ************************************************************************* */
Rot3 exmap(const Rot3& R, const Vector& v) {
return R.exmap(v);
}
/* ************************************************************************* */
Vector log(const Rot3& R, const Rot3& S) {
return between(R,S).log();
return Rot3(r1, r2, r3);
}
/* ************************************************************************* */
Point3 rotate(const Rot3& R, const Point3& p) {
return R * p;
}
/* ************************************************************************* */
Matrix Drotate1(const Rot3& R, const Point3& p) {
Point3 q = R * p;
return skewSymmetric(-q.x(), -q.y(), -q.z());
}
/* ************************************************************************* */
Rot3 rodriguez(const Vector& w) {
double t = norm_2(w);
if (t < 1e-5) return Rot3();
return rodriguez(w/t, t);
}
/* ************************************************************************* */
Matrix Drotate2(const Rot3& R) {
return R.matrix();
}
/* ************************************************************************* */
Point3 rotate(const Rot3& R, const Point3& p) {
return R.r1() * p.x() + R.r2() * p.y() + R.r3() * p.z();
}
/* ************************************************************************* */
Point3 unrotate(const Rot3& R, const Point3& p) {
return R.unrotate(p);
}
/* ************************************************************************* */
Matrix Drotate1(const Rot3& R, const Point3& p) {
Point3 q = R * p;
return skewSymmetric(-q.x(), -q.y(), -q.z());
}
/* ************************************************************************* */
/** see libraries/caml/geometry/math.lyx, derivative of unrotate */
/* ************************************************************************* */
Matrix Dunrotate1(const Rot3 & R, const Point3 & p) {
Point3 q = R.unrotate(p);
return skewSymmetric(q.x(), q.y(), q.z()) * R.transpose();
}
/* ************************************************************************* */
Matrix Drotate2(const Rot3& R) {
return R.matrix();
}
/* ************************************************************************* */
Matrix Dunrotate2(const Rot3 & R) {
return R.transpose();
}
/* ************************************************************************* */
Point3 unrotate(const Rot3& R, const Point3& p) {
return Point3(
R.r1().x() * p.x() + R.r1().y() * p.y() + R.r1().z() * p.z(),
R.r2().x() * p.x() + R.r2().y() * p.y() + R.r2().z() * p.z(),
R.r3().x() * p.x() + R.r3().y() * p.y() + R.r3().z() * p.z()
);
}
/* ************************************************************************* */
Rot3 between(const Rot3& R1, const Rot3& R2) {
return R2 * R1.inverse();
}
/* ************************************************************************* */
/** see libraries/caml/geometry/math.lyx, derivative of unrotate */
/* ************************************************************************* */
Matrix Dunrotate1(const Rot3 & R, const Point3 & p) {
Point3 q = unrotate(R,p);
return skewSymmetric(q.x(), q.y(), q.z()) * R.transpose();
}
/* ************************************************************************* */
pair<Matrix,Vector> RQ(const Matrix& A) {
double A21 = A(2, 1), A22 = A(2, 2), a = sqrt(A21 * A21 + A22 * A22);
double Cx = A22 / a; //cosX
double Sx = -A21 / a; //sinX
Matrix Qx = Matrix_(3, 3,
1.0, 0.0, 0.0,
0.0, Cx, -Sx,
0.0, Sx, Cx);
Matrix B = A * Qx;
/* ************************************************************************* */
Matrix Dunrotate2(const Rot3 & R) {
return R.transpose();
}
double B20 = B(2, 0), B22 = B(2, 2), b = sqrt(B20 * B20 + B22 * B22);
double Cy = B22 / b; //cosY
double Sy = B20 / b; //sinY
Matrix Qy = Matrix_(3,3,
Cy, 0.0, Sy,
0.0, 1.0, 0.0,
-Sy, 0.0, Cy);
Matrix C = B * Qy;
/* ************************************************************************* */
Matrix Dcompose1(const Rot3& R1, const Rot3& R2){
return eye(3);
}
double C10 = C(1, 0), C11 = C(1, 1), c = sqrt(C10 * C10 + C11 * C11);
double Cz = C11 / c; //cosZ
double Sz = -C10 / c; //sinZ
Matrix Qz = Matrix_(3, 3,
Cz, -Sz, 0.0,
Sz, Cz, 0.0,
0.0, 0.0, 1.0);
Matrix R = C * Qz;
/* ************************************************************************* */
Matrix Dcompose2(const Rot3& R1, const Rot3& R2){
return R1.matrix();
}
Vector angles(3);
angles(0) = -atan2(Sx, Cx);
angles(1) = -atan2(Sy, Cy);
angles(2) = -atan2(Sz, Cz);
/* ************************************************************************* */
Matrix Dbetween1(const Rot3& R1, const Rot3& R2){
return -between(R1,R2).matrix();
}
return make_pair(R,angles);
}
/* ************************************************************************* */
Matrix Dbetween2(const Rot3& R1, const Rot3& R2){
return eye(3);
}
/* ************************************************************************* */
/* ************************************************************************* */
pair<Matrix,Vector> RQ(const Matrix& A) {
double A21 = A(2, 1), A22 = A(2, 2), a = sqrt(A21 * A21 + A22 * A22);
double Cx = A22 / a; //cosX
double Sx = -A21 / a; //sinX
Matrix Qx = Matrix_(3, 3,
1.0, 0.0, 0.0,
0.0, Cx, -Sx,
0.0, Sx, Cx);
Matrix B = A * Qx;
double B20 = B(2, 0), B22 = B(2, 2), b = sqrt(B20 * B20 + B22 * B22);
double Cy = B22 / b; //cosY
double Sy = B20 / b; //sinY
Matrix Qy = Matrix_(3,3,
Cy, 0.0, Sy,
0.0, 1.0, 0.0,
-Sy, 0.0, Cy);
Matrix C = B * Qy;
double C10 = C(1, 0), C11 = C(1, 1), c = sqrt(C10 * C10 + C11 * C11);
double Cz = C11 / c; //cosZ
double Sz = -C10 / c; //sinZ
Matrix Qz = Matrix_(3, 3,
Cz, -Sz, 0.0,
Sz, Cz, 0.0,
0.0, 0.0, 1.0);
Matrix R = C * Qz;
Vector angles(3);
angles(0) = -atan2(Sx, Cx);
angles(1) = -atan2(Sy, Cy);
angles(2) = -atan2(Sz, Cz);
return make_pair(R,angles);
}
/* ************************************************************************* */
} // namespace gtsam

202
cpp/Rot3.h
View File

@ -12,71 +12,53 @@
#include "Point3.h"
#include "Testable.h"
#include "Lie.h"
namespace gtsam {
/* Rotation matrix */
/* 3D Rotation */
class Rot3: Testable<Rot3> {
private:
/** we store columns ! */
Point3 r1_, r2_, r3_;
public:
/** default constructor, unit rotation */
Rot3() : r1_(Point3(1.0,0.0,0.0)),
r2_(Point3(0.0,1.0,0.0)),
r3_(Point3(0.0,0.0,1.0)) {
}
/** constructor from columns */
Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
r1_(r1), r2_(r2), r3_(r3) {
}
/** constructor from vector */
Rot3(const Vector &v) :
r1_(Point3(v(0),v(1),v(2))),
r2_(Point3(v(3),v(4),v(5))),
r3_(Point3(v(6),v(7),v(8)))
{ }
/** default constructor, unit rotation */
Rot3() :
r1_(Point3(1.0,0.0,0.0)),
r2_(Point3(0.0,1.0,0.0)),
r3_(Point3(0.0,0.0,1.0)) {}
/** constructor from doubles in *row* order !!! */
Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
r1_(Point3(R11, R21, R31)),
r2_(Point3(R12, R22, R32)),
r3_(Point3(R13, R23, R33)) {}
/** constructor from columns */
Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
r1_(r1), r2_(r2), r3_(r3) {}
/** constructor from matrix */
Rot3(const Matrix& R):
r1_(Point3(R(0,0), R(1,0), R(2,0))),
r2_(Point3(R(0,1), R(1,1), R(2,1))),
r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
/** constructor from vector */
Rot3(const Vector &v) :
r1_(Point3(v(0),v(1),v(2))),
r2_(Point3(v(3),v(4),v(5))),
r3_(Point3(v(6),v(7),v(8))) {}
/** print */
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
/** constructor from doubles in *row* order !!! */
Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
r1_(Point3(R11, R21, R31)),
r2_(Point3(R12, R22, R32)),
r3_(Point3(R13, R23, R33)) {}
/** equals with an tolerance */
bool equals(const Rot3& p, double tol = 1e-9) const;
/** constructor from matrix */
Rot3(const Matrix& R):
r1_(Point3(R(0,0), R(1,0), R(2,0))),
r2_(Point3(R(0,1), R(1,1), R(2,1))),
r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
/** return DOF, dimensionality of tangent space */
size_t dim() const { return 3;}
/**
* @param a 3-dim tangent vector d (canonical coordinates of between(R,S))
* @return new rotation S=exp(d)*R
*/
Rot3 exmap(const Vector& d) const;
/**
* @return log(R), i.e. canonical coordinates of R
*/
Vector log() const;
/** print */
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
/** return vectorized form (column-wise)*/
Vector vector() const;
/** equals with an tolerance */
bool equals(const Rot3& p, double tol = 1e-9) const;
/** return 3*3 rotation matrix */
Matrix matrix() const;
@ -86,39 +68,26 @@ namespace gtsam {
/** returns column vector specified by index */
Point3 column(int index) const;
/** inverse transformation */
Rot3 inverse() const;
/** composition */
inline Rot3 operator*(const Rot3& B) const { return Rot3(matrix()*B.matrix());}
/** rotate from rotated to world, syntactic sugar = R*p */
inline Point3 operator*(const Point3& p) const {
return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();
}
/** rotate from world to rotated = R'*p */
Point3 unrotate(const Point3& p) const;
Point3 r1() const { return r1_; }
Point3 r2() const { return r2_; }
Point3 r3() const { return r3_; }
/** use RQ to calculate yaw-pitch-roll angle representation */
Vector ypr() const;
/** friends */
friend Matrix Dunrotate1(const Rot3& R, const Point3& p);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(r1_);
ar & BOOST_SERIALIZATION_NVP(r2_);
ar & BOOST_SERIALIZATION_NVP(r3_);
}
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(r1_);
ar & BOOST_SERIALIZATION_NVP(r2_);
ar & BOOST_SERIALIZATION_NVP(r3_);
}
};
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param w is the rotation axis, unit length
@ -143,26 +112,62 @@ namespace gtsam {
*/
inline Rot3 rodriguez(double wx, double wy, double wz) { return rodriguez(Vector_(3,wx,wy,wz));}
/** return DOF, dimensionality of tangent space */
inline size_t dim(const Rot3&) { return 3; }
// Exponential map at identity - create a rotation from canonical coordinates
// using Rodriguez' formula
template<> inline Rot3 expmap(const Vector& v) {
if(zero(v)) return Rot3();
else return rodriguez(v);
}
// Log map at identity - return the canonical coordinates of this rotation
inline Vector logmap(const Rot3& R) {
double tr = R.r1().x()+R.r2().y()+R.r3().z();
if (tr==3.0) return ones(3); // todo: identity?
if (tr==-1.0) throw std::domain_error("Rot3::log: trace == -1 not yet handled :-(");;
double theta = acos((tr-1.0)/2.0);
return (theta/2.0/sin(theta))*Vector_(3,
R.r2().z()-R.r3().y(),
R.r3().x()-R.r1().z(),
R.r1().y()-R.r2().x());
}
// Compose two rotations
inline Rot3 compose(const Rot3& R1, const Rot3& R2) {
return Rot3(R1.matrix() * R2.matrix()); }
// Find the inverse rotation R^T s.t. inverse(R)*R = Rot3()
inline Rot3 inverse(const Rot3& R) {
return Rot3(
R.r1().x(), R.r1().y(), R.r1().z(),
R.r2().x(), R.r2().y(), R.r2().z(),
R.r3().x(), R.r3().y(), R.r3().z());
}
/**
* Update Rotation with incremental rotation
* @param v a vector of incremental roll,pitch,yaw
* @param R a rotated frame
* @return incremental rotation matrix
*/
Rot3 exmap(const Rot3& R, const Vector& v);
//Rot3 exp(const Rot3& R, const Vector& v);
/**
* @param a rotation R
* @param a rotation S
* @return log(S*R'), i.e. canonical coordinates of between(R,S)
*/
Vector log(const Rot3& R, const Rot3& S);
//Vector log(const Rot3& R, const Rot3& S);
/**
* rotate point from rotated coordinate frame to
* world = R*p
*/
Point3 rotate (const Rot3& R, const Point3& p);
Point3 rotate(const Rot3& R, const Point3& p);
inline Point3 operator*(const Rot3& R, const Point3& p) { return rotate(R,p); }
Matrix Drotate1(const Rot3& R, const Point3& p);
Matrix Drotate2(const Rot3& R); // does not depend on p !
@ -170,25 +175,34 @@ namespace gtsam {
* rotate point from world to rotated
* frame = R'*p
*/
Point3 unrotate (const Rot3& R, const Point3& p);
Point3 unrotate(const Rot3& R, const Point3& p);
Matrix Dunrotate1(const Rot3& R, const Point3& p);
Matrix Dunrotate2(const Rot3& R); // does not depend on p !
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/
Rot3 between(const Rot3& R1, const Rot3& R2);
/**
* compose two rotations i.e., R=R1*R2
*/
//Rot3 compose (const Rot3& R1, const Rot3& R2);
Matrix Dcompose1(const Rot3& R1, const Rot3& R2);
Matrix Dcompose2(const Rot3& R1, const Rot3& R2);
/**
* [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
* and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'
* such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will
* be the identity and Q is a yaw-pitch-roll decomposition of A.
* The implementation uses Givens rotations and is based on Hartley-Zisserman.
* @param a 3 by 3 matrix A=RQ
* @return an upper triangular matrix R
* @return a vector [thetax, thetay, thetaz] in radians.
*/
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/
//Rot3 between (const Rot3& R1, const Rot3& R2);
Matrix Dbetween1(const Rot3& R1, const Rot3& R2);
Matrix Dbetween2(const Rot3& R1, const Rot3& R2);
/**
* [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
* and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'
* such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will
* be the identity and Q is a yaw-pitch-roll decomposition of A.
* The implementation uses Givens rotations and is based on Hartley-Zisserman.
* @param a 3 by 3 matrix A=RQ
* @return an upper triangular matrix R
* @return a vector [thetax, thetay, thetaz] in radians.
*/
std::pair<Matrix,Vector> RQ(const Matrix& A);
}

4
cpp/SQPOptimizer-inl.h
View File

@ -126,8 +126,8 @@ SQPOptimizer<G, C> SQPOptimizer<G, C>::iterate(Verbosity v) const {
if (verbose) delta.print("Delta Config");
// update both state variables
shared_config newConfig(new C(config_->exmap(delta)));
shared_vconfig newLambdas(new VectorConfig(lagrange_config_->exmap(delta)));
shared_config newConfig(new C(expmap(*config_, delta)));
shared_vconfig newLambdas(new VectorConfig(expmap(*lagrange_config_, delta)));
// construct a new optimizer
return SQPOptimizer<G, C>(*graph_, full_ordering_, newConfig, newLambdas);

14
cpp/VSLAMConfig.cpp
View File

@ -23,7 +23,7 @@ namespace gtsam {
/* ************************************************************************* */
// Exponential map
VSLAMConfig VSLAMConfig::exmap(const VectorConfig & delta) const {
VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta) {
VSLAMConfig newConfig;
@ -32,16 +32,16 @@ VSLAMConfig VSLAMConfig::exmap(const VectorConfig & delta) const {
string key = it->first;
if (key[0] == 'x') {
int cameraNumber = atoi(key.substr(1, key.size() - 1).c_str());
if (cameraPoseExists(cameraNumber)) {
Pose3 basePose = cameraPose(cameraNumber);
newConfig.addCameraPose(cameraNumber, basePose.exmap(it->second));
if (c.cameraPoseExists(cameraNumber)) {
Pose3 basePose = c.cameraPose(cameraNumber);
newConfig.addCameraPose(cameraNumber, expmap(basePose, it->second));
}
}
if (key[0] == 'l') {
int landmarkNumber = atoi(key.substr(1, key.size() - 1).c_str());
if (landmarkPointExists(landmarkNumber)) {
Point3 basePoint = landmarkPoint(landmarkNumber);
newConfig.addLandmarkPoint(landmarkNumber, basePoint.exmap(it->second));
if (c.landmarkPointExists(landmarkNumber)) {
Point3 basePoint = c.landmarkPoint(landmarkNumber);
newConfig.addLandmarkPoint(landmarkNumber, expmap(basePoint, it->second));
}
}
}

18
cpp/VSLAMConfig.h
View File

@ -44,13 +44,6 @@ class VSLAMConfig : Testable<VSLAMConfig> {
VSLAMConfig(const VSLAMConfig& original):
cameraPoses_(original.cameraPoses_), landmarkPoints_(original.landmarkPoints_){}
/**
* Exponential map: takes 6D vectors in VectorConfig
* and applies them to the poses in the VSLAMConfig.
* Needed for use in nonlinear optimization
*/
VSLAMConfig exmap(const VectorConfig & delta) const;
PoseMap::const_iterator cameraIteratorBegin() const { return cameraPoses_.begin();}
PoseMap::const_iterator cameraIteratorEnd() const { return cameraPoses_.end();}
PointMap::const_iterator landmarkIteratorBegin() const { return landmarkPoints_.begin();}
@ -115,7 +108,18 @@ class VSLAMConfig : Testable<VSLAMConfig> {
inline size_t size(){
return landmarkPoints_.size() + cameraPoses_.size();
}
friend VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta);
};
/**
* Exponential map: takes 6D vectors in VectorConfig
* and applies them to the poses in the VSLAMConfig.
* Needed for use in nonlinear optimization
*/
VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta);
} // namespace gtsam

8
cpp/VectorConfig.cpp
View File

@ -107,11 +107,11 @@ VectorConfig VectorConfig::operator-(const VectorConfig& b) const {
}
/* ************************************************************************* */
VectorConfig VectorConfig::exmap(const VectorConfig& delta) const
VectorConfig expmap(const VectorConfig& original, const VectorConfig& delta)
{
VectorConfig newConfig;
string j; Vector vj;
FOREACH_PAIR(j, vj, values) {
FOREACH_PAIR(j, vj, original.values) {
if (delta.contains(j)) {
const Vector& dj = delta[j];
check_size(j,vj,dj);
@ -124,12 +124,12 @@ VectorConfig VectorConfig::exmap(const VectorConfig& delta) const
}
/* ************************************************************************* */
VectorConfig VectorConfig::exmap(const Vector& delta) const
VectorConfig expmap(const VectorConfig& original, const Vector& delta)
{
VectorConfig newConfig;
size_t i = 0;
string j; Vector vj;
FOREACH_PAIR(j, vj, values) {
FOREACH_PAIR(j, vj, original.values) {
size_t mj = vj.size();
Vector dj = sub(delta, i, i+mj);
newConfig.insert(j, vj + dj);

23
cpp/VectorConfig.h
View File

@ -43,18 +43,6 @@ namespace gtsam {
/** Add to vector at position j */
void add(const std::string& j, const Vector& a);
/**
* Add a delta config, needed for use in NonlinearOptimizer
* For VectorConfig, this is just addition.
*/
VectorConfig exmap(const VectorConfig& delta) const;
/**
* Add a delta vector (not a config)
* Will use the ordering that map uses to loop over vectors
*/
VectorConfig exmap(const Vector& delta) const;
const_iterator begin() const {return values.begin();}
const_iterator end() const {return values.end();}
@ -107,6 +95,17 @@ namespace gtsam {
/** Dot product */
double dot(const VectorConfig& b) const;
/**
* Add a delta config, needed for use in NonlinearOptimizer
* For VectorConfig, this is just addition.
*/
friend VectorConfig expmap(const VectorConfig& original, const VectorConfig& delta);
/**
* Add a delta vector (not a config)
* Will use the ordering that map uses to loop over vectors
*/
friend VectorConfig expmap(const VectorConfig& original, const Vector& delta);
private:
/** Serialization function */
friend class boost::serialization::access;

68
cpp/numericalDerivative.h
View File

@ -10,12 +10,14 @@
#include "Matrix.h"
//#define LINEARIZE_AT_IDENTITY
namespace gtsam {
/**
* Numerically compute gradient of scalar function
* Class X is the input argument
* The class X needs to have dim, exmap, vector
* The class X needs to have dim, expmap, vector
*/
template<class X>
Vector numericalGradient(double (*h)(const X&), const X& x, double delta=1e-5) {
@ -24,8 +26,8 @@ namespace gtsam {
const size_t n = x.dim();
Vector d(n,0.0), g(n,0.0);
for (size_t j=0;j<n;j++) {
d(j) += delta; double hxplus = h(x.exmap(d));
d(j) -= 2*delta; double hxmin = h(x.exmap(d));
d(j) += delta; double hxplus = h(expmap(x,d));
d(j) -= 2*delta; double hxmin = h(expmap(x,d));
d(j) += delta; g(j) = (hxplus-hxmin)*factor;
}
return g;
@ -45,18 +47,23 @@ namespace gtsam {
* Templated version (starts with LOWERCASE n)
* Class Y is the output argument
* Class X is the input argument
* Both classes X,Y need dim, exmap, vector
* Both classes X,Y need dim, expmap, vector
*/
template<class Y, class X>
Matrix numericalDerivative11(Y (*h)(const X&), const X& x, double delta=1e-5) {
Vector hx = h(x).vector();
Y hx = h(x);
double factor = 1.0/(2.0*delta);
const size_t m = hx.size(), n = x.dim();
const size_t m = dim(hx), n = dim(x);
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = h(x.exmap(d)).vector();
d(j) -= 2*delta; Vector hxmin = h(x.exmap(d)).vector();
#ifdef LINEARIZE_AT_IDENTITY
d(j) += delta; Vector hxplus = logmap(h(expmap(x,d)));
d(j) -= 2*delta; Vector hxmin = logmap(h(expmap(x,d)));
#else
d(j) += delta; Vector hxplus = logmap(hx, h(expmap(x,d)));
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(expmap(x,d)));
#endif
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -76,19 +83,24 @@ namespace gtsam {
/**
* Templated version (starts with LOWERCASE n)
* All classes Y,X1,X2 need dim, exmap, vector
* All classes Y,X1,X2 need dim, expmap, vector
*/
template<class Y, class X1, class X2>
Matrix numericalDerivative21(Y (*h)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta=1e-5) {
Vector hx = h(x1,x2).vector();
Y hx = h(x1,x2);
double factor = 1.0/(2.0*delta);
const size_t m = hx.size(), n = x1.dim();
const size_t m = dim(hx), n = dim(x1);
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = h(x1.exmap(d),x2).vector();
d(j) -= 2*delta; Vector hxmin = h(x1.exmap(d),x2).vector();
#ifdef LINEARIZE_AT_IDENTITY
d(j) += delta; Vector hxplus = logmap(h(expmap(x1,d),x2));
d(j) -= 2*delta; Vector hxmin = logmap(h(expmap(x1,d),x2));
#else
d(j) += delta; Vector hxplus = logmap(hx, h(expmap(x1,d),x2));
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(expmap(x1,d),x2));
#endif
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -108,21 +120,26 @@ namespace gtsam {
/**
* Templated version (starts with LOWERCASE n)
* All classes Y,X1,X2 need dim, exmap, vector
* All classes Y,X1,X2 need dim, expmap, vector
*/
template<class Y, class X1, class X2>
Matrix numericalDerivative22
(Y (*h)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta=1e-5)
{
Vector hx = h(x1,x2).vector();
Y hx = h(x1,x2);
double factor = 1.0/(2.0*delta);
const size_t m = hx.size(), n = x2.dim();
const size_t m = dim(hx), n = dim(x2);
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = h(x1,x2.exmap(d)).vector();
d(j) -= 2*delta; Vector hxmin = h(x1,x2.exmap(d)).vector();
#ifdef LINEARIZE_AT_IDENTITY
d(j) += delta; Vector hxplus = logmap(h(x1,expmap(x2,d)));
d(j) -= 2*delta; Vector hxmin = logmap(h(x1,expmap(x2,d)));
#else
d(j) += delta; Vector hxplus = logmap(hx, h(x1,expmap(x2,d)));
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(x1,expmap(x2,d)));
#endif
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}
@ -142,21 +159,26 @@ namespace gtsam {
/**
* Templated version (starts with LOWERCASE n)
* All classes Y,X1,X2,X3 need dim, exmap, vector
* All classes Y,X1,X2,X3 need dim, expmap, vector
*/
template<class Y, class X1, class X2, class X3>
Matrix numericalDerivative31
(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta=1e-5)
{
Vector hx = h(x1,x2,x3).vector();
Y hx = h(x1,x2,x3);
double factor = 1.0/(2.0*delta);
const size_t m = hx.size(), n = x1.dim();
const size_t m = dim(hx), n = dim(x1);
Vector d(n,0.0);
Matrix H = zeros(m,n);
for (size_t j=0;j<n;j++) {
d(j) += delta; Vector hxplus = h(x1.exmap(d),x2,x3).vector();
d(j) -= 2*delta; Vector hxmin = h(x1.exmap(d),x2,x3).vector();
#ifdef LINEARIZE_AT_IDENTITY
d(j) += delta; Vector hxplus = logmap(h(expmap(x1,d),x2,x3));
d(j) -= 2*delta; Vector hxmin = logmap(h(expmap(x1,d),x2,x3));
#else
d(j) += delta; Vector hxplus = logmap(hx, h(expmap(x1,d),x2,x3));
d(j) -= 2*delta; Vector hxmin = logmap(hx, h(expmap(x1,d),x2,x3));
#endif
d(j) += delta; Vector dh = (hxplus-hxmin)*factor;
for (size_t i=0;i<m;i++) H(i,j) = dh(i);
}

4
cpp/testPoint2.cpp
View File

@ -11,9 +11,9 @@ using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( Point2, exmap) {
TEST( Point2, expmap) {
Vector d(2);d(0)=1;d(1)=-1;
Point2 a(4,5), b=a.exmap(d),c(5,4);
Point2 a(4,5), b=expmap(a,d),c(5,4);
CHECK(assert_equal(b,c));
}

93
cpp/testPose2.cpp
View File

@ -14,33 +14,9 @@
using namespace gtsam;
using namespace std;
Matrix toManifoldDerivative(Matrix vectorDerivative, Pose2 linearizationPoint) {
Matrix manifoldDerivative(vectorDerivative.size1(), vectorDerivative.size2());
for(int j=0; j<vectorDerivative.size2(); j++) {
// Find the pose implied by the vector derivative, which is usually
// linearized about identity.
Pose2 partialPose = Pose2().exmap(Vector_(3,
vectorDerivative(0,j),
vectorDerivative(1,j),
vectorDerivative(2,j)) + linearizationPoint.vector());
// Now convert the derivative to the tangent space of the new linearization
// point.
Vector manifoldPartial = linearizationPoint.log(partialPose);
manifoldDerivative(0,j) = manifoldPartial(0);
manifoldDerivative(1,j) = manifoldPartial(1);
manifoldDerivative(2,j) = manifoldPartial(2);
}
return manifoldDerivative;
}
/* ************************************************************************* */
//TEST(Pose2, print) {
// Pose2 pose(Point2(1.0,2.0), Rot2(3.0));
// pose.print("thepose");
//}
/* ************************************************************************* */
TEST(Pose2, constructors) {
//cout << "constructors" << endl;
Point2 p;
Pose2 pose(0,p);
Pose2 origin;
@ -49,39 +25,43 @@ TEST(Pose2, constructors) {
/* ************************************************************************* */
TEST(Pose2, manifold) {
//cout << "manifold" << endl;
Pose2 t1(M_PI_2, Point2(1, 2));
Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01));
Pose2 origin;
Vector d12 = t1.log(t2);
CHECK(assert_equal(t2, t1.exmap(d12)));
CHECK(assert_equal(t2, origin.exmap(d12)*t1));
Vector d21 = t2.log(t1);
CHECK(assert_equal(t1, t2.exmap(d21)));
CHECK(assert_equal(t1, origin.exmap(d21)*t2));
Vector d12 = logmap(t1,t2);
CHECK(assert_equal(t2, expmap(t1,d12)));
CHECK(assert_equal(t2, expmap(origin,d12)*t1));
Vector d21 = logmap(t2,t1);
CHECK(assert_equal(t1, expmap(t2,d21)));
CHECK(assert_equal(t1, expmap(origin,d21)*t2));
}
/* ************************************************************************* */
TEST(Pose2, exmap) {
TEST(Pose2, expmap) {
//cout << "expmap" << endl;
Pose2 pose(M_PI_2, Point2(1, 2));
Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01));
Pose2 actual = pose.exmap(Vector_(3, 0.01, -0.015, 0.018));
Pose2 actual = expmap(pose, Vector_(3, 0.01, -0.015, 0.018));
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Pose2, exmap0) {
TEST(Pose2, expmap0) {
//cout << "expmap0" << endl;
Pose2 pose(M_PI_2, Point2(1, 2));
Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01));
Pose2 actual = Pose2().exmap(Vector_(3, 0.01, -0.015, 0.018)) * pose;
Pose2 actual = expmap<Pose2>(Vector_(3, 0.01, -0.015, 0.018)) * pose;
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(Pose2, log) {
TEST(Pose2, logmap) {
//cout << "logmap" << endl;
Pose2 pose0(M_PI_2, Point2(1, 2));
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01));
Vector expected = Vector_(3, 0.01, -0.015, 0.018);
Vector actual = pose0.log(pose);
Vector actual = logmap(pose0,pose);
CHECK(assert_equal(expected, actual));
}
@ -97,6 +77,7 @@ TEST(Pose2, log) {
/* ************************************************************************* */
TEST( Pose2, transform_to )
{
//cout << "transform_to" << endl;
Pose2 pose(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y
Point2 point(-1,4); // landmark at (-1,4)
@ -124,6 +105,7 @@ TEST( Pose2, transform_to )
/* ************************************************************************* */
TEST(Pose2, compose_a)
{
//cout << "compose_a" << endl;
Pose2 pose1(M_PI/4.0, Point2(sqrt(0.5), sqrt(0.5)));
Pose2 pose2(M_PI/2.0, Point2(0.0, 2.0));
@ -133,24 +115,23 @@ TEST(Pose2, compose_a)
Point2 point(sqrt(0.5), 3.0*sqrt(0.5));
Point2 expected_point(-1.0, -1.0);
Point2 actual_point1 = pose2 * pose1 * point;
Point2 actual_point2 = (pose2 * pose1) * point;
Point2 actual_point3 = pose2 * (pose1 * point);
Point2 actual_point1 = transform_to(pose2 * pose1, point);
Point2 actual_point2 = transform_to(pose2, transform_to(pose1, point));
CHECK(assert_equal(expected_point, actual_point1));
CHECK(assert_equal(expected_point, actual_point2));
CHECK(assert_equal(expected_point, actual_point3));
}
/* ************************************************************************* */
TEST(Pose2, compose_b)
{
//cout << "compose_b" << endl;
Pose2 pose1(Rot2(M_PI/10.0), Point2(.75, .5));
Pose2 pose2(Rot2(M_PI/4.0-M_PI/10.0), Point2(0.701289620636, 1.34933052585));
Pose2 pose_expected(Rot2(M_PI/4.0), Point2(1.0, 2.0));
Pose2 pose_actual_op = pose2 * pose1;
Pose2 pose_actual_fcn = pose2.compose(pose1);
Pose2 pose_actual_fcn = compose(pose2,pose1);
CHECK(assert_equal(pose_expected, pose_actual_op));
CHECK(assert_equal(pose_expected, pose_actual_fcn));
@ -159,13 +140,14 @@ TEST(Pose2, compose_b)
/* ************************************************************************* */
TEST(Pose2, compose_c)
{
//cout << "compose_c" << endl;
Pose2 pose1(Rot2(M_PI/4.0), Point2(1.0, 1.0));
Pose2 pose2(Rot2(M_PI/4.0), Point2(sqrt(.5), sqrt(.5)));
Pose2 pose_expected(Rot2(M_PI/2.0), Point2(1.0, 2.0));
Pose2 pose_actual_op = pose2 * pose1;
Pose2 pose_actual_fcn = pose2.compose(pose1);
Pose2 pose_actual_fcn = compose(pose2,pose1);
CHECK(assert_equal(pose_expected, pose_actual_op));
CHECK(assert_equal(pose_expected, pose_actual_fcn));
@ -175,37 +157,32 @@ TEST(Pose2, compose_c)
/* ************************************************************************* */
TEST( Pose2, between )
{
//cout << "between" << endl;
Pose2 p1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 p2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
// expected
Pose2 expected(M_PI_2, Point2(2,2));
Pose2 actual = between(p1,p2);
CHECK(assert_equal(expected,actual));
Matrix expectedH1 = Matrix_(3,3,
0.0,-1.0,-2.0,
1.0, 0.0,-2.0,
0.0, 0.0,-1.0
);
Matrix numericalH1 = numericalDerivative21(between<Pose2>, p1, p2, 1e-5);
Matrix actualH1 = Dbetween1(p1,p2);
CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(numericalH1,actualH1));
Matrix expectedH2 = Matrix_(3,3,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0
);
// actual
Pose2 actual = between(p1,p2);
Matrix actualH1 = Dbetween1(p1,p2);
Matrix numericalH2 = numericalDerivative22(between<Pose2>, p1, p2, 1e-5);
Matrix actualH2 = Dbetween2(p1,p2);
Matrix numericalH1 = numericalDerivative21(between, p1, p2, 1e-5);
numericalH1 = toManifoldDerivative(numericalH1, between(p1,p2));
Matrix numericalH2 = numericalDerivative22(between, p1, p2, 1e-5);
numericalH2 = toManifoldDerivative(numericalH2, between(p1,p2));
CHECK(assert_equal(expected,actual));
CHECK(assert_equal(expectedH1,actualH1));
CHECK(assert_equal(expectedH2,actualH2));
CHECK(assert_equal(numericalH1,actualH1));
CHECK(assert_equal(numericalH2,actualH2));
}

2
cpp/testPose2Factor.cpp
View File

@ -82,7 +82,7 @@ TEST(Pose2Factor, optimize) {
Pose2Config feasible;
feasible.insert("p0", Pose2(0,0,0));
fg.push_back(Pose2Graph::sharedFactor(
new NonlinearEquality<Pose2Config>("p0", feasible, Pose2().dim(), poseCompare)));
new NonlinearEquality<Pose2Config>("p0", feasible, dim(Pose2()), poseCompare)));
fg.push_back(Pose2Graph::sharedFactor(
new Pose2Factor("p0", "p1", Pose2(1,2,M_PI_2), Matrix_(3,3,
0.5, 0.0, 0.0,

148
cpp/testPose3.cpp
View File

@ -16,7 +16,7 @@ Pose3 T(R,t);
Point3 P(0.2,0.7,-2);
Rot3 r1 = rodriguez(-90, 0, 0);
Pose3 pose1(r1, Point3(1, 2, 3));
double error = 1e-9;
double error = 1e-8;
#define PI 3.14159265358979323846
@ -31,15 +31,24 @@ TEST( Pose3, equals)
}
/* ************************************************************************* */
TEST( Pose3, exmap)
TEST( Pose3, expmap_a)
{
Pose3 id;
Vector v(6);
fill(v.begin(), v.end(), 0);
v(0) = 0.3;
CHECK(assert_equal(id.exmap(v), Pose3(R, Point3())));
CHECK(assert_equal(expmap(id,v), Pose3(R, Point3())));
v(3)=0.2;v(4)=0.7;v(5)=-2;
CHECK(assert_equal(id.exmap(v), Pose3(R, P)));
CHECK(assert_equal(expmap(id,v), Pose3(R, P)));
}
TEST(Pose3, expmap_b)
{
Pose3 p1(Rot3(), Point3(100, 0, 0));
Pose3 p2 = expmap(p1, Vector_(6,
0.0, 0.0, 0.1, 0.0, 0.0, 0.0));
Pose3 expected(rodriguez(0.0, 0.0, 0.1), Point3(100.0, 0.0, 0.0));
CHECK(assert_equal(expected, p2));
}
/* ************************************************************************* */
@ -48,35 +57,73 @@ TEST( Pose3, compose )
Matrix actual = (T*T).matrix();
Matrix expected = T.matrix()*T.matrix();
CHECK(assert_equal(actual,expected,error));
}
Matrix numericalH1 = numericalDerivative21<Pose3,Pose3,Pose3>(compose, T, T, 1e-5);
Matrix actualH1 = Dcompose1(T, T);
CHECK(assert_equal(numericalH1,actualH1));
Matrix actualH2 = Dcompose2(T, T);
Matrix numericalH2 = numericalDerivative22<Pose3,Pose3,Pose3>(compose, T, T, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));}
/* ************************************************************************* */
TEST( Pose3, inverse)
{
Matrix actual = T.inverse().matrix();
Matrix actual = inverse(T).matrix();
Matrix expected = inverse(T.matrix());
CHECK(assert_equal(actual,expected,error));
Matrix numericalH = numericalDerivative11<Pose3,Pose3>(inverse, T, 1e-5);
Matrix actualH = Dinverse(T);
CHECK(assert_equal(numericalH,actualH));
}
/* ************************************************************************* */
TEST( Pose3, compose_inverse)
{
Matrix actual = (T*T.inverse()).matrix();
Matrix actual = (T*inverse(T)).matrix();
Matrix expected = eye(4,4);
CHECK(assert_equal(actual,expected,error));
}
/* ************************************************************************* */
// transform derivatives
TEST( Pose3, Dtransform_from1)
TEST( Pose3, Dtransform_from1_a)
{
Matrix computed = Dtransform_from1(T, P);
Matrix numerical = numericalDerivative21(transform_from,T,P);
CHECK(assert_equal(numerical,computed,error));
}
TEST( Pose3, Dtransform_from1_b)
{
Pose3 origin;
Matrix computed = Dtransform_from1(origin, P);
Matrix numerical = numericalDerivative21(transform_from,origin,P);
CHECK(assert_equal(numerical,computed,error));
}
TEST( Pose3, Dtransform_from1_c)
{
Point3 origin;
Pose3 T0(R,origin);
Matrix computed = Dtransform_from1(T0, P);
Matrix numerical = numericalDerivative21(transform_from,T0,P);
CHECK(assert_equal(numerical,computed,error));
}
TEST( Pose3, Dtransform_from1_d)
{
Rot3 I;
Point3 t0(100,0,0);
Pose3 T0(I,t0);
Matrix computed = Dtransform_from1(T0, P);
//print(computed, "Dtransform_from1_d computed:");
Matrix numerical = numericalDerivative21(transform_from,T0,P);
//print(numerical, "Dtransform_from1_d numerical:");
CHECK(assert_equal(numerical,computed,error));
}
/* ************************************************************************* */
TEST( Pose3, Dtransform_from2)
{
Matrix computed = Dtransform_from2(T);
@ -84,6 +131,7 @@ TEST( Pose3, Dtransform_from2)
CHECK(assert_equal(numerical,computed,error));
}
/* ************************************************************************* */
TEST( Pose3, Dtransform_to1)
{
Matrix computed = Dtransform_to1(T, P);
@ -91,12 +139,14 @@ TEST( Pose3, Dtransform_to1)
CHECK(assert_equal(numerical,computed,error));
}
/* ************************************************************************* */
TEST( Pose3, Dtransform_to2)
{
Matrix computed = Dtransform_to2(T,P);
Matrix numerical = numericalDerivative22(transform_to,T,P);
CHECK(assert_equal(numerical,computed,error));
}
/* ************************************************************************* */
TEST( Pose3, transform_to_translate)
{
@ -104,6 +154,8 @@ TEST( Pose3, transform_to_translate)
Point3 expected(9.,18.,27.);
CHECK(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( Pose3, transform_to_rotate)
{
Pose3 transform(rodriguez(0,0,-1.570796), Point3());
@ -111,6 +163,8 @@ TEST( Pose3, transform_to_rotate)
Point3 expected(-1,2,10);
CHECK(assert_equal(expected, actual, 0.001));
}
/* ************************************************************************* */
TEST( Pose3, transform_to)
{
Pose3 transform(rodriguez(0,0,-1.570796), Point3(2,4, 0));
@ -142,12 +196,16 @@ TEST( Pose3, transformPose_to_origin)
Pose3 actual = pose1.transform_to(Pose3());
CHECK(assert_equal(pose1, actual, error));
}
/* ************************************************************************* */
TEST( Pose3, transformPose_to_itself)
{
// transform to itself
Pose3 actual = pose1.transform_to(pose1);
CHECK(assert_equal(Pose3(), actual, error));
}
/* ************************************************************************* */
TEST( Pose3, transformPose_to_translation)
{
// transform translation only
@ -157,6 +215,8 @@ TEST( Pose3, transformPose_to_translation)
Pose3 expected(r, Point3(20.,30.,10.));
CHECK(assert_equal(expected, actual, error));
}
/* ************************************************************************* */
TEST( Pose3, transformPose_to_simple_rotate)
{
// transform translation only
@ -167,6 +227,8 @@ TEST( Pose3, transformPose_to_simple_rotate)
Pose3 expected(Rot3(), Point3(-30.,20.,10.));
CHECK(assert_equal(expected, actual, 0.001));
}
/* ************************************************************************* */
TEST( Pose3, transformPose_to)
{
// transform to
@ -182,22 +244,56 @@ TEST( Pose3, transformPose_to)
/* ************************************************************************* */
TEST( Pose3, composeTransform )
{
// known transform
Rot3 r = rodriguez(0,0,-1.570796);
Pose3 exp_transform(r, Point3(1,2,3));
// current
Rot3 r2 = rodriguez(0,0,0.698131701);
Pose3 current(r2, Point3(21.,32.,13.));
// target
Pose3 target(rodriguez(0,0,2.26892803), Point3(-30.,20.,10.));
// calculate transform
Pose3 actual = compose(current, target);
//verify
CHECK(assert_equal(exp_transform, actual, 0.001));
// known transform
Rot3 R1 = rodriguez(0, 0, -1.570796);
Pose3 expected(R1, Point3(1, 2, 3));
// current
Rot3 R2 = rodriguez(0, 0, 0.698131701);
Pose3 current(R2, Point3(21., 32., 13.));
// target
Pose3 target(rodriguez(0, 0, 2.26892803), Point3(-30., 20., 10.));
// calculate transform
// todo: which should this be?
//Pose3 actual = compose(current, target);
Pose3 actual = between<Pose3> (target, current);
//verify
CHECK(assert_equal(expected, actual, 0.001));
}
/* ************************************************************************* */
TEST(Pose3, manifold) {
//cout << "manifold" << endl;
Pose3 t1 = T;
Pose3 t2 = pose1;
Pose3 origin;
Vector d12 = logmap(t1,t2);
CHECK(assert_equal(t2, expmap(t1,d12)));
// todo: richard - commented out because this tests for "compose-style" (new) expmap
//CHECK(assert_equal(t2, expmap(origin,d12)*t1));
Vector d21 = logmap(t2,t1);
CHECK(assert_equal(t1, expmap(t2,d21)));
// todo: richard - commented out because this tests for "compose-style" (new) expmap
//CHECK(assert_equal(t1, expmap(origin,d21)*t2));
}
/* ************************************************************************* */
TEST( Pose3, between )
{
Pose3 expected = pose1 * inverse(T);
Pose3 actual = between(T, pose1);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(between<Pose3> , T, pose1, 1e-5);
Matrix actualH1 = Dbetween1(T, pose1);
// CHECK(assert_equal(numericalH1,actualH1)); // chain rule does not work ??
Matrix actualH2 = Dbetween2(T, pose1);
Matrix numericalH2 = numericalDerivative22(between<Pose3> , T, pose1, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */

14
cpp/testRot2.cpp
View File

@ -22,13 +22,13 @@ TEST( Rot2, angle)
/* ************************************************************************* */
TEST( Rot2, transpose)
{
CHECK(assert_equal(R.inverse().matrix(),R.transpose()));
CHECK(assert_equal(inverse(R).matrix(),R.transpose()));
}
/* ************************************************************************* */
TEST( Rot2, negtranspose)
{
CHECK(assert_equal(-R.inverse().matrix(),R.negtranspose()));
CHECK(assert_equal(-inverse(R).matrix(),R.negtranspose()));
}
/* ************************************************************************* */
@ -41,7 +41,7 @@ TEST( Rot2, compose)
/* ************************************************************************* */
TEST( Rot2, invcompose)
{
CHECK(assert_equal(Rot2(0.2), Rot2(0.25).invcompose(Rot2(0.45))));
CHECK(assert_equal(Rot2(0.2), invcompose(Rot2(0.25),Rot2(0.45))));
}
/* ************************************************************************* */
@ -53,19 +53,19 @@ TEST( Rot2, equals)
}
/* ************************************************************************* */
TEST( Rot2, exmap)
TEST( Rot2, expmap)
{
Vector v = zero(1);
CHECK(assert_equal(R.exmap(v), R));
CHECK(assert_equal(expmap(R,v), R));
}
/* ************************************************************************* */
TEST(Rot2, log)
TEST(Rot2, logmap)
{
Rot2 rot0(M_PI_2);
Rot2 rot(M_PI);
Vector expected = Vector_(1, M_PI_2);
Vector actual = rot0.log(rot);
Vector actual = logmap(rot0,rot);
CHECK(assert_equal(expected, actual));
}

114
cpp/testRot3.cpp
View File

@ -50,7 +50,7 @@ TEST( Rot3, constructor3) {
TEST( Rot3, transpose) {
Rot3 R(1,2,3,4,5,6,7,8,9);
Point3 r1(1,2,3), r2(4,5,6), r3(7,8,9);
CHECK(assert_equal(R.inverse(),Rot3(r1,r2,r3)));
CHECK(assert_equal(inverse(R),Rot3(r1,r2,r3)));
}
/* ************************************************************************* */
@ -96,54 +96,11 @@ TEST( Rot3, rodriguez3) {
}
/* ************************************************************************* */
TEST( Rot3, exmap)
TEST( Rot3, expmap)
{
Vector v(3);
fill(v.begin(), v.end(), 0);
CHECK(assert_equal(R.exmap(v), R));
}
/* ************************************************************************* */
TEST(Rot3, log)
{
Vector w1 = Vector_(3, 0.1, 0.0, 0.0);
Rot3 R1 = rodriguez(w1);
CHECK(assert_equal(w1, R1.log()));
Vector w2 = Vector_(3, 0.0, 0.1, 0.0);
Rot3 R2 = rodriguez(w2);
CHECK(assert_equal(w2, R2.log()));
Vector w3 = Vector_(3, 0.0, 0.0, 0.1);
Rot3 R3 = rodriguez(w3);
CHECK(assert_equal(w3, R3.log()));
Vector w = Vector_(3, 0.1, 0.4, 0.2);
Rot3 R = rodriguez(w);
CHECK(assert_equal(w, R.log()));
}
/* ************************************************************************* */
TEST(Rot3, between)
{
Rot3 R = rodriguez(0.1, 0.4, 0.2);
Rot3 origin;
CHECK(assert_equal(R, between(origin,R)));
CHECK(assert_equal(R.inverse(), between(R,origin)));
}
/* ************************************************************************* */
TEST(Rot3, manifold)
{
Rot3 t1 = rodriguez(0.1, 0.4, 0.2);
Rot3 t2 = rodriguez(0.3, 0.1, 0.7);
Rot3 origin;
Vector d12 = log(t1, t2);
CHECK(assert_equal(t2, t1.exmap(d12)));
CHECK(assert_equal(t2, origin.exmap(d12)*t1));
Vector d21 = log(t2, t1);
CHECK(assert_equal(t1, t2.exmap(d21)));
CHECK(assert_equal(t1, origin.exmap(d21)*t2));
Vector v(3);
fill(v.begin(), v.end(), 0);
CHECK(assert_equal(expmap(R,v), R));
}
/* ************************************************************************* */
@ -156,6 +113,13 @@ TEST( Rot3, Drotate1)
CHECK(assert_equal(numerical,computed,error));
}
TEST( Rot3, Drotate1_)
{
Matrix computed = Drotate1(inverse(R), P);
Matrix numerical = numericalDerivative21(rotate,inverse(R),P);
CHECK(assert_equal(numerical,computed,error));
}
TEST( Rot3, Drotate2_DNrotate2)
{
Matrix computed = Drotate2(R);
@ -164,6 +128,8 @@ TEST( Rot3, Drotate2_DNrotate2)
}
/* ************************************************************************* */
// unrotate
TEST( Rot3, unrotate)
{
Point3 w = R*P;
@ -188,29 +154,41 @@ TEST( Rot3, Dunrotate2_DNunrotate2)
}
/* ************************************************************************* */
TEST( Rot3, RQ)
TEST( Rot3, compose )
{
// Try RQ on a pure rotation
Matrix actualK; Vector actual;
boost::tie(actualK,actual) = RQ(R.matrix());
Vector expected = Vector_(3,0.14715, 0.385821, 0.231671);
CHECK(assert_equal(eye(3),actualK));
CHECK(assert_equal(expected,actual,1e-6));
Rot3 R1 = rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = rodriguez(0.2, 0.3, 0.5);
// Try using ypr call
actual = R.ypr();
CHECK(assert_equal(expected,actual,1e-6));
Rot3 expected = R1*R2;
Rot3 actual = compose(R1, R2);
CHECK(assert_equal(expected,actual));
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
Matrix K = Matrix_(3,3,
500.0, 0.0, 320.0,
0.0, 500.0, 240.0,
0.0, 0.0, 1.0
);
Matrix A = K*R.matrix();
boost::tie(actualK,actual) = RQ(A);
CHECK(assert_equal(K,actualK));
CHECK(assert_equal(expected,actual,1e-6));
Matrix numericalH1 = numericalDerivative21<Rot3,Rot3,Rot3>(compose, R1, R2, 1e-5);
Matrix actualH1 = Dcompose1(R1, R2);
CHECK(assert_equal(numericalH1,actualH1));
Matrix actualH2 = Dcompose2(R1, R2);
Matrix numericalH2 = numericalDerivative22<Rot3,Rot3,Rot3>(compose, R1, R2, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
TEST( Rot3, between )
{
Rot3 R1 = rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = rodriguez(0.2, 0.3, 0.5);
Rot3 expected = R2*inverse(R1);
Rot3 actual = between(R1, R2);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(between<Rot3> , R1, R2, 1e-5);
Matrix actualH1 = Dbetween1(R1, R2);
CHECK(assert_equal(numericalH1,actualH1));
Matrix actualH2 = Dbetween2(R1, R2);
Matrix numericalH2 = numericalDerivative22(between<Rot3> , R1, R2, 1e-5);
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */

8
cpp/testSQP.cpp
View File

@ -121,7 +121,7 @@ TEST (SQP, problem1_cholesky ) {
if (verbose) delta.print("Delta");
// update initial estimate
VectorConfig newState = state.exmap(delta);
VectorConfig newState = expmap(state, delta);
state = newState;
if (verbose) state.print("Updated State");
@ -222,7 +222,7 @@ TEST (SQP, problem1_sqp ) {
if (verbose) delta.print("Delta");
// update initial estimate
VectorConfig newState = state.exmap(delta.scale(-1.0));
VectorConfig newState = expmap(state, delta.scale(-1.0));
// set the state to the updated state
state = newState;
@ -445,9 +445,9 @@ TEST (SQP, two_pose ) {
if (verbose) delta.print("Delta Config");
// update both state variables
state = state.exmap(delta);
state = expmap(state, delta);
if (verbose) state.print("newState");
state_lambda = state_lambda.exmap(delta);
state_lambda = expmap(state_lambda, delta);
if (verbose) state_lambda.print("newStateLam");
}

10
cpp/testSimulated3D.cpp
View File

@ -17,7 +17,7 @@ using namespace gtsam;
TEST( simulated3D, Dprior_3D )
{
Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0));
Vector v = x1.vector();
Vector v = logmap(x1);
Matrix numerical = NumericalDerivative11(prior_3D,v);
Matrix computed = Dprior_3D(v);
CHECK(assert_equal(numerical,computed,1e-9));
@ -27,9 +27,9 @@ TEST( simulated3D, Dprior_3D )
TEST( simulated3D, DOdo1 )
{
Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0));
Vector v1 = x1.vector();
Vector v1 = logmap(x1);
Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0));
Vector v2 = x2.vector();
Vector v2 = logmap(x2);
Matrix numerical = NumericalDerivative21(odo_3D,v1,v2);
Matrix computed = Dodo1_3D(v1,v2);
CHECK(assert_equal(numerical,computed,1e-9));
@ -39,9 +39,9 @@ TEST( simulated3D, DOdo1 )
TEST( simulated3D, DOdo2 )
{
Pose3 x1(rodriguez(0, 0, 1.57), Point3(1, 5, 0));
Vector v1 = x1.vector();
Vector v1 = logmap(x1);
Pose3 x2(rodriguez(0, 0, 0), Point3(2, 3, 0));
Vector v2 = x2.vector();
Vector v2 = logmap(x2);
Matrix numerical = NumericalDerivative22(odo_3D,v1,v2);
Matrix computed = Dodo2_3D(v1,v2);
CHECK(assert_equal(numerical,computed,1e-9));

2
cpp/testVSLAMConfig.cpp
View File

@ -23,7 +23,7 @@ TEST( VSLAMConfig, update_with_large_delta) {
delta.insert("l1", Vector_(3, 0.1, 0.1, 0.1));
delta.insert("x2", Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1));
VSLAMConfig actual = init.exmap(delta);
VSLAMConfig actual = expmap(init, delta);
VSLAMConfig expected;
expected.addCameraPose(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
expected.addLandmarkPoint(1, Point3(1.1, 2.1, 3.1));

4
cpp/testVSLAMFactor.cpp
View File

@ -59,11 +59,11 @@ TEST( VSLAMFactor, error )
GaussianFactorGraph actual_lfg = graph.linearize(config);
CHECK(assert_equal(expected_lfg,actual_lfg));
// exmap on a config
// expmap on a config
VectorConfig delta;
delta.insert("x1",Vector_(6, 0.,0.,0., 1.,1.,1.));
delta.insert("l1",Vector_(3, 1.,2.,3.));
VSLAMConfig actual_config = config.exmap(delta);
VSLAMConfig actual_config = expmap(config, delta);
VSLAMConfig expected_config;
Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.addCameraPose(1, x2);
Point3 l2(1,2,3); expected_config.addLandmarkPoint(1, l2);

8
cpp/testVectorConfig.cpp
View File

@ -69,11 +69,11 @@ TEST( VectorConfig, contains)
}
/* ************************************************************************* */
TEST( VectorConfig, exmap)
TEST( VectorConfig, expmap)
{
VectorConfig c = createConfig();
Vector v = Vector_(6, 0.0,-1.0, 0.0, 0.0, 1.5, 0.0); // l1, x1, x2
CHECK(assert_equal(c.exmap(c),c.exmap(v)));
CHECK(assert_equal(expmap(c,c),expmap(c,v)));
}
/* ************************************************************************* */
@ -93,7 +93,7 @@ TEST( VectorConfig, plus)
expected.insert("x", wx).insert("y",wy);
// functional
VectorConfig actual = c.exmap(delta);
VectorConfig actual = expmap(c,delta);
CHECK(assert_equal(expected,actual));
}
@ -123,7 +123,7 @@ TEST( VectorConfig, update_with_large_delta) {
delta.insert("y", Vector_(2, 0.1, 0.1));
delta.insert("penguin", Vector_(2, 0.1, 0.1));
VectorConfig actual = init.exmap(delta);
VectorConfig actual = expmap(init,delta);
VectorConfig expected;
expected.insert("x", Vector_(2, 1.1, 2.1));
expected.insert("y", Vector_(2, 3.1, 4.1));