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

updated Matrix.h with commonly used matrices.

release/4.3a0
Natesh Srinivasan 2014-12-03 09:59:10 -05:00
parent a8c1510343
commit 4e557d38e6
20 changed files with 103 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -6,3 +6,4 @@
/examples/Data/pose2example-rewritten.txt
/examples/Data/pose3example-rewritten.txt
*.txt.user
*.txt.user.6d59f0c

42
gtsam/base/Matrix.h
View File

@ -76,6 +76,48 @@ typedef Eigen::Matrix<double,3,9> Matrix39;
typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix;
// Identity Matrices
static const int I1 = 1;
static const Matrix2 I2 = Matrix2::Identity();
static const Matrix3 I3 = Matrix3::Identity();
static const Matrix4 I4 = Matrix4::Identity();
static const Matrix5 I5 = Matrix5::Identity();
static const Matrix6 I6 = Matrix6::Identity();
// Negative Identity
static const Matrix2 _I2 = -I2;
static const Matrix3 _I3 = -I3;
static const Matrix4 _I4 = -I4;
static const Matrix5 _I5 = -I5;
static const Matrix6 _I6 = -I6;
// Zero matrices
// TODO : Make these for rectangular sized matrices as well.
static const int Z1 = 0;
static const Matrix2 Z2 = Matrix2::Zero();
static const Matrix3 Z3 = Matrix3::Zero();
static const Matrix4 Z4 = Matrix4::Zero();
static const Matrix5 Z5 = Matrix5::Zero();
static const Matrix6 Z6 = Matrix6::Zero();
// Ones matrices
// TODO : Make these for rectangular sized matrices as well.
static const int O1 = 1;
static const Matrix2 O2 = Matrix2::Ones();
static const Matrix3 O3 = Matrix3::Ones();
static const Matrix4 O4 = Matrix4::Ones();
static const Matrix5 O5 = Matrix5::Ones();
static const Matrix6 O6 = Matrix6::Ones();
// Negative Ones
static const Matrix2 _O2 = -O2;
static const Matrix3 _O3 = -O3;
static const Matrix4 _O4 = -O4;
static const Matrix5 _O5 = -O5;
static const Matrix6 _O6 = -O6;
// Matlab-like syntax
/**

4
gtsam/geometry/Cal3_S2.h
View File

@ -173,8 +173,8 @@ public:
inline Cal3_S2 between(const Cal3_S2& q,
OptionalJacobian<5,5> H1=boost::none,
OptionalJacobian<5,5> H2=boost::none) const {
if(H1) *H1 = -Matrix5::Identity();
if(H2) *H2 = Matrix5::Identity();
if(H1) *H1 = _I5;
if(H2) *H2 = I5;
return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
}

2
gtsam/geometry/CalibratedCamera.cpp
View File

@ -36,7 +36,7 @@ Point2 CalibratedCamera::project_to_camera(const Point3& P,
OptionalJacobian<2,3> H1) {
if (H1) {
double d = 1.0 / P.z(), d2 = d * d;
(*H1) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
*H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
}
return Point2(P.x() / P.z(), P.y() / P.z());
}

4
gtsam/geometry/EssentialMatrix.cpp
View File

@ -27,9 +27,9 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
// First get 2*3 derivative from Unit3::FromPoint3
Matrix23 D_direction_1T2;
Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
H->block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); // upper left
H->block<3, 3>(0, 0) << I3; // upper left
H->block<2, 3>(3, 0).setZero(); // lower left
H->block<3, 3>(0, 3).setZero(); // upper right
H->block<3, 3>(0, 3) << Z3; // upper right
H->block<2, 3>(3, 3) = D_direction_1T2 * _1R2_.matrix(); // lower right
return EssentialMatrix(_1R2_, direction);
}

1
gtsam/geometry/Point2.cpp
View File

@ -50,7 +50,6 @@ double Point2::norm(OptionalJacobian<1,2> H) const {
}
/* ************************************************************************* */
static const Matrix2 I2 = Eigen::Matrix2d::Identity();
double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1,
OptionalJacobian<1,2> H2) const {
Point2 d = point - *this;

8
gtsam/geometry/Point2.h
View File

@ -127,8 +127,8 @@ public:
inline Point2 compose(const Point2& q,
OptionalJacobian<2,2> H1=boost::none,
OptionalJacobian<2,2> H2=boost::none) const {
if(H1) *H1 = Eigen::Matrix2d::Identity();
if(H2) *H2 = Eigen::Matrix2d::Identity();
if(H1) *H1 = I2;
if(H2) *H2 = I2;
return *this + q;
}
@ -139,8 +139,8 @@ public:
inline Point2 between(const Point2& q,
OptionalJacobian<2,2> H1=boost::none,
OptionalJacobian<2,2> H2=boost::none) const {
if(H1) *H1 = -Eigen::Matrix2d::Identity();
if(H2) *H2 = Eigen::Matrix2d::Identity();
if(H1) *H1 = _I2;
if(H2) *H2 = I2;
return q - (*this);
}

8
gtsam/geometry/Point3.cpp
View File

@ -66,9 +66,9 @@ Point3 Point3::operator/(double s) const {
Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1)
(*H1).setIdentity();
*H1= I3;
if (H2)
(*H2).setIdentity();
*H2= I3;
return *this + q;
}
@ -76,9 +76,9 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1)
(*H1).setIdentity();
(*H1) = I3;
if (H2)
(*H2).setIdentity();
(*H2) = I3;
return *this - q;
}

4
gtsam/geometry/Point3.h
View File

@ -143,12 +143,12 @@ namespace gtsam {
/// Left-trivialized derivative of the exponential map
static Matrix3 dexpL(const Vector& v) {
return Eigen::Matrix3d::Identity();
return I3;
}
/// Left-trivialized derivative inverse of the exponential map
static Matrix3 dexpInvL(const Vector& v) {
return Eigen::Matrix3d::Identity();
return I3;
}
/// @}

1
gtsam/geometry/Pose2.cpp
View File

@ -33,7 +33,6 @@ INSTANTIATE_LIE(Pose2);
/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose2);
static const Matrix3 I3 = Eigen::Matrix3d::Identity();
static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
/* ************************************************************************* */

12
gtsam/geometry/Pose3.cpp
View File

@ -32,8 +32,6 @@ INSTANTIATE_LIE(Pose3);
/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3);
static const Matrix3 I3 = Eigen::Matrix3d::Identity(), Z3 = Eigen::Matrix3d::Zero(), _I3 = -I3;
/* ************************************************************************* */
Pose3::Pose3(const Pose2& pose2) :
R_(Rot3::rodriguez(0, 0, pose2.theta())), t_(
@ -102,9 +100,9 @@ Matrix6 Pose3::dExpInv_exp(const Vector6& xi) {
0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished();
static const int N = 5; // order of approximation
Matrix6 res;
res.setIdentity();
res = I6;
Matrix6 ad_i;
ad_i.setIdentity();
ad_i = I6;
Matrix6 ad_xi = adjointMap(xi);
double fac = 1.0;
for (int i = 1; i < N; ++i) {
@ -279,7 +277,7 @@ Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1,
if (H1)
*H1 = p2.inverse().AdjointMap();
if (H2)
(*H2).setIdentity();
*H2 = I6;
return (*this) * p2;
}
@ -299,7 +297,7 @@ Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1,
if (H1)
*H1 = -result.inverse().AdjointMap();
if (H2)
(*H2).setIdentity();
(*H2) = I6;
return result;
}
@ -366,7 +364,7 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
// Recover transform with correction from Eggert97machinevisionandapplications
Matrix3 UVtranspose = U * V.transpose();
Matrix3 detWeighting = Eigen::Matrix3d::Identity();
Matrix3 detWeighting = I3;
detWeighting(2, 2) = UVtranspose.determinant();
Rot3 R(Matrix(V * detWeighting * U.transpose()));
Point3 t = Point3(cq) - R * Point3(cp);

8
gtsam/geometry/Rot2.h
View File

@ -118,8 +118,8 @@ namespace gtsam {
/** Compose - make a new rotation by adding angles */
inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) (*H1)<< 1; // set to 1x1 identity matrix
if (H2) (*H2)<< 1; // set to 1x1 identity matrix
if (H1) *H1 << I1; // set to 1x1 identity matrix
if (H2) *H2 << I1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
}
@ -131,8 +131,8 @@ namespace gtsam {
/** Between using the default implementation */
inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) *H1 << -1; // set to 1x1 identity matrix
if (H2) *H2 << 1; // set to 1x1 identity matrix
if (H1) *H1 << -I1; // set to 1x1 identity matrix
if (H2) *H2 << I1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
}

10
gtsam/geometry/Rot3.cpp
View File

@ -27,8 +27,6 @@ using namespace std;
namespace gtsam {
static const Matrix3 I3 = Matrix3::Identity();
/* ************************************************************************* */
void Rot3::print(const std::string& s) const {
gtsam::print((Matrix)matrix(), s);
@ -186,11 +184,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) {
double normx = norm_2(x); // rotation angle
Matrix3 Jr;
if (normx < 10e-8){
Jr = Matrix3::Identity();
Jr = I3;
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X +
Jr = I3 - ((1-cos(normx))/(normx*normx)) * X +
((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
}
return Jr;
@ -203,11 +201,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) {
Matrix3 Jrinv;
if (normx < 10e-8){
Jrinv = Matrix3::Identity();
Jrinv = I3;
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jrinv = Matrix3::Identity() +
Jrinv = I3 +
0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
}
return Jrinv;

4
gtsam/geometry/Rot3M.cpp
View File

@ -30,10 +30,8 @@ using namespace std;
namespace gtsam {
static const Matrix3 I3 = Matrix3::Identity();
/* ************************************************************************* */
Rot3::Rot3() : rot_(Matrix3::Identity()) {}
Rot3::Rot3() : rot_(I3) {}
/* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {

33
gtsam/geometry/Unit3.cpp
View File

@ -43,7 +43,7 @@ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
Unit3 direction(point);
if (H) {
// 3*3 Derivative of representation with respect to point is 3*3:
Matrix D_p_point;
Matrix3 D_p_point;
point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
// Calculate the 2*3 Jacobian
*H << direction.basis().transpose() * D_p_point;
@ -108,33 +108,34 @@ Matrix3 Unit3::skew() const {
}
/* ************************************************************************* */
Vector Unit3::error(const Unit3& q, boost::optional<Matrix&> H) const {
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
Matrix Bt = basis().transpose();
Vector xi = Bt * q.p_.vector();
Matrix23 Bt = basis().transpose();
Vector2 xi = Bt * q.p_.vector();
if (H)
*H = Bt * q.basis();
return xi;
}
/* ************************************************************************* */
double Unit3::distance(const Unit3& q, boost::optional<Matrix&> H) const {
Vector xi = error(q, H);
double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
Matrix2 H_;
Vector2 xi = error(q, H_);
double theta = xi.norm();
if (H)
*H = (xi.transpose() / theta) * (*H);
*H = (xi.transpose() / theta) * H_;
return theta;
}
/* ************************************************************************* */
Unit3 Unit3::retract(const Vector& v) const {
Unit3 Unit3::retract(const Vector2& v) const {
// Get the vector form of the point and the basis matrix
Vector p = Point3::Logmap(p_);
Matrix B = basis();
Vector3 p = Point3::Logmap(p_);
Matrix32 B = basis();
// Compute the 3D xi_hat vector
Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
double xi_hat_norm = xi_hat.norm();
@ -146,17 +147,17 @@ Unit3 Unit3::retract(const Vector& v) const {
return Unit3(-point3());
}
Vector exp_p_xi_hat = cos(xi_hat_norm) * p
Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p
+ sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
return Unit3(exp_p_xi_hat);
}
/* ************************************************************************* */
Vector Unit3::localCoordinates(const Unit3& y) const {
Vector2 Unit3::localCoordinates(const Unit3& y) const {
Vector p = Point3::Logmap(p_);
Vector q = Point3::Logmap(y.p_);
Vector3 p = Point3::Logmap(p_);
Vector3 q = Point3::Logmap(y.p_);
double dot = p.dot(q);
// Check for special cases
@ -167,7 +168,7 @@ Vector Unit3::localCoordinates(const Unit3& y) const {
else {
// no special case
double theta = acos(dot);
Vector result_hat = (theta / sin(theta)) * (q - p * dot);
Vector3 result_hat = (theta / sin(theta)) * (q - p * dot);
return basis().transpose() * result_hat;
}
}

15
gtsam/geometry/Unit3.h
View File

@ -50,6 +50,11 @@ public:
p_(p / p.norm()) {
}
/// Construct from a vector3
explicit Unit3(const Vector3& p) :
p_(p / p.norm()) {
}
/// Construct from x,y,z
Unit3(double x, double y, double z) :
p_(x, y, z) {
@ -103,12 +108,12 @@ public:
}
/// Signed, vector-valued error between two directions
Vector error(const Unit3& q,
boost::optional<Matrix&> H = boost::none) const;
Vector2 error(const Unit3& q,
OptionalJacobian<2,2> H = boost::none) const;
/// Distance between two directions
double distance(const Unit3& q,
boost::optional<Matrix&> H = boost::none) const;
OptionalJacobian<1,2> H = boost::none) const;
/// @}
@ -131,10 +136,10 @@ public:
};
/// The retract function
Unit3 retract(const Vector& v) const;
Unit3 retract(const Vector2& v) const;
/// The local coordinates function
Vector localCoordinates(const Unit3& s) const;
Vector2 localCoordinates(const Unit3& s) const;
/// @}

3
gtsam/geometry/tests/testRot3.cpp
View File

@ -37,7 +37,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3)
static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
static Point3 P(0.2, 0.7, -2.0);
static double error = 1e-9, epsilon = 0.001;
static const Matrix I3 = eye(3);
/* ************************************************************************* */
TEST( Rot3, chart)
@ -578,7 +577,7 @@ TEST(Rot3, quaternion) {
TEST( Rot3, Cayley ) {
Matrix A = skewSymmetric(1,2,-3);
Matrix Q = Cayley(A);
EXPECT(assert_equal(I3, trans(Q)*Q));
EXPECT(assert_equal((Matrix)I3, trans(Q)*Q));
EXPECT(assert_equal(A, Cayley(Q)));
}

1
gtsam/geometry/tests/testRot3M.cpp
View File

@ -37,7 +37,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3)
static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
static Point3 P(0.2, 0.7, -2.0);
static const Matrix I3 = eye(3);
/* ************************************************************************* */
TEST(Rot3, manifold_cayley)

6
gtsam/geometry/tests/testUnit3.cpp
View File

@ -108,9 +108,9 @@ TEST(Unit3, unrotate) {
TEST(Unit3, error) {
Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
r = p.retract(Vector2(0.8, 0));
EXPECT(assert_equal(Vector2(0, 0), p.error(p), 1e-8));
EXPECT(assert_equal(Vector2(0.479426, 0), p.error(q), 1e-5));
EXPECT(assert_equal(Vector2(0.717356, 0), p.error(r), 1e-5));
EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-8));
EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5));
EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5));
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian

3
gtsam_unstable/slam/AHRS.cpp
View File

@ -20,9 +20,6 @@ Matrix cov(const Matrix& m) {
return DDt / (num_observations - 1);
}
Matrix I3 = eye(3);
Matrix Z3 = zeros(3, 3);
/* ************************************************************************* */
AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
bool flat) :