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

Reformatted with new style file, renamed some derivatives to Dcal, Dpose, Dpoint etc.

release/4.3a0
Frank Dellaert 2013-10-12 05:13:36 +00:00
parent ca9caf6a66
commit 752a9877c5
4 changed files with 648 additions and 586 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

101
gtsam/geometry/Cal3_S2.cpp
View File

@ -22,61 +22,72 @@
#include <iostream> #include <iostream>
namespace gtsam { namespace gtsam {
using namespace std; using namespace std;
/* ************************************************************************* */ /* ************************************************************************* */
Cal3_S2::Cal3_S2(double fov, int w, int h) : Cal3_S2::Cal3_S2(double fov, int w, int h) :
s_(0), u0_((double) w / 2.0), v0_((double) h / 2.0) { s_(0), u0_((double) w / 2.0), v0_((double) h / 2.0) {
double a = fov * M_PI / 360.0; // fov/2 in radians double a = fov * M_PI / 360.0; // fov/2 in radians
fx_ = (double)w / (2.0*tan(a)); // old formula: fx_ = (double) w * tan(a); fx_ = (double) w / (2.0 * tan(a)); // old formula: fx_ = (double) w * tan(a);
fy_ = fx_; fy_ = fx_;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Cal3_S2::Cal3_S2(const std::string &path) : Cal3_S2::Cal3_S2(const std::string &path) :
fx_(320), fy_(320), s_(0), u0_(320), v0_(140) { fx_(320), fy_(320), s_(0), u0_(320), v0_(140) {
char buffer[200]; char buffer[200];
buffer[0] = 0; buffer[0] = 0;
sprintf(buffer, "%s/calibration_info.txt", path.c_str()); sprintf(buffer, "%s/calibration_info.txt", path.c_str());
std::ifstream infile(buffer, std::ios::in); std::ifstream infile(buffer, std::ios::in);
if (infile) if (infile)
infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_; infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
else { else {
printf("Unable to load the calibration\n"); printf("Unable to load the calibration\n");
exit(0); exit(0);
}
infile.close();
} }
/* ************************************************************************* */ infile.close();
void Cal3_S2::print(const std::string& s) const { }
gtsam::print(matrix(), s);
}
/* ************************************************************************* */ /* ************************************************************************* */
bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { void Cal3_S2::print(const std::string& s) const {
if (fabs(fx_ - K.fx_) > tol) return false; gtsam::print(matrix(), s);
if (fabs(fy_ - K.fy_) > tol) return false; }
if (fabs(s_ - K.s_) > tol) return false;
if (fabs(u0_ - K.u0_) > tol) return false;
if (fabs(v0_ - K.v0_) > tol) return false;
return true;
}
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1, bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
boost::optional<Matrix&> H2) const { if (fabs(fx_ - K.fx_) > tol)
const double x = p.x(), y = p.y(); return false;
if (H1) if (fabs(fy_ - K.fy_) > tol)
*H1 = Matrix_(2, 5, return false;
x, 0.0, y, 1.0, 0.0, if (fabs(s_ - K.s_) > tol)
0.0, y, 0.0, 0.0, 1.0); return false;
if (H2) *H2 = Matrix_(2, 2, fx_, s_, 0.000, fy_); if (fabs(u0_ - K.u0_) > tol)
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); return false;
} if (fabs(v0_ - K.v0_) > tol)
return false;
return true;
}
/* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const {
const double x = p.x(), y = p.y();
if (Dcal)
*Dcal = Matrix_(2, 5, x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0);
if (Dp)
*Dp = Matrix_(2, 2, fx_, s_, 0.000, fy_);
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
/* ************************************************************************* */
Point2 Cal3_S2::calibrate(const Point2& p) const {
const double u = p.x(), v = p.y();
return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)),
(1 / fy_) * (v - v0_));
}
/* ************************************************************************* */ /* ************************************************************************* */

263
gtsam/geometry/Cal3_S2.h
View File

@ -26,166 +26,179 @@
namespace gtsam { namespace gtsam {
/** /**
* @brief The most common 5DOF 3D->2D calibration * @brief The most common 5DOF 3D->2D calibration
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT Cal3_S2 : public DerivedValue<Cal3_S2> { class GTSAM_EXPORT Cal3_S2: public DerivedValue<Cal3_S2> {
private: private:
double fx_, fy_, s_, u0_, v0_; double fx_, fy_, s_, u0_, v0_;
public: public:
typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
/// Create a default calibration that leaves coordinates unchanged /// Create a default calibration that leaves coordinates unchanged
Cal3_S2() : Cal3_S2() :
fx_(1), fy_(1), s_(0), u0_(0), v0_(0) { fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {
} }
/// constructor from doubles /// constructor from doubles
Cal3_S2(double fx, double fy, double s, double u0, double v0) : Cal3_S2(double fx, double fy, double s, double u0, double v0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) { fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
} }
/// constructor from vector /// constructor from vector
Cal3_S2(const Vector &d): fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)){} Cal3_S2(const Vector &d) :
fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {
}
/**
* Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
* @param fov field of view in degrees
* @param w image width
* @param h image height
*/
Cal3_S2(double fov, int w, int h);
/** /// @}
* Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect /// @name Advanced Constructors
* @param fov field of view in degrees /// @{
* @param w image width
* @param h image height
*/
Cal3_S2(double fov, int w, int h);
/// @} /// load calibration from location (default name is calibration_info.txt)
/// @name Advanced Constructors Cal3_S2(const std::string &path);
/// @{
/// load calibration from location (default name is calibration_info.txt) /// @}
Cal3_S2(const std::string &path); /// @name Testable
/// @{
/// @} /// print with optional string
/// @name Testable void print(const std::string& s = "Cal3_S2") const;
/// @{
/// print with optional string /// Check if equal up to specified tolerance
void print(const std::string& s = "Cal3_S2") const; bool equals(const Cal3_S2& K, double tol = 10e-9) const;
/// Check if equal up to specified tolerance /// @}
bool equals(const Cal3_S2& K, double tol = 10e-9) const; /// @name Standard Interface
/// @{
/// @} /// focal length x
/// @name Standard Interface inline double fx() const {
/// @{ return fx_;
}
/// focal length x /// focal length y
inline double fx() const { return fx_;} inline double fy() const {
return fy_;
}
/// focal length y /// skew
inline double fy() const { return fy_;} inline double skew() const {
return s_;
}
/// skew /// image center in x
inline double skew() const { return s_;} inline double px() const {
return u0_;
}
/// image center in x /// image center in y
inline double px() const { return u0_;} inline double py() const {
return v0_;
}
/// image center in y /// return the principal point
inline double py() const { return v0_;} Point2 principalPoint() const {
return Point2(u0_, v0_);
}
/// return the principal point /// vectorized form (column-wise)
Point2 principalPoint() const { Vector vector() const {
return Point2(u0_, v0_); double r[] = { fx_, fy_, s_, u0_, v0_ };
} Vector v(5);
std::copy(r, r + 5, v.data());
return v;
}
/// vectorized form (column-wise) /// return calibration matrix K
Vector vector() const { Matrix matrix() const {
double r[] = { fx_, fy_, s_, u0_, v0_ }; return Matrix_(3, 3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
Vector v(5); }
std::copy(r, r + 5, v.data());
return v;
}
/// return calibration matrix K /// return inverted calibration matrix inv(K)
Matrix matrix() const { Matrix matrix_inverse() const {
return Matrix_(3, 3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
} return Matrix_(3, 3, 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0);
}
/// return inverted calibration matrix inv(K) /**
Matrix matrix_inverse() const { * convert intrinsic coordinates xy to image coordinates uv
const double fxy = fx_*fy_, sv0 = s_*v0_, fyu0 = fy_*u0_; * @param p point in intrinsic coordinates
return Matrix_(3, 3, * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
1.0/fx_, -s_/fxy, (sv0-fyu0)/fxy, * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
0.0, 1.0/fy_, -v0_/fy_, * @return point in image coordinates
0.0, 0.0, 1.0); */
} Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal =
boost::none, boost::optional<Matrix&> Dp = boost::none) const;
/** /**
* convert intrinsic coordinates xy to image coordinates uv * convert image coordinates uv to intrinsic coordinates xy
* with optional derivatives * @param p point in image coordinates
*/ * @return point in intrinsic coordinates
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> H1 = */
boost::none, boost::optional<Matrix&> H2 = boost::none) const; Point2 calibrate(const Point2& p) const;
/// convert image coordinates uv to intrinsic coordinates xy /// @}
Point2 calibrate(const Point2& p) const { /// @name Manifold
const double u = p.x(), v = p.y(); /// @{
return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)),
(1 / fy_) * (v - v0_));
}
/// @} /// return DOF, dimensionality of tangent space
/// @name Manifold inline size_t dim() const {
/// @{ return 5;
}
/// return DOF, dimensionality of tangent space /// return DOF, dimensionality of tangent space
inline size_t dim() const { static size_t Dim() {
return 5; return 5;
} }
/// return DOF, dimensionality of tangent space /// Given 5-dim tangent vector, create new calibration
static size_t Dim() { inline Cal3_S2 retract(const Vector& d) const {
return 5; return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4));
} }
/// Given 5-dim tangent vector, create new calibration /// Unretraction for the calibration
inline Cal3_S2 retract(const Vector& d) const { Vector localCoordinates(const Cal3_S2& T2) const {
return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4)); return vector() - T2.vector();
} }
/// Unretraction for the calibration /// @}
Vector localCoordinates(const Cal3_S2& T2) const { /// @name Advanced Interface
return vector() - T2.vector(); /// @{
}
/// @} private:
/// @name Advanced Interface
/// @{
private: /// Serialization function
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("Cal3_S2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
}
/// Serialization function /// @}
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Cal3_S2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
}
/// @} };
};
} // \ namespace gtsam } // \ namespace gtsam

625
gtsam/geometry/Pose3.cpp
View File

@ -26,333 +26,348 @@ using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */ /** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose3); INSTANTIATE_LIE(Pose3);
/** instantiate concept checks */ /** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3); GTSAM_CONCEPT_POSE_INST(Pose3);
static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3=-I3; static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3;
static const Matrix6 I6 = eye(6); static const Matrix6 I6 = eye(6);
/* ************************************************************************* */ /* ************************************************************************* */
Pose3::Pose3(const Pose2& pose2) : Pose3::Pose3(const Pose2& pose2) :
R_(Rot3::rodriguez(0, 0, pose2.theta())), R_(Rot3::rodriguez(0, 0, pose2.theta())), t_(
t_(Point3(pose2.x(), pose2.y(), 0)) { Point3(pose2.x(), pose2.y(), 0)) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Calculate Adjoint map // Calculate Adjoint map
// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
// Experimental - unit tests of derivatives based on it do not check out yet // Experimental - unit tests of derivatives based on it do not check out yet
Matrix6 Pose3::AdjointMap() const { Matrix6 Pose3::AdjointMap() const {
const Matrix3 R = R_.matrix(); const Matrix3 R = R_.matrix();
const Vector3 t = t_.vector(); const Vector3 t = t_.vector();
Matrix3 A = skewSymmetric(t)*R; Matrix3 A = skewSymmetric(t) * R;
Matrix6 adj; Matrix6 adj;
adj << R, Z3, A, R; adj << R, Z3, A, R;
return adj; return adj;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix6 Pose3::adjointMap(const Vector& xi) { Matrix6 Pose3::adjointMap(const Vector& xi) {
Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
Matrix6 adj; Matrix6 adj;
adj << w_hat, Z3, v_hat, w_hat; adj << w_hat, Z3, v_hat, w_hat;
return adj; return adj;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Pose3::adjoint(const Vector& xi, const Vector& y, boost::optional<Matrix&> H) { Vector Pose3::adjoint(const Vector& xi, const Vector& y,
if (H) { boost::optional<Matrix&> H) {
*H = zeros(6,6); if (H) {
for (int i = 0; i<6; ++i) { *H = zeros(6, 6);
Vector dxi = zero(6); dxi(i) = 1.0; for (int i = 0; i < 6; ++i) {
Matrix Gi = adjointMap(dxi); Vector dxi = zero(6);
(*H).col(i) = Gi*y; dxi(i) = 1.0;
} Matrix Gi = adjointMap(dxi);
} (*H).col(i) = Gi * y;
return adjointMap(xi)*y;
}
/* ************************************************************************* */
Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, boost::optional<Matrix&> H) {
if (H) {
*H = zeros(6,6);
for (int i = 0; i<6; ++i) {
Vector dxi = zero(6); dxi(i) = 1.0;
Matrix GTi = adjointMap(dxi).transpose();
(*H).col(i) = GTi*y;
}
}
Matrix adjT = adjointMap(xi).transpose();
return adjointMap(xi).transpose() * y;
}
/* ************************************************************************* */
Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
// Bernoulli numbers, from Wikipedia
static const Vector B = Vector_(9, 1.0, -1.0/2.0, 1./6., 0.0, -1.0/30.0, 0.0, 1.0/42.0, 0.0, -1.0/30);
static const int N = 5; // order of approximation
Matrix res = I6;
Matrix6 ad_i = I6;
Matrix6 ad_xi = adjointMap(xi);
double fac = 1.0;
for (int i = 1 ; i<N; ++i) {
ad_i = ad_xi * ad_i;
fac = fac*i;
res = res + B(i)/fac*ad_i;
}
return res;
}
/* ************************************************************************* */
void Pose3::print(const string& s) const {
cout << s;
R_.print("R:\n");
t_.print("t: ");
}
/* ************************************************************************* */
bool Pose3::equals(const Pose3& pose, double tol) const {
return R_.equals(pose.R_,tol) && t_.equals(pose.t_,tol);
}
/* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */
Pose3 Pose3::Expmap(const Vector& xi) {
// get angular velocity omega and translational velocity v from twist xi
Point3 w(xi(0),xi(1),xi(2)), v(xi(3),xi(4),xi(5));
double theta = w.norm();
if (theta < 1e-10) {
static const Rot3 I;
return Pose3(I, v);
}
else {
Point3 n(w/theta); // axis unit vector
Rot3 R = Rot3::rodriguez(n.vector(),theta);
double vn = n.dot(v); // translation parallel to n
Point3 n_cross_v = n.cross(v); // points towards axis
Point3 t = (n_cross_v - R*n_cross_v)/theta + vn*n;
return Pose3(R, t);
} }
} }
return adjointMap(xi) * y;
}
/* ************************************************************************* */ /* ************************************************************************* */
Vector6 Pose3::Logmap(const Pose3& p) { Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y,
Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); boost::optional<Matrix&> H) {
double t = w.norm(); if (H) {
if (t < 1e-10) { *H = zeros(6, 6);
Vector6 log; for (int i = 0; i < 6; ++i) {
log << w, T; Vector dxi = zero(6);
return log; dxi(i) = 1.0;
} Matrix GTi = adjointMap(dxi).transpose();
else { (*H).col(i) = GTi * y;
Matrix3 W = skewSymmetric(w/t);
// Formula from Agrawal06iros, equation (14)
// simplified with Mathematica, and multiplying in T to avoid matrix math
double Tan = tan(0.5*t);
Vector3 WT = W*T;
Vector3 u = T - (0.5*t)*WT + (1 - t/(2.*Tan)) * (W * WT);
Vector6 log;
log << w, u;
return log;
} }
} }
Matrix adjT = adjointMap(xi).transpose();
return adjointMap(xi).transpose() * y;
}
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::retractFirstOrder(const Vector& xi) const { Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
Vector3 omega(sub(xi, 0, 3)); // Bernoulli numbers, from Wikipedia
Point3 v(sub(xi, 3, 6)); static const Vector B = Vector_(9, 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
Rot3 R = R_.retract(omega); // R is done exactly 0.0, 1.0 / 42.0, 0.0, -1.0 / 30);
Point3 t = t_ + R_ * v; // First order t approximation static const int N = 5; // order of approximation
return Pose3(R, t); Matrix res = I6;
Matrix6 ad_i = I6;
Matrix6 ad_xi = adjointMap(xi);
double fac = 1.0;
for (int i = 1; i < N; ++i) {
ad_i = ad_xi * ad_i;
fac = fac * i;
res = res + B(i) / fac * ad_i;
} }
return res;
}
/* ************************************************************************* */ /* ************************************************************************* */
// Different versions of retract void Pose3::print(const string& s) const {
Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const { cout << s;
if(mode == Pose3::EXPMAP) { R_.print("R:\n");
// Lie group exponential map, traces out geodesic t_.print("t: ");
return compose(Expmap(xi)); }
} else if(mode == Pose3::FIRST_ORDER) {
// First order
return retractFirstOrder(xi);
} else {
// Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently
// Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation
assert(false);
exit(1);
}
}
/* ************************************************************************* */ /* ************************************************************************* */
// different versions of localCoordinates bool Pose3::equals(const Pose3& pose, double tol) const {
Vector6 Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const { return R_.equals(pose.R_, tol) && t_.equals(pose.t_, tol);
if(mode == Pose3::EXPMAP) { }
// Lie group logarithm map, exact inverse of exponential map
return Logmap(between(T));
} else if(mode == Pose3::FIRST_ORDER) {
// R is always done exactly in all three retract versions below
Vector3 omega = R_.localCoordinates(T.rotation());
// Incorrect version /* ************************************************************************* */
// Independently computes the logmap of the translation and rotation /** Modified from Murray94book version (which assumes w and v normalized?) */
// Vector v = t_.localCoordinates(T.translation()); Pose3 Pose3::Expmap(const Vector& xi) {
// Correct first order t inverse // get angular velocity omega and translational velocity v from twist xi
Point3 d = R_.unrotate(T.translation() - t_); Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
// TODO: correct second order t inverse double theta = w.norm();
Vector6 local; if (theta < 1e-10) {
local << omega(0),omega(1),omega(2),d.x(),d.y(),d.z(); static const Rot3 I;
return local; return Pose3(I, v);
} else { } else {
assert(false); Point3 n(w / theta); // axis unit vector
exit(1); Rot3 R = Rot3::rodriguez(n.vector(), theta);
} double vn = n.dot(v); // translation parallel to n
} Point3 n_cross_v = n.cross(v); // points towards axis
Point3 t = (n_cross_v - R * n_cross_v) / theta + vn * n;
/* ************************************************************************* */
Matrix4 Pose3::matrix() const {
const Matrix3 R = R_.matrix();
const Vector3 T = t_.vector();
Eigen::Matrix<double,1,4> A14;
A14 << 0.0, 0.0, 0.0, 1.0;
Matrix4 mat;
mat << R, T, A14;
return mat;
}
/* ************************************************************************* */
Pose3 Pose3::transform_to(const Pose3& pose) const {
Rot3 cRv = R_ * Rot3(pose.R_.inverse());
Point3 t = pose.transform_to(t_);
return Pose3(cRv, t);
}
/* ************************************************************************* */
Point3 Pose3::transform_from(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) {
const Matrix R = R_.matrix();
Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
H1->resize(3,6);
(*H1) << DR, R;
}
if (H2) *H2 = R_.matrix();
return R_ * p + t_;
}
/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Point3 result = R_.unrotate(p - t_);
if (H1) {
const Point3& q = result;
Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
H1->resize(3,6);
(*H1) << DR, _I3;
}
if (H2) *H2 = R_.transpose();
return result;
}
/* ************************************************************************* */
Pose3 Pose3::compose(const Pose3& p2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = p2.inverse().AdjointMap();
if (H2) *H2 = I6;
return (*this) * p2;
}
/* ************************************************************************* */
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -AdjointMap();
Rot3 Rt = R_.inverse();
return Pose3(Rt, Rt*(-t_));
}
/* ************************************************************************* */
// between = compose(p2,inverse(p1));
Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Pose3 result = inverse()*p2;
if (H1) *H1 = -result.inverse().AdjointMap();
if (H2) *H2 = I6;
return result;
}
/* ************************************************************************* */
double Pose3::range(const Point3& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (!H1 && !H2) return transform_to(point).norm();
Point3 d = transform_to(point, H1, H2);
double x = d.x(), y = d.y(), z = d.z(),
d2 = x * x + y * y + z * z, n = sqrt(d2);
Matrix D_result_d = Matrix_(1, 3, x / n, y / n, z / n);
if (H1) *H1 = D_result_d * (*H1);
if (H2) *H2 = D_result_d * (*H2);
return n;
}
/* ************************************************************************* */
double Pose3::range(const Pose3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
double r = range(point.translation(), H1, H2);
if (H2) {
Matrix H2_ = *H2 * point.rotation().matrix();
*H2 = zeros(1, 6);
insertSub(*H2, H2_, 0, 3);
}
return r;
}
/* ************************************************************************* */
boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
const size_t n = pairs.size();
if (n<3) return boost::none; // we need at least three pairs
// calculate centroids
Vector cp = zero(3),cq = zero(3);
BOOST_FOREACH(const Point3Pair& pair, pairs) {
cp += pair.first.vector();
cq += pair.second.vector();
}
double f = 1.0/n;
cp *= f; cq *= f;
// Add to form H matrix
Matrix H = zeros(3,3);
BOOST_FOREACH(const Point3Pair& pair, pairs) {
Vector dp = pair.first.vector() - cp;
Vector dq = pair.second.vector() - cq;
H += dp * dq.transpose();
}
// Compute SVD
Matrix U,V;
Vector S;
svd(H,U,S,V);
// Recover transform with correction from Eggert97machinevisionandapplications
Matrix UVtranspose = U * V.transpose();
Matrix detWeighting = eye(3,3);
detWeighting(2,2) = UVtranspose.determinant();
Rot3 R(Matrix(V * detWeighting * U.transpose()));
Point3 t = Point3(cq) - R * Point3(cp);
return Pose3(R, t); return Pose3(R, t);
} }
}
/* ************************************************************************* */ /* ************************************************************************* */
std::ostream &operator<<(std::ostream &os, const Pose3& pose) { Vector6 Pose3::Logmap(const Pose3& p) {
os << pose.rotation() << "\n" << pose.translation() << endl; Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
return os; double t = w.norm();
if (t < 1e-10) {
Vector6 log;
log << w, T;
return log;
} else {
Matrix3 W = skewSymmetric(w / t);
// Formula from Agrawal06iros, equation (14)
// simplified with Mathematica, and multiplying in T to avoid matrix math
double Tan = tan(0.5 * t);
Vector3 WT = W * T;
Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
Vector6 log;
log << w, u;
return log;
} }
}
/* ************************************************************************* */
Pose3 Pose3::retractFirstOrder(const Vector& xi) const {
Vector3 omega(sub(xi, 0, 3));
Point3 v(sub(xi, 3, 6));
Rot3 R = R_.retract(omega); // R is done exactly
Point3 t = t_ + R_ * v; // First order t approximation
return Pose3(R, t);
}
/* ************************************************************************* */
// Different versions of retract
Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const {
if (mode == Pose3::EXPMAP) {
// Lie group exponential map, traces out geodesic
return compose(Expmap(xi));
} else if (mode == Pose3::FIRST_ORDER) {
// First order
return retractFirstOrder(xi);
} else {
// Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently
// Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation
assert(false);
exit(1);
}
}
/* ************************************************************************* */
// different versions of localCoordinates
Vector6 Pose3::localCoordinates(const Pose3& T,
Pose3::CoordinatesMode mode) const {
if (mode == Pose3::EXPMAP) {
// Lie group logarithm map, exact inverse of exponential map
return Logmap(between(T));
} else if (mode == Pose3::FIRST_ORDER) {
// R is always done exactly in all three retract versions below
Vector3 omega = R_.localCoordinates(T.rotation());
// Incorrect version
// Independently computes the logmap of the translation and rotation
// Vector v = t_.localCoordinates(T.translation());
// Correct first order t inverse
Point3 d = R_.unrotate(T.translation() - t_);
// TODO: correct second order t inverse
Vector6 local;
local << omega(0), omega(1), omega(2), d.x(), d.y(), d.z();
return local;
} else {
assert(false);
exit(1);
}
}
/* ************************************************************************* */
Matrix4 Pose3::matrix() const {
const Matrix3 R = R_.matrix();
const Vector3 T = t_.vector();
Eigen::Matrix<double, 1, 4> A14;
A14 << 0.0, 0.0, 0.0, 1.0;
Matrix4 mat;
mat << R, T, A14;
return mat;
}
/* ************************************************************************* */
Pose3 Pose3::transform_to(const Pose3& pose) const {
Rot3 cRv = R_ * Rot3(pose.R_.inverse());
Point3 t = pose.transform_to(t_);
return Pose3(cRv, t);
}
/* ************************************************************************* */
Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint) const {
if (Dpose) {
const Matrix R = R_.matrix();
Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
Dpose->resize(3, 6);
(*Dpose) << DR, R;
}
if (Dpoint)
*Dpoint = R_.matrix();
return R_ * p + t_;
}
/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint) const {
const Point3 result = R_.unrotate(p - t_);
if (Dpose) {
const Point3& q = result;
Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
Dpose->resize(3, 6);
(*Dpose) << DR, _I3;
}
if (Dpoint)
*Dpoint = R_.transpose();
return result;
}
/* ************************************************************************* */
Pose3 Pose3::compose(const Pose3& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (H1)
*H1 = p2.inverse().AdjointMap();
if (H2)
*H2 = I6;
return (*this) * p2;
}
/* ************************************************************************* */
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
if (H1)
*H1 = -AdjointMap();
Rot3 Rt = R_.inverse();
return Pose3(Rt, Rt * (-t_));
}
/* ************************************************************************* */
// between = compose(p2,inverse(p1));
Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Pose3 result = inverse() * p2;
if (H1)
*H1 = -result.inverse().AdjointMap();
if (H2)
*H2 = I6;
return result;
}
/* ************************************************************************* */
double Pose3::range(const Point3& point, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (!H1 && !H2)
return transform_to(point).norm();
Point3 d = transform_to(point, H1, H2);
double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt(
d2);
Matrix D_result_d = Matrix_(1, 3, x / n, y / n, z / n);
if (H1)
*H1 = D_result_d * (*H1);
if (H2)
*H2 = D_result_d * (*H2);
return n;
}
/* ************************************************************************* */
double Pose3::range(const Pose3& point, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
double r = range(point.translation(), H1, H2);
if (H2) {
Matrix H2_ = *H2 * point.rotation().matrix();
*H2 = zeros(1, 6);
insertSub(*H2, H2_, 0, 3);
}
return r;
}
/* ************************************************************************* */
boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
const size_t n = pairs.size();
if (n < 3)
return boost::none; // we need at least three pairs
// calculate centroids
Vector cp = zero(3), cq = zero(3);
BOOST_FOREACH(const Point3Pair& pair, pairs){
cp += pair.first.vector();
cq += pair.second.vector();
}
double f = 1.0 / n;
cp *= f;
cq *= f;
// Add to form H matrix
Matrix H = zeros(3, 3);
BOOST_FOREACH(const Point3Pair& pair, pairs){
Vector dp = pair.first.vector() - cp;
Vector dq = pair.second.vector() - cq;
H += dp * dq.transpose();
}
// Compute SVD
Matrix U, V;
Vector S;
svd(H, U, S, V);
// Recover transform with correction from Eggert97machinevisionandapplications
Matrix UVtranspose = U * V.transpose();
Matrix detWeighting = eye(3, 3);
detWeighting(2, 2) = UVtranspose.determinant();
Rot3 R(Matrix(V * detWeighting * U.transpose()));
Point3 t = Point3(cq) - R * Point3(cp);
return Pose3(R, t);
}
/* ************************************************************************* */
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
os << pose.rotation() << "\n" << pose.translation() << endl;
return os;
}
} // namespace gtsam } // namespace gtsam

245
gtsam/geometry/Pose3.h
View File

@ -15,7 +15,6 @@
*/ */
// \callgraph // \callgraph
#pragma once #pragma once
#include <gtsam/config.h> #include <gtsam/config.h>
@ -32,111 +31,123 @@
namespace gtsam { namespace gtsam {
class Pose2; // forward declare class Pose2;
// forward declare
/**
* A 3D pose (R,t) : (Rot3,Point3)
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Pose3: public DerivedValue<Pose3> {
public:
static const size_t dimension = 6;
/** Pose Concept requirements */
typedef Rot3 Rotation;
typedef Point3 Translation;
private:
Rot3 R_;
Point3 t_;
public:
/// @name Standard Constructors
/// @{
/** Default constructor is origin */
Pose3() {
}
/** 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 Pose2 */
explicit Pose3(const Pose2& pose2);
/** 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)) {
}
/// @}
/// @name Testable
/// @{
/// print with optional string
void print(const std::string& s = "") const;
/// assert equality up to a tolerance
bool equals(const Pose3& pose, double tol = 1e-9) const;
/// @}
/// @name Group
/// @{
/// identity for group operation
static Pose3 identity() {
return Pose3();
}
/// inverse transformation with derivatives
Pose3 inverse(boost::optional<Matrix&> H1 = boost::none) const;
///compose this transformation onto another (first *this and then p2)
Pose3 compose(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
/// compose syntactic sugar
Pose3 operator*(const Pose3& T) const {
return Pose3(R_ * T.R_, t_ + R_ * T.t_);
}
/** /**
* A 3D pose (R,t) : (Rot3,Point3) * Return relative pose between p1 and p2, in p1 coordinate frame
* @addtogroup geometry * as well as optionally the derivatives
* \nosubgrouping
*/ */
class GTSAM_EXPORT Pose3 : public DerivedValue<Pose3> { Pose3 between(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
public: boost::optional<Matrix&> H2 = boost::none) const;
static const size_t dimension = 6;
/** Pose Concept requirements */ /// @}
typedef Rot3 Rotation; /// @name Manifold
typedef Point3 Translation; /// @{
private: /** Enum to indicate which method should be used in Pose3::retract() and
Rot3 R_; * Pose3::localCoordinates()
Point3 t_; */
enum CoordinatesMode {
EXPMAP, ///< The correct exponential map, computationally expensive.
FIRST_ORDER ///< A fast first-order approximation to the exponential map.
};
public: /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes
static size_t Dim() {
return dimension;
}
/// @name Standard Constructors /// Dimensionality of the tangent space = 6 DOF
/// @{ size_t dim() const {
return dimension;
}
/** Default constructor is origin */ /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map
Pose3() {} Pose3 retractFirstOrder(const Vector& d) const;
/** Copy constructor */ /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ to Pose3 manifold neighborhood around current pose
Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {} Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode =
POSE3_DEFAULT_COORDINATES_MODE) const;
/** Construct from R,t */ /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose
Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {} Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =POSE3_DEFAULT_COORDINATES_MODE) const;
/** Construct from Pose2 */
explicit Pose3(const Pose2& pose2);
/** 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)) {}
/// @}
/// @name Testable
/// @{
/// print with optional string
void print(const std::string& s = "") const;
/// assert equality up to a tolerance
bool equals(const Pose3& pose, double tol = 1e-9) const;
/// @}
/// @name Group
/// @{
/// identity for group operation
static Pose3 identity() { return Pose3(); }
/// inverse transformation with derivatives
Pose3 inverse(boost::optional<Matrix&> H1=boost::none) const;
///compose this transformation onto another (first *this and then p2)
Pose3 compose(const Pose3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/// compose syntactic sugar
Pose3 operator*(const Pose3& T) const {
return Pose3(R_*T.R_, t_ + R_*T.t_);
}
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives
*/
Pose3 between(const Pose3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/// @}
/// @name Manifold
/// @{
/** Enum to indicate which method should be used in Pose3::retract() and
* Pose3::localCoordinates()
*/
enum CoordinatesMode {
EXPMAP, ///< The correct exponential map, computationally expensive.
FIRST_ORDER ///< A fast first-order approximation to the exponential map.
};
/// Dimensionality of tangent space = 6 DOF - used to autodetect sizes
static size_t Dim() { return dimension; }
/// Dimensionality of the tangent space = 6 DOF
size_t dim() const { return dimension; }
/// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map
Pose3 retractFirstOrder(const Vector& d) const;
/// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ to Pose3 manifold neighborhood around current pose
Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const;
/// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose
Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const;
/// @} /// @}
/// @name Lie Group /// @name Lie Group
@ -218,16 +229,28 @@ namespace gtsam {
/// @name Group Action on Point3 /// @name Group Action on Point3
/// @{ /// @{
/** receives the point in Pose coordinates and transforms it to world coordinates */ /**
* @brief takes point in Pose coordinates and transforms it to world coordinates
* @param p point in Pose coordinates
* @param Dpose optional 3*6 Jacobian wrpt this pose
* @param Dpoint optional 3*3 Jacobian wrpt point
* @return point in world coordinates
*/
Point3 transform_from(const Point3& p, Point3 transform_from(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const; boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const;
/** syntactic sugar for transform_from */ /** syntactic sugar for transform_from */
inline Point3 operator*(const Point3& p) const { return transform_from(p); } inline Point3 operator*(const Point3& p) const { return transform_from(p); }
/** receives the point in world coordinates and transforms it to Pose coordinates */ /**
* @brief takes point in world coordinates and transforms it to Pose coordinates
* @param p point in world coordinates
* @param Dpose optional 3*6 Jacobian wrpt this pose
* @param Dpoint optional 3*3 Jacobian wrpt point
* @return point in Pose coordinates
*/
Point3 transform_to(const Point3& p, Point3 transform_to(const Point3& p,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const; boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const;
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
@ -305,7 +328,7 @@ namespace gtsam {
} }
/// @} /// @}
}; // Pose3 class };// Pose3 class
/** /**
* wedge for Pose3: * wedge for Pose3:
@ -314,16 +337,16 @@ namespace gtsam {
* v = 3D velocity * v = 3D velocity
* @return xihat, 4*4 element of Lie algebra that can be exponentiated * @return xihat, 4*4 element of Lie algebra that can be exponentiated
*/ */
template <> template<>
inline Matrix wedge<Pose3>(const Vector& xi) { inline Matrix wedge<Pose3>(const Vector& xi) {
return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
} }
/** /**
* Calculate pose between a vector of 3D point correspondences (p,q) * Calculate pose between a vector of 3D point correspondences (p,q)
* where q = Pose3::transform_from(p) = t + R*p * where q = Pose3::transform_from(p) = t + R*p
*/ */
typedef std::pair<Point3,Point3> Point3Pair; typedef std::pair<Point3, Point3> Point3Pair;
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs); GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
} // namespace gtsam } // namespace gtsam