fixed size improvements for significant speedup in LBA
parent
f8a03ddbca
commit
d95e1738d4
|
@ -38,6 +38,8 @@ typedef Eigen::MatrixXd Matrix;
|
|||
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
|
||||
|
||||
typedef Eigen::Matrix3d Matrix3;
|
||||
typedef Eigen::Matrix4d Matrix4;
|
||||
typedef Eigen::Matrix<double,6,6> Matrix6;
|
||||
|
||||
// Matrix expressions for accessing parts of matrices
|
||||
typedef Eigen::Block<Matrix> SubMatrix;
|
||||
|
|
|
@ -31,7 +31,8 @@ namespace gtsam {
|
|||
/** instantiate concept checks */
|
||||
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) :
|
||||
|
@ -43,13 +44,13 @@ namespace gtsam {
|
|||
// Calculate Adjoint map
|
||||
// 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
|
||||
Matrix Pose3::adjointMap() const {
|
||||
const Matrix R = R_.matrix();
|
||||
const Vector t = t_.vector();
|
||||
Matrix A = skewSymmetric(t)*R;
|
||||
Matrix DR = collect(2, &R, &Z3);
|
||||
Matrix Dt = collect(2, &A, &R);
|
||||
return gtsam::stack(2, &DR, &Dt);
|
||||
Matrix6 Pose3::adjointMap() const {
|
||||
const Matrix3 R = R_.matrix();
|
||||
const Vector3 t = t_.vector();
|
||||
Matrix3 A = skewSymmetric(t)*R;
|
||||
Matrix6 adj;
|
||||
adj << R, Z3, A, R;
|
||||
return adj;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
@ -86,25 +87,30 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose3::Logmap(const Pose3& p) {
|
||||
Vector w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
|
||||
Vector6 Pose3::Logmap(const Pose3& p) {
|
||||
Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
|
||||
double t = w.norm();
|
||||
if (t < 1e-10)
|
||||
return concatVectors(2, &w, &T);
|
||||
if (t < 1e-10) {
|
||||
Vector6 log;
|
||||
log << w, T;
|
||||
return log;
|
||||
}
|
||||
else {
|
||||
Matrix W = skewSymmetric(w/t);
|
||||
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);
|
||||
Vector WT = W*T;
|
||||
Vector u = T - (0.5*t)*WT + (1 - t/(2.*Tan)) * (W * WT);
|
||||
return concatVectors(2, &w, &u);
|
||||
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 {
|
||||
Vector omega(sub(xi, 0, 3));
|
||||
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
|
||||
|
@ -130,7 +136,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// 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) {
|
||||
// Lie group logarithm map, exact inverse of exponential map
|
||||
return Logmap(between(T));
|
||||
|
@ -146,8 +152,9 @@ namespace gtsam {
|
|||
Point3 d = R_.unrotate(T.translation() - t_);
|
||||
|
||||
// TODO: correct second order t inverse
|
||||
|
||||
return Vector_(6,omega(0),omega(1),omega(2),d.x(),d.y(),d.z());
|
||||
Vector6 local;
|
||||
local << omega(0),omega(1),omega(2),d.x(),d.y(),d.z();
|
||||
return local;
|
||||
} else {
|
||||
assert(false);
|
||||
exit(1);
|
||||
|
@ -155,11 +162,14 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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 gtsam::stack(2, &A34, &A14);
|
||||
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;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
@ -175,7 +185,8 @@ namespace gtsam {
|
|||
if (H1) {
|
||||
const Matrix R = R_.matrix();
|
||||
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();
|
||||
return R_ * p + t_;
|
||||
|
@ -188,7 +199,8 @@ namespace gtsam {
|
|||
if (H1) {
|
||||
const Point3& q = result;
|
||||
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();
|
||||
return result;
|
||||
|
|
|
@ -132,7 +132,7 @@ namespace gtsam {
|
|||
Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const;
|
||||
|
||||
/// 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
|
||||
|
@ -142,13 +142,13 @@ namespace gtsam {
|
|||
static Pose3 Expmap(const Vector& xi);
|
||||
|
||||
/// 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
|
||||
* 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
|
||||
|
||||
/**
|
||||
|
@ -201,7 +201,7 @@ namespace gtsam {
|
|||
double z() const { return t_.z(); }
|
||||
|
||||
/** convert to 4*4 matrix */
|
||||
Matrix matrix() const;
|
||||
Matrix4 matrix() const;
|
||||
|
||||
/** receives a pose in world coordinates and transforms it to local coordinates */
|
||||
Pose3 transform_to(const Pose3& pose) const;
|
||||
|
|
Loading…
Reference in New Issue