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

fixed size improvements for significant speedup in LBA

release/4.3a0
Chris Beall 2012-05-09 17:19:01 +00:00
parent f8a03ddbca
commit d95e1738d4
3 changed files with 45 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/base/Matrix.h
View File

@ -38,6 +38,8 @@ typedef Eigen::MatrixXd Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor; typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
typedef Eigen::Matrix3d Matrix3; typedef Eigen::Matrix3d Matrix3;
typedef Eigen::Matrix4d Matrix4;
typedef Eigen::Matrix<double,6,6> Matrix6;
// Matrix expressions for accessing parts of matrices // Matrix expressions for accessing parts of matrices
typedef Eigen::Block<Matrix> SubMatrix; typedef Eigen::Block<Matrix> SubMatrix;

66
gtsam/geometry/Pose3.cpp
View File

@ -31,7 +31,8 @@ namespace gtsam {
/** instantiate concept checks */ /** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3); GTSAM_CONCEPT_POSE_INST(Pose3);
static const Matrix I3 = eye(3), Z3 = zeros(3, 3), _I3=-I3, I6 = eye(6); static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3=-I3;
static const Matrix6 I6 = eye(6);
/* ************************************************************************* */ /* ************************************************************************* */
Pose3::Pose3(const Pose2& pose2) : Pose3::Pose3(const Pose2& pose2) :
@ -43,13 +44,13 @@ namespace gtsam {
// 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
Matrix Pose3::adjointMap() const { Matrix6 Pose3::adjointMap() const {
const Matrix R = R_.matrix(); const Matrix3 R = R_.matrix();
const Vector t = t_.vector(); const Vector3 t = t_.vector();
Matrix A = skewSymmetric(t)*R; Matrix3 A = skewSymmetric(t)*R;
Matrix DR = collect(2, &R, &Z3); Matrix6 adj;
Matrix Dt = collect(2, &A, &R); adj << R, Z3, A, R;
return gtsam::stack(2, &DR, &Dt); return adj;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -86,25 +87,30 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Pose3::Logmap(const Pose3& p) { Vector6 Pose3::Logmap(const Pose3& p) {
Vector w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
double t = w.norm(); double t = w.norm();
if (t < 1e-10) if (t < 1e-10) {
return concatVectors(2, &w, &T); Vector6 log;
log << w, T;
return log;
}
else { else {
Matrix W = skewSymmetric(w/t); Matrix3 W = skewSymmetric(w/t);
// Formula from Agrawal06iros, equation (14) // Formula from Agrawal06iros, equation (14)
// simplified with Mathematica, and multiplying in T to avoid matrix math // simplified with Mathematica, and multiplying in T to avoid matrix math
double Tan = tan(0.5*t); double Tan = tan(0.5*t);
Vector WT = W*T; Vector3 WT = W*T;
Vector u = T - (0.5*t)*WT + (1 - t/(2.*Tan)) * (W * WT); Vector3 u = T - (0.5*t)*WT + (1 - t/(2.*Tan)) * (W * WT);
return concatVectors(2, &w, &u); Vector6 log;
log << w, u;
return log;
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::retractFirstOrder(const Vector& xi) const { Pose3 Pose3::retractFirstOrder(const Vector& xi) const {
Vector omega(sub(xi, 0, 3)); Vector3 omega(sub(xi, 0, 3));
Point3 v(sub(xi, 3, 6)); Point3 v(sub(xi, 3, 6));
Rot3 R = R_.retract(omega); // R is done exactly Rot3 R = R_.retract(omega); // R is done exactly
Point3 t = t_ + R_ * v; // First order t approximation Point3 t = t_ + R_ * v; // First order t approximation
@ -130,7 +136,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
// different versions of localCoordinates // different versions of localCoordinates
Vector Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const { Vector6 Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const {
if(mode == Pose3::EXPMAP) { if(mode == Pose3::EXPMAP) {
// Lie group logarithm map, exact inverse of exponential map // Lie group logarithm map, exact inverse of exponential map
return Logmap(between(T)); return Logmap(between(T));
@ -146,8 +152,9 @@ namespace gtsam {
Point3 d = R_.unrotate(T.translation() - t_); Point3 d = R_.unrotate(T.translation() - t_);
// TODO: correct second order t inverse // TODO: correct second order t inverse
Vector6 local;
return Vector_(6,omega(0),omega(1),omega(2),d.x(),d.y(),d.z()); local << omega(0),omega(1),omega(2),d.x(),d.y(),d.z();
return local;
} else { } else {
assert(false); assert(false);
exit(1); exit(1);
@ -155,11 +162,14 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Pose3::matrix() const { Matrix4 Pose3::matrix() const {
const Matrix R = R_.matrix(), T = Matrix_(3,1, t_.vector()); const Matrix3 R = R_.matrix();
const Matrix A34 = collect(2, &R, &T); const Vector3 T = t_.vector();
const Matrix A14 = Matrix_(1,4, 0.0, 0.0, 0.0, 1.0); Eigen::Matrix<double,1,4> A14;
return gtsam::stack(2, &A34, &A14); A14 << 0.0, 0.0, 0.0, 1.0;
Matrix4 mat;
mat << R, T, A14;
return mat;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -175,7 +185,8 @@ namespace gtsam {
if (H1) { if (H1) {
const Matrix R = R_.matrix(); const Matrix R = R_.matrix();
Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z()); Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z());
*H1 = collect(2,&DR,&R); H1->resize(3,6);
(*H1) << DR, R;
} }
if (H2) *H2 = R_.matrix(); if (H2) *H2 = R_.matrix();
return R_ * p + t_; return R_ * p + t_;
@ -188,7 +199,8 @@ namespace gtsam {
if (H1) { if (H1) {
const Point3& q = result; const Point3& q = result;
Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
*H1 = collect(2, &DR, &_I3); H1->resize(3,6);
(*H1) << DR, _I3;
} }
if (H2) *H2 = R_.transpose(); if (H2) *H2 = R_.transpose();
return result; return result;

8
gtsam/geometry/Pose3.h
View File

@ -132,7 +132,7 @@ namespace gtsam {
Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const; Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const;
/// Local 6D coordinates of Pose3 manifold neighborhood around current pose /// Local 6D coordinates of Pose3 manifold neighborhood around current pose
Vector localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const; Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const;
/// @} /// @}
/// @name Lie Group /// @name Lie Group
@ -142,13 +142,13 @@ namespace gtsam {
static Pose3 Expmap(const Vector& xi); static Pose3 Expmap(const Vector& xi);
/// Log map at identity - return the canonical coordinates of this rotation /// Log map at identity - return the canonical coordinates of this rotation
static Vector Logmap(const Pose3& p); static Vector6 Logmap(const Pose3& p);
/** /**
* 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)
*/ */
Matrix adjointMap() const; /// FIXME Not tested - marked as incorrect Matrix6 adjointMap() const; /// FIXME Not tested - marked as incorrect
Vector adjoint(const Vector& xi) const {return adjointMap()*xi; } /// FIXME Not tested - marked as incorrect Vector adjoint(const Vector& xi) const {return adjointMap()*xi; } /// FIXME Not tested - marked as incorrect
/** /**
@ -201,7 +201,7 @@ namespace gtsam {
double z() const { return t_.z(); } double z() const { return t_.z(); }
/** convert to 4*4 matrix */ /** convert to 4*4 matrix */
Matrix matrix() const; Matrix4 matrix() const;
/** receives a pose in world coordinates and transforms it to local coordinates */ /** receives a pose in world coordinates and transforms it to local coordinates */
Pose3 transform_to(const Pose3& pose) const; Pose3 transform_to(const Pose3& pose) const;