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

Deprecated all inline functions in Matrix.h.

release/4.3a0
Alex Hagiopol 2016-04-11 15:11:29 -04:00
parent 1feed7c20e
commit 70b2aab352
97 changed files with 348 additions and 401 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -242,7 +242,7 @@ else()
endif()
# Add the bundled version of eigen to the include path so that it can still be included
# with #include <Eigen/Core>
include_directories(BEFORE "gtsam/3rdparty/Eigen/")
include_directories(BEFORE SYSTEM "gtsam/3rdparty/Eigen/")
# set full path to be used by external projects
# this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in

49
gtsam/base/Matrix.h
View File

@ -74,7 +74,7 @@ typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix;
/**
* equals with an tolerance
* equals with a tolerance
*/
template <class MATRIX>
bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBase<MATRIX>& B, double tol = 1e-9) {
@ -133,37 +133,12 @@ GTSAM_EXPORT bool linear_independent(const Matrix& A, const Matrix& B, double to
*/
GTSAM_EXPORT bool linear_dependent(const Matrix& A, const Matrix& B, double tol = 1e-9);
/**
* BLAS Level-2 style e <- e + alpha*A*x
*/
GTSAM_EXPORT void multiplyAdd(double alpha, const Matrix& A, const Vector& x, Vector& e);
/**
* BLAS Level-2 style e <- e + A*x
*/
GTSAM_EXPORT void multiplyAdd(const Matrix& A, const Vector& x, Vector& e);
/**
* overload ^ for trans(A)*v
* We transpose the vectors for speed.
*/
GTSAM_EXPORT Vector operator^(const Matrix& A, const Vector & v);
/**
* BLAS Level-2 style x <- x + alpha*A'*e
*/
GTSAM_EXPORT void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, Vector& x);
/**
* BLAS Level-2 style x <- x + A'*e
*/
GTSAM_EXPORT void transposeMultiplyAdd(const Matrix& A, const Vector& e, Vector& x);
/**
* BLAS Level-2 style x <- x + alpha*A'*e
*/
GTSAM_EXPORT void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVector x);
/** products using old-style format to improve compatibility */
template<class MATRIX>
inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
@ -248,17 +223,6 @@ const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) {
return A.row(j);
}
/**
* inserts a column into a matrix IN PLACE
* NOTE: there is no size checking
* Alternate form allows for vectors smaller than the whole column to be inserted
* @param A matrix to be modified in place
* @param col is the vector to be inserted
* @param j is the index to insert the column
*/
GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t j);
GTSAM_EXPORT void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j);
/**
* Zeros all of the elements below the diagonal of a matrix, in place
* @param A is a matrix, to be modified in place
@ -320,17 +284,6 @@ inline typename Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::ReshapedTy
return Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::reshape(m);
}
/**
* solve AX=B via in-place Lu factorization and backsubstitution
* After calling, A contains LU, B the solved RHS vectors
*/
GTSAM_EXPORT void solve(Matrix& A, Matrix& B);
/**
* invert A
*/
GTSAM_EXPORT Matrix inverse(const Matrix& A);
/**
* QR factorization, inefficient, best use imperative householder below
* m*n matrix -> m*m Q, m*n R

2
gtsam/base/numericalDerivative.h
View File

@ -142,7 +142,7 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::funct
dx.setZero();
// Fill in Jacobian H
Matrix H = zeros(m, N);
Matrix H = Matrix::Zero(m, N);
const double factor = 1.0 / (2.0 * delta);
for (int j = 0; j < N; j++) {
dx(j) = delta;

52
gtsam/base/tests/testMatrix.cpp
View File

@ -156,8 +156,8 @@ TEST(Matrix, collect2 )
TEST(Matrix, collect3 )
{
Matrix A, B;
A = eye(2, 3);
B = eye(2, 3);
A = Matrix::Identity(2,3);
B = Matrix::Identity(2,3);
vector<const Matrix*> matrices;
matrices.push_back(&A);
matrices.push_back(&B);
@ -214,11 +214,11 @@ TEST(Matrix, column )
/* ************************************************************************* */
TEST(Matrix, insert_column )
{
Matrix big = zeros(5, 6);
Matrix big = Matrix::Zero(5,6);
Vector col = ones(5);
size_t j = 3;
insertColumn(big, col, j);
big.col(j) = col;
Matrix expected = (Matrix(5, 6) <<
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
@ -233,15 +233,15 @@ TEST(Matrix, insert_column )
/* ************************************************************************* */
TEST(Matrix, insert_subcolumn )
{
Matrix big = zeros(5, 6);
Matrix big = Matrix::Zero(5,6);
Vector col1 = ones(2);
size_t i = 1;
size_t j = 3;
insertColumn(big, col1, i, j); // check 1
big.col(j).segment(i,col1.size()) = col1;
Vector col2 = ones(1);
insertColumn(big, col2, 4, 5); // check 2
big.col(5).segment(4,col2.size()) = col2;
Matrix expected = (Matrix(5, 6) <<
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
@ -283,7 +283,7 @@ TEST(Matrix, zeros )
A(1, 1) = 0;
A(1, 2) = 0;
Matrix zero = zeros(2, 3);
Matrix zero = Matrix::Zero(2,3);
EQUALITY(A , zero);
}
@ -291,7 +291,7 @@ TEST(Matrix, zeros )
/* ************************************************************************* */
TEST(Matrix, insert_sub )
{
Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0,
Matrix big = Matrix::Zero(5,6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0,
1.0).finished();
insertSub(big, small, 1, 2);
@ -307,9 +307,9 @@ TEST(Matrix, insert_sub )
TEST(Matrix, diagMatrices )
{
std::vector<Matrix> Hs;
Hs.push_back(ones(3,3));
Hs.push_back(ones(4,4)*2);
Hs.push_back(ones(2,2)*3);
Hs.push_back(Matrix::Ones(3,3));
Hs.push_back(Matrix::Ones(4,4)*2);
Hs.push_back(Matrix::Ones(2,2)*3);
Matrix actual = diag(Hs);
@ -723,9 +723,9 @@ TEST(Matrix, inverse )
A(2, 1) = 0;
A(2, 2) = 6;
Matrix Ainv = inverse(A);
EXPECT(assert_equal(eye(3), A*Ainv));
EXPECT(assert_equal(eye(3), Ainv*A));
Matrix Ainv = A.inverse();
EXPECT(assert_equal((Matrix) I_3x3, A*Ainv));
EXPECT(assert_equal((Matrix) I_3x3, Ainv*A));
Matrix expected(3, 3);
expected(0, 0) = 1.0909;
@ -746,13 +746,13 @@ TEST(Matrix, inverse )
0.0, -1.0, 1.0,
1.0, 0.0, 2.0,
0.0, 0.0, 1.0).finished(),
inverse(lMg)));
lMg.inverse()));
Matrix gMl((Matrix(3, 3) << 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0).finished());
EXPECT(assert_equal((Matrix(3, 3) <<
0.0, 1.0,-2.0,
-1.0, 0.0, 1.0,
0.0, 0.0, 1.0).finished(),
inverse(gMl)));
gMl.inverse()));
}
/* ************************************************************************* */
@ -769,7 +769,7 @@ TEST(Matrix, inverse2 )
A(2, 1) = 0;
A(2, 2) = 1;
Matrix Ainv = inverse(A);
Matrix Ainv = A.inverse();
Matrix expected(3, 3);
expected(0, 0) = 0;
@ -996,7 +996,7 @@ TEST(Matrix, inverse_square_root )
10.0).finished();
EQUALITY(expected,actual);
EQUALITY(measurement_covariance,inverse(actual*actual));
EQUALITY(measurement_covariance,(actual*actual).inverse());
// Randomly generated test. This test really requires inverse to
// be working well; if it's not, there's the possibility of a
@ -1058,8 +1058,7 @@ TEST(Matrix, multiplyAdd )
Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished();
Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.),
expected = e + A * x;
multiplyAdd(1, A, x, e);
e += A * x;
EXPECT(assert_equal(expected, e));
}
@ -1069,8 +1068,7 @@ TEST(Matrix, transposeMultiplyAdd )
Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished();
Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.),
expected = x + trans(A) * e;
transposeMultiplyAdd(1, A, e, x);
x += A.transpose() * e;
EXPECT(assert_equal(expected, x));
}
@ -1102,12 +1100,12 @@ TEST(Matrix, linear_dependent3 )
TEST(Matrix, svd1 )
{
Vector v = Vector3(2., 1., 0.);
Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1)
Matrix U1 = Matrix::Identity(4, 3), S1 = v.asDiagonal(), V1 = I_3x3, A = (U1 * S1)
* Matrix(trans(V1));
Matrix U, V;
Vector s;
svd(A, U, s, V);
Matrix S = diag(s);
Matrix S = s.asDiagonal();
EXPECT(assert_equal(U*S*Matrix(trans(V)),A));
EXPECT(assert_equal(S,S1));
}
@ -1158,7 +1156,7 @@ TEST(Matrix, svd3 )
V = -V;
}
Matrix S = diag(s);
Matrix S = s.asDiagonal();
Matrix t = U * S;
Matrix Vt = trans(V);
@ -1202,7 +1200,7 @@ TEST(Matrix, svd4 )
V.col(1) = -V.col(1);
}
Matrix reconstructed = U * diag(s) * trans(V);
Matrix reconstructed = U * s.asDiagonal() * trans(V);
EXPECT(assert_equal(A, reconstructed, 1e-4));
EXPECT(assert_equal(expectedU,U, 1e-3));

2
gtsam/base/tests/testSymmetricBlockMatrix.cpp
View File

@ -42,7 +42,7 @@ TEST(SymmetricBlockMatrix, ReadBlocks)
23, 29).finished();
Matrix actual1 = testBlockMatrix(1, 1);
// Test only writing the upper triangle for efficiency
Matrix actual1t = Matrix::Zero(2, 2);
Matrix actual1t = Z_2x2;
actual1t.triangularView<Eigen::Upper>() = testBlockMatrix(1, 1).triangularView();
EXPECT(assert_equal(expected1, actual1));
EXPECT(assert_equal(Matrix(expected1.triangularView<Eigen::Upper>()), actual1t));

2
gtsam/geometry/OrientedPlane3.cpp
View File

@ -42,7 +42,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
double pred_d = n_.unitVector().dot(xr.translation()) + d_;
if (Hr) {
*Hr = Matrix::Zero(3, 6);
*Hr = Matrix::Zero(3,6);
Hr->block<2, 3>(0, 0) = D_rotated_plane;
Hr->block<1, 3>(2, 3) = unit_vec;
}

2
gtsam/geometry/Pose2.cpp
View File

@ -132,7 +132,7 @@ Matrix3 Pose2::AdjointMap() const {
/* ************************************************************************* */
Matrix3 Pose2::adjointMap(const Vector3& v) {
// See Chirikjian12book2, vol.2, pg. 36
Matrix3 ad = Matrix::Zero(3,3);
Matrix3 ad = Z_3x3;
ad(0,1) = -v[2];
ad(1,0) = v[2];
ad(0,2) = v[1];

4
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -122,8 +122,8 @@ TEST(Cal3_S2, between) {
Matrix H1, H2;
EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2)));
EXPECT(assert_equal(-eye(5), H1));
EXPECT(assert_equal(eye(5), H2));
EXPECT(assert_equal(-I_5x5, H1));
EXPECT(assert_equal(I_5x5, H2));
}

2
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -116,7 +116,7 @@ TEST( PinholeCamera, lookat)
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
EXPECT(assert_equal(I, eye(3)));
EXPECT(assert_equal(I, I_3x3));
}
/* ************************************************************************* */

2
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -87,7 +87,7 @@ TEST( PinholePose, lookat)
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
EXPECT(assert_equal(I, eye(3)));
EXPECT(assert_equal(I, I_3x3));
}
/* ************************************************************************* */

8
gtsam/geometry/tests/testPoint2.cpp
View File

@ -73,12 +73,12 @@ TEST(Point2, Lie) {
Matrix H1, H2;
EXPECT(assert_equal(Point2(5,7), traits<Point2>::Compose(p1, p2, H1, H2)));
EXPECT(assert_equal(eye(2), H1));
EXPECT(assert_equal(eye(2), H2));
EXPECT(assert_equal(I_2x2, H1));
EXPECT(assert_equal(I_2x2, H2));
EXPECT(assert_equal(Point2(3,3), traits<Point2>::Between(p1, p2, H1, H2)));
EXPECT(assert_equal(-eye(2), H1));
EXPECT(assert_equal(eye(2), H2));
EXPECT(assert_equal(-I_2x2, H1));
EXPECT(assert_equal(I_2x2, H2));
EXPECT(assert_equal(Point2(5,7), traits<Point2>::Retract(p1, Vector2(4., 5.))));
EXPECT(assert_equal(Vector2(3.,3.), traits<Point2>::Local(p1,p2)));

8
gtsam/geometry/tests/testPoint3.cpp
View File

@ -47,12 +47,12 @@ TEST(Point3, Lie) {
Matrix H1, H2;
EXPECT(assert_equal(Point3(5, 7, 9), traits<Point3>::Compose(p1, p2, H1, H2)));
EXPECT(assert_equal(eye(3), H1));
EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(I_3x3, H1));
EXPECT(assert_equal(I_3x3, H2));
EXPECT(assert_equal(Point3(3, 3, 3), traits<Point3>::Between(p1, p2, H1, H2)));
EXPECT(assert_equal(-eye(3), H1));
EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(-I_3x3, H1));
EXPECT(assert_equal(I_3x3, H2));
EXPECT(assert_equal(Point3(5, 7, 9), traits<Point3>::Retract(p1, Vector3(4,5,6))));
EXPECT(assert_equal(Vector3(3, 3, 3), traits<Point3>::Local(p1,p2)));

4
gtsam/geometry/tests/testPose2.cpp
View File

@ -102,7 +102,7 @@ TEST(Pose2, expmap3) {
0.99, 0.0, -0.015,
0.0, 0.0, 0.0).finished();
Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0;
Matrix expected = eye(3) + A + A2 + A3 + A4;
Matrix expected = I_3x3 + A + A2 + A3 + A4;
Vector v = Vector3(0.01, -0.015, 0.99);
Pose2 pose = Pose2::Expmap(v);
@ -311,7 +311,7 @@ TEST(Pose2, compose_a)
-1.0, 0.0, 2.0,
0.0, 0.0, 1.0
).finished();
Matrix expectedH2 = eye(3);
Matrix expectedH2 = I_3x3;
Matrix numericalH1 = numericalDerivative21<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
Matrix numericalH2 = numericalDerivative22<Pose2, Pose2, Pose2>(testing::compose, pose1, pose2);
EXPECT(assert_equal(expectedH1,actualDcompose1));

8
gtsam/geometry/tests/testPose3.cpp
View File

@ -153,7 +153,7 @@ Pose3 Agrawal06iros(const Vector& xi) {
return Pose3(Rot3(), Point3(v));
else {
Matrix W = skewSymmetric(w/t);
Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
Matrix A = I_3x3 + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
return Pose3(Rot3::Expmap (w), Point3(A * v));
}
}
@ -267,7 +267,7 @@ TEST( Pose3, inverse)
{
Matrix actualDinverse;
Matrix actual = T.inverse(actualDinverse).matrix();
Matrix expected = inverse(T.matrix());
Matrix expected = T.matrix().inverse();
EXPECT(assert_equal(actual,expected,1e-8));
Matrix numericalH = numericalDerivative11(testing::inverse<Pose3>, T);
@ -293,7 +293,7 @@ TEST( Pose3, inverseDerivatives2)
TEST( Pose3, compose_inverse)
{
Matrix actual = (T*T.inverse()).matrix();
Matrix expected = eye(4,4);
Matrix expected = I_4x4;
EXPECT(assert_equal(actual,expected,1e-8));
}
@ -723,7 +723,7 @@ TEST( Pose3, adjointMap) {
Matrix res = Pose3::adjointMap(screwPose3::xi);
Matrix wh = skewSymmetric(screwPose3::xi(0), screwPose3::xi(1), screwPose3::xi(2));
Matrix vh = skewSymmetric(screwPose3::xi(3), screwPose3::xi(4), screwPose3::xi(5));
Matrix Z3 = zeros(3,3);
Matrix Z3 = Z_3x3;
Matrix6 expected;
expected << wh, Z3, vh, wh;
EXPECT(assert_equal(expected,res,1e-5));

8
gtsam/geometry/tests/testRot2.cpp
View File

@ -62,8 +62,8 @@ TEST( Rot2, compose)
Matrix H1, H2;
(void) Rot2::fromAngle(1.0).compose(Rot2::fromAngle(2.0), H1, H2);
EXPECT(assert_equal(eye(1), H1));
EXPECT(assert_equal(eye(1), H2));
EXPECT(assert_equal(I_1x1, H1));
EXPECT(assert_equal(I_1x1, H2));
}
/* ************************************************************************* */
@ -74,8 +74,8 @@ TEST( Rot2, between)
Matrix H1, H2;
(void) Rot2::fromAngle(1.0).between(Rot2::fromAngle(2.0), H1, H2);
EXPECT(assert_equal(-eye(1), H1));
EXPECT(assert_equal(eye(1), H2));
EXPECT(assert_equal(-I_1x1, H1));
EXPECT(assert_equal(I_1x1, H2));
}
/* ************************************************************************* */

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

@ -555,8 +555,8 @@ TEST(Rot3, quaternion) {
/* ************************************************************************* */
Matrix Cayley(const Matrix& A) {
Matrix::Index n = A.cols();
const Matrix I = eye(n);
return (I-A)*inverse(I+A);
const Matrix I = Matrix::Identity(n,n);
return (I-A)*(I+A).inverse();
}
TEST( Rot3, Cayley ) {

2
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -76,7 +76,7 @@ TEST( SimpleCamera, lookat)
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
CHECK(assert_equal(I, eye(3)));
CHECK(assert_equal(I, I_3x3));
}
/* ************************************************************************* */

2
gtsam/linear/GaussianConditional.cpp
View File

@ -183,7 +183,7 @@ namespace gtsam {
if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front());
for (const_iterator it = beginParents(); it!= endParents(); it++)
gtsam::transposeMultiplyAdd(-1.0, Matrix(getA(it)), frontalVec, gy[*it]);
gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec;
// Scale by sigmas
if(model_)

3
gtsam/linear/JacobianFactor.cpp
View File

@ -565,8 +565,7 @@ void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
pair<VectorValues::iterator, bool> xi = x.tryInsert(j, Vector());
if (xi.second)
xi.first->second = Vector::Zero(getDim(begin() + pos));
gtsam::transposeMultiplyAdd(Ab_(pos), E, xi.first->second);
xi.first->second += Ab_(pos).transpose()*E;
}
}

4
gtsam/linear/KalmanFilter.cpp
View File

@ -115,7 +115,7 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
// f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
// See documentation in HessianFactor, we have A1 = -F, A2 = I_, b = B*u:
// TODO: starts to seem more elaborate than straight-up KF equations?
Matrix M = inverse(Q), Ft = trans(F);
Matrix M = Q.inverse(), Ft = trans(F);
Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M;
Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
double f = dot(b, g2);
@ -147,7 +147,7 @@ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
const Vector& z, const Matrix& Q) const {
Key k = step(p);
Matrix M = inverse(Q), Ht = trans(H);
Matrix M = Q.inverse(), Ht = trans(H);
Matrix G = Ht * M * H;
Vector g = Ht * M * z;
double f = dot(z, M * z);

2
gtsam/linear/iterative.h
View File

@ -86,7 +86,7 @@ namespace gtsam {
/** x += alpha* A'*e */
void transposeMultiplyAdd(double alpha, const Vector& e, Vector& x) const {
gtsam::transposeMultiplyAdd(alpha, A(), e, x);
x += alpha * A().transpose() * e;
}
};

4
gtsam/linear/tests/testGaussianBayesTree.cpp
View File

@ -170,7 +170,7 @@ TEST(GaussianBayesTree, complicatedMarginal) {
LONGS_EQUAL(1, (long)actualJacobianQR.size());
LONGS_EQUAL(5, (long)actualJacobianQR.keys()[0]);
Matrix actualA = actualJacobianQR.getA(actualJacobianQR.begin());
Matrix actualCov = inverse(actualA.transpose() * actualA);
Matrix actualCov = (actualA.transpose() * actualA).inverse();
EXPECT(assert_equal(expectedCov, actualCov, 1e-1));
// Marginal on 6
@ -187,7 +187,7 @@ TEST(GaussianBayesTree, complicatedMarginal) {
LONGS_EQUAL(1, (long)actualJacobianQR.size());
LONGS_EQUAL(6, (long)actualJacobianQR.keys()[0]);
actualA = actualJacobianQR.getA(actualJacobianQR.begin());
actualCov = inverse(actualA.transpose() * actualA);
actualCov = (actualA.transpose() * actualA).inverse();
EXPECT(assert_equal(expectedCov, actualCov, 1e1));
}

20
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -47,10 +47,10 @@ TEST(GaussianFactorGraph, initialization) {
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
fg +=
JacobianFactor(0, 10*eye(2), -1.0*ones(2), unit2),
JacobianFactor(0, -10*eye(2),1, 10*eye(2), Vector2(2.0, -1.0), unit2),
JacobianFactor(0, -5*eye(2), 2, 5*eye(2), Vector2(0.0, 1.0), unit2),
JacobianFactor(1, -5*eye(2), 2, 5*eye(2), Vector2(-1.0, 1.5), unit2);
JacobianFactor(0, 10*I_2x2, -1.0*Matrix::Ones(2,2), unit2),
JacobianFactor(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2),
JacobianFactor(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2),
JacobianFactor(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2);
EXPECT_LONGS_EQUAL(4, (long)fg.size());
@ -166,13 +166,13 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() {
GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2);
fg += JacobianFactor(2, 10*I_2x2, -1.0*Matrix::Ones(2,2), unit2);
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg += JacobianFactor(0, 10*eye(2), 2, -10*eye(2), Vector2(2.0, -1.0), unit2);
fg += JacobianFactor(0, 10*I_2x2, 2, -10*I_2x2, Vector2(2.0, -1.0), unit2);
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg += JacobianFactor(1, 5*eye(2), 2, -5*eye(2), Vector2(0.0, 1.0), unit2);
fg += JacobianFactor(1, 5*I_2x2, 2, -5*I_2x2, Vector2(0.0, 1.0), unit2);
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector2(-1.0, 1.5), unit2);
fg += JacobianFactor(0, -5*I_2x2, 1, 5*I_2x2, Vector2(-1.0, 1.5), unit2);
return fg;
}
@ -280,8 +280,8 @@ TEST( GaussianFactorGraph, multiplyHessianAdd )
/* ************************************************************************* */
static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() {
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
gfg += HessianFactor(1, 2, 100*eye(2,2), zeros(2,2), Vector2(0.0, 1.0),
400*eye(2,2), Vector2(1.0, 1.0), 3.0);
gfg += HessianFactor(1, 2, 100*I_2x2, Z_2x2, Vector2(0.0, 1.0),
400*I_2x2, Vector2(1.0, 1.0), 3.0);
return gfg;
}

6
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -54,7 +54,7 @@ TEST(HessianFactor, emptyConstructor)
HessianFactor f;
DOUBLES_EQUAL(0.0, f.constantTerm(), 1e-9); // Constant term should be zero
EXPECT(assert_equal(Vector(), f.linearTerm())); // Linear term should be empty
EXPECT(assert_equal(zeros(1,1), f.info())); // Full matrix should be 1-by-1 zero matrix
EXPECT(assert_equal((Matrix) Z_1x1, f.info())); // Full matrix should be 1-by-1 zero matrix
DOUBLES_EQUAL(0.0, f.error(VectorValues()), 1e-9); // Should have zero error
}
@ -123,11 +123,11 @@ TEST(HessianFactor, Constructor1)
TEST(HessianFactor, Constructor1b)
{
Vector mu = Vector2(1.0,2.0);
Matrix Sigma = eye(2,2);
Matrix Sigma = I_2x2;
HessianFactor factor(0, mu, Sigma);
Matrix G = eye(2,2);
Matrix G = I_2x2;
Vector g = G*mu;
double f = dot(g,mu);

34
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -168,19 +168,19 @@ namespace simple_graph {
Key keyX(10), keyY(8), keyZ(12);
double sigma1 = 0.1;
Matrix A11 = Matrix::Identity(2, 2);
Matrix A11 = I_2x2;
Vector2 b1(2, -1);
JacobianFactor factor1(keyX, A11, b1, noiseModel::Isotropic::Sigma(2, sigma1));
double sigma2 = 0.5;
Matrix A21 = -2 * Matrix::Identity(2, 2);
Matrix A22 = 3 * Matrix::Identity(2, 2);
Matrix A21 = -2 * I_2x2;
Matrix A22 = 3 * I_2x2;
Vector2 b2(4, -5);
JacobianFactor factor2(keyX, A21, keyY, A22, b2, noiseModel::Isotropic::Sigma(2, sigma2));
double sigma3 = 1.0;
Matrix A32 = -4 * Matrix::Identity(2, 2);
Matrix A33 = 5 * Matrix::Identity(2, 2);
Matrix A32 = -4 * I_2x2;
Matrix A33 = 5 * I_2x2;
Vector2 b3(3, -6);
JacobianFactor factor3(keyY, A32, keyZ, A33, b3, noiseModel::Isotropic::Sigma(2, sigma3));
@ -193,8 +193,8 @@ TEST( JacobianFactor, construct_from_graph)
{
using namespace simple_graph;
Matrix A1(6,2); A1 << A11, A21, Matrix::Zero(2,2);
Matrix A2(6,2); A2 << Matrix::Zero(2,2), A22, A32;
Matrix A1(6,2); A1 << A11, A21, Z_2x2;
Matrix A2(6,2); A2 << Z_2x2, A22, A32;
Matrix A3(6,2); A3 << Matrix::Zero(4,2), A33;
Vector b(6); b << b1, b2, b3;
Vector sigmas(6); sigmas << sigma1, sigma1, sigma2, sigma2, sigma3, sigma3;
@ -268,9 +268,9 @@ TEST(JacobianFactor, matrices_NULL)
// hessianBlockDiagonal
map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
LONGS_EQUAL(3,actualBD.size());
EXPECT(assert_equal(1*eye(3),actualBD[5]));
EXPECT(assert_equal(4*eye(3),actualBD[10]));
EXPECT(assert_equal(9*eye(3),actualBD[15]));
EXPECT(assert_equal(1*I_3x3,actualBD[5]));
EXPECT(assert_equal(4*I_3x3,actualBD[10]));
EXPECT(assert_equal(9*I_3x3,actualBD[15]));
}
/* ************************************************************************* */
@ -314,9 +314,9 @@ TEST(JacobianFactor, matrices)
// hessianBlockDiagonal
map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
LONGS_EQUAL(3,actualBD.size());
EXPECT(assert_equal(4*eye(3),actualBD[5]));
EXPECT(assert_equal(16*eye(3),actualBD[10]));
EXPECT(assert_equal(36*eye(3),actualBD[15]));
EXPECT(assert_equal(4*I_3x3,actualBD[5]));
EXPECT(assert_equal(16*I_3x3,actualBD[10]));
EXPECT(assert_equal(36*I_3x3,actualBD[15]));
}
/* ************************************************************************* */
@ -324,7 +324,7 @@ TEST(JacobianFactor, operators )
{
SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1);
Matrix I = eye(2);
Matrix I = I_2x2;
Vector b = Vector2(0.2,-0.1);
JacobianFactor lf(1, -I, 2, I, b, sigma0_1);
@ -405,7 +405,7 @@ TEST(JacobianFactor, eliminate)
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Matrix zero3x3 = zeros(3,3);
Matrix zero3x3 = Matrix::Zero(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector9 b; b << b1, b0, b2;
@ -561,7 +561,7 @@ TEST ( JacobianFactor, constraint_eliminate1 )
{
// construct a linear constraint
Vector v(2); v(0)=1.2; v(1)=3.4;
JacobianFactor lc(1, eye(2), v, noiseModel::Constrained::All(2));
JacobianFactor lc(1, I_2x2, v, noiseModel::Constrained::All(2));
// eliminate it
pair<GaussianConditional::shared_ptr, JacobianFactor::shared_ptr>
@ -572,7 +572,7 @@ TEST ( JacobianFactor, constraint_eliminate1 )
// verify conditional Gaussian
Vector sigmas = Vector2(0.0, 0.0);
GaussianConditional expCG(1, v, eye(2), noiseModel::Diagonal::Sigmas(sigmas));
GaussianConditional expCG(1, v, I_2x2, noiseModel::Diagonal::Sigmas(sigmas));
EXPECT(assert_equal(expCG, *actual.first));
}

30
gtsam/linear/tests/testKalmanFilter.cpp
View File

@ -51,7 +51,7 @@ TEST( KalmanFilter, constructor ) {
EXPECT(assert_equal(x_initial, p1->mean()));
Matrix Sigma = (Matrix(2, 2) << 0.01, 0.0, 0.0, 0.01).finished();
EXPECT(assert_equal(Sigma, p1->covariance()));
EXPECT(assert_equal(inverse(Sigma), p1->information()));
EXPECT(assert_equal(Sigma.inverse(), p1->information()));
// Create one with a sharedGaussian
KalmanFilter::State p2 = kf1.init(x_initial, Sigma);
@ -65,33 +65,33 @@ TEST( KalmanFilter, constructor ) {
TEST( KalmanFilter, linear1 ) {
// Create the controls and measurement properties for our example
Matrix F = eye(2, 2);
Matrix B = eye(2, 2);
Matrix F = I_2x2;
Matrix B = I_2x2;
Vector u = Vector2(1.0, 0.0);
SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1);
Matrix Q = 0.01*eye(2, 2);
Matrix H = eye(2, 2);
Matrix Q = 0.01*I_2x2;
Matrix H = I_2x2;
State z1(1.0, 0.0);
State z2(2.0, 0.0);
State z3(3.0, 0.0);
SharedDiagonal modelR = noiseModel::Isotropic::Sigma(2, 0.1);
Matrix R = 0.01*eye(2, 2);
Matrix R = 0.01*I_2x2;
// Create the set of expected output TestValues
State expected0(0.0, 0.0);
Matrix P00 = 0.01*eye(2, 2);
Matrix P00 = 0.01*I_2x2;
State expected1(1.0, 0.0);
Matrix P01 = P00 + Q;
Matrix I11 = inverse(P01) + inverse(R);
Matrix I11 = P01.inverse() + R.inverse();
State expected2(2.0, 0.0);
Matrix P12 = inverse(I11) + Q;
Matrix I22 = inverse(P12) + inverse(R);
Matrix P12 = I11.inverse() + Q;
Matrix I22 = P12.inverse() + R.inverse();
State expected3(3.0, 0.0);
Matrix P23 = inverse(I22) + Q;
Matrix I33 = inverse(P23) + inverse(R);
Matrix P23 = I22.inverse() + Q;
Matrix I33 = P23.inverse() + R.inverse();
// Create a Kalman filter of dimension 2
KalmanFilter kf(2);
@ -140,7 +140,7 @@ TEST( KalmanFilter, predict ) {
Vector u = Vector3(1.0, 0.0, 2.0);
Matrix R = (Matrix(2, 2) << 1.0, 0.5, 0.0, 3.0).finished();
Matrix M = trans(R)*R;
Matrix Q = inverse(M);
Matrix Q = M.inverse();
// Create a Kalman filter of dimension 2
KalmanFilter kf(2);
@ -197,7 +197,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0).finished();
Matrix B = zeros(9, 1);
Matrix B = Matrix::Zero(9, 1);
Vector u = zero(1);
Matrix dt_Q_k = 1e-6 * (Matrix(9, 9) <<
33.7, 3.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
@ -270,7 +270,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
EXPECT(assert_equal(expected2, pb2->covariance(), 1e-7));
// do the above update again, this time with a full Matrix Q
Matrix modelQ = diag(sigmas.array().square());
Matrix modelQ = ((Matrix) sigmas.array().square()).asDiagonal();
KalmanFilter::State pa3 = kfa.updateQ(pa, H, z, modelQ);
KalmanFilter::State pb3 = kfb.updateQ(pb, H, z, modelQ);

16
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -112,7 +112,7 @@ TEST(NoiseModel, Unit)
TEST(NoiseModel, equals)
{
Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R),
g2 = Gaussian::SqrtInformation(eye(3,3));
g2 = Gaussian::SqrtInformation(I_3x3);
Diagonal::shared_ptr d1 = Diagonal::Sigmas(Vector3(kSigma, kSigma, kSigma)),
d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3));
Isotropic::shared_ptr i1 = Isotropic::Sigma(3, kSigma),
@ -395,7 +395,7 @@ TEST(NoiseModel, SmartSqrtInformation )
{
bool smart = true;
gtsam::SharedGaussian expected = Unit::Create(3);
gtsam::SharedGaussian actual = Gaussian::SqrtInformation(eye(3), smart);
gtsam::SharedGaussian actual = Gaussian::SqrtInformation(I_3x3, smart);
EXPECT(assert_equal(*expected,*actual));
}
@ -404,7 +404,7 @@ TEST(NoiseModel, SmartSqrtInformation2 )
{
bool smart = true;
gtsam::SharedGaussian expected = Unit::Isotropic::Sigma(3,2);
gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*eye(3), smart);
gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*I_3x3, smart);
EXPECT(assert_equal(*expected,*actual));
}
@ -413,7 +413,7 @@ TEST(NoiseModel, SmartInformation )
{
bool smart = true;
gtsam::SharedGaussian expected = Unit::Isotropic::Variance(3,2);
Matrix M = 0.5*eye(3);
Matrix M = 0.5*I_3x3;
EXPECT(checkIfDiagonal(M));
gtsam::SharedGaussian actual = Gaussian::Information(M, smart);
EXPECT(assert_equal(*expected,*actual));
@ -424,7 +424,7 @@ TEST(NoiseModel, SmartCovariance )
{
bool smart = true;
gtsam::SharedGaussian expected = Unit::Create(3);
gtsam::SharedGaussian actual = Gaussian::Covariance(eye(3), smart);
gtsam::SharedGaussian actual = Gaussian::Covariance(I_3x3, smart);
EXPECT(assert_equal(*expected,*actual));
}
@ -433,7 +433,7 @@ TEST(NoiseModel, ScalarOrVector )
{
bool smart = true;
SharedGaussian expected = Unit::Create(3);
SharedGaussian actual = Gaussian::Covariance(eye(3), smart);
SharedGaussian actual = Gaussian::Covariance(I_3x3, smart);
EXPECT(assert_equal(*expected,*actual));
}
@ -442,9 +442,9 @@ TEST(NoiseModel, WhitenInPlace)
{
Vector sigmas = Vector3(0.1, 0.1, 0.1);
SharedDiagonal model = Diagonal::Sigmas(sigmas);
Matrix A = eye(3);
Matrix A = I_3x3;
model->WhitenInPlace(A);
Matrix expected = eye(3) * 10;
Matrix expected = I_3x3 * 10;
EXPECT(assert_equal(expected, A));
}

8
gtsam/linear/tests/testSerializationLinear.cpp
View File

@ -51,7 +51,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* ************************************************************************* */
// example noise models
static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3));
static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * I_3x3);
static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector3(0.0, 0.0, 0.1));
static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
@ -144,7 +144,7 @@ TEST (Serialization, linear_factors) {
EXPECT(equalsBinary<VectorValues>(values));
Key i1 = 4, i2 = 7;
Matrix A1 = eye(3), A2 = -1.0 * eye(3);
Matrix A1 = I_3x3, A2 = -1.0 * I_3x3;
Vector b = ones(3);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0));
JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
@ -185,8 +185,8 @@ TEST (Serialization, gaussian_factor_graph) {
{
Key i1 = 4, i2 = 7;
Matrix A1 = eye(3), A2 = -1.0 * eye(3);
Vector b = ones(3);
Matrix A1 = I_3x3, A2 = -1.0 * I_3x3;
Vector b = Matrix::Ones(3,3);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0));
JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
HessianFactor hessianfactor(jacobianfactor);

2
gtsam/navigation/AttitudeFactor.h
View File

@ -189,7 +189,7 @@ public:
Vector e = attitudeError(nTb.rotation(), H);
if (H) {
Matrix H23 = *H;
*H = Matrix::Zero(2, 6);
*H = Matrix::Zero(2,6);
H->block<2,3>(0,0) = H23;
}
return e;

4
gtsam/navigation/MagFactor.h
View File

@ -154,7 +154,7 @@ public:
// measured bM = nRb<52> * nM + b, where b is unknown bias
Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
if (H2)
*H2 = eye(3);
*H2 = I_3x3;
return (hx - measured_);
}
};
@ -205,7 +205,7 @@ public:
*H2 = scale * H * (*H2);
}
if (H3)
*H3 = eye(3);
*H3 = I_3x3;
return (hx - measured_);
}
};

2
gtsam/navigation/PreintegrationBase.cpp
View File

@ -105,7 +105,7 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
// Update derivative: centrifugal causes the correlation between acc and omega!!!
if (correctedAcc_H_unbiasedOmega) {
double wdp = correctedOmega.dot(b_arm);
*correctedAcc_H_unbiasedOmega = -(diag(Vector3::Constant(wdp))
*correctedAcc_H_unbiasedOmega = -( (Matrix) Vector3::Constant(wdp).asDiagonal()
+ correctedOmega * b_arm.transpose()) * bRs.matrix()
+ 2 * b_arm * unbiasedOmega.transpose();
}

10
gtsam/nonlinear/NonlinearEquality.h
View File

@ -142,11 +142,11 @@ public:
const size_t nj = traits<T>::GetDimension(feasible_);
if (allow_error_) {
if (H)
*H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
*H = Matrix::Identity(nj,nj); // FIXME: this is not the right linearization for nonlinear compare
return traits<T>::Local(xj,feasible_);
} else if (compare_(feasible_, xj)) {
if (H)
*H = eye(nj);
*H = Matrix::Identity(nj,nj);
return zero(nj); // set error to zero if equal
} else {
if (H)
@ -249,7 +249,7 @@ public:
Vector evaluateError(const X& x1,
boost::optional<Matrix&> H = boost::none) const {
if (H)
(*H) = eye(traits<X>::GetDimension(x1));
(*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
// manifold equivalent of h(x)-z -> log(z,h(x))
return traits<X>::Local(value_,x1);
}
@ -322,8 +322,8 @@ public:
Vector evaluateError(const X& x1, const X& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
static const size_t p = traits<X>::dimension;
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p);
if (H1) *H1 = -Matrix::Identity(p,p);
if (H2) *H2 = Matrix::Identity(p,p);
return traits<X>::Local(x1,x2);
}

6
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -269,11 +269,11 @@ Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobi
OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
// return dummy derivatives (not correct, but that's ok for testing here)
if (H1)
*H1 = eye(3);
*H1 = I_3x3;
if (H2)
*H2 = eye(3);
*H2 = I_3x3;
if (H3)
*H3 = eye(3);
*H3 = I_3x3;
return R1 * (R2 * R3);
}

2
gtsam/nonlinear/tests/testLinearContainerFactor.cpp
View File

@ -234,7 +234,7 @@ TEST( testLinearContainerFactor, creation ) {
// create a linear factor
SharedDiagonal model = noiseModel::Unit::Create(2);
JacobianFactor::shared_ptr linear_factor(new JacobianFactor(
l3, eye(2,2), l5, 2.0 * eye(2,2), zero(2), model));
l3, I_2x2, l5, 2.0 * I_2x2, zero(2), model));
// create a set of values - build with full set of values
gtsam::Values full_values, exp_values;

6
gtsam/slam/GeneralSFMFactor.h
View File

@ -266,9 +266,9 @@ public:
return reprojError.vector();
}
catch( CheiralityException& e) {
if (H1) *H1 = zeros(2, 6);
if (H2) *H2 = zeros(2, 3);
if (H3) *H3 = zeros(2, DimK);
if (H1) *H1 = Matrix::Zero(2, 6);
if (H2) *H2 = Matrix::Zero(2, 3);
if (H3) *H3 = Matrix::Zero(2, DimK);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
}

11
gtsam/slam/InitializePose3.cpp
View File

@ -31,10 +31,9 @@ using namespace std;
namespace gtsam {
namespace InitializePose3 {
//static const Matrix I = eye(1);
static const Matrix I9 = Matrix::Identity(9,9);
static const Matrix I9 = I_9x9;
static const Vector zero9 = Vector::Zero(9);
static const Matrix zero33= Matrix::Zero(3,3);
static const Matrix zero33 = Z_3x3;
static const Key keyAnchor = symbol('Z', 9999999);
@ -54,11 +53,9 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
else
std::cout << "Error in buildLinearOrientationGraph" << std::endl;
// std::cout << "Rij \n" << Rij << std::endl;
const FastVector<Key>& keys = factor->keys();
Key key1 = keys[0], key2 = keys[1];
Matrix M9 = Matrix::Zero(9,9);
Matrix M9 = Z_9x9;
M9.block(0,0,3,3) = Rij;
M9.block(3,3,3,3) = Rij;
M9.block(6,6,3,3) = Rij;
@ -74,7 +71,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
gttic(InitializePose3_computeOrientationsChordal);
Matrix ppm = Matrix::Zero(3,3); // plus plus minus
Matrix ppm = Z_3x3; // plus plus minus
ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
Values validRot3;

2
gtsam/slam/JacobianFactorQ.h
View File

@ -56,7 +56,7 @@ public:
Base() {
size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
// Calculate projector Q
Matrix Q = eye(m2) - E * P * E.transpose();
Matrix Q = Matrix::Identity(m2,m2) - E * P * E.transpose();
// Calculate pre-computed Jacobian matrices
// TODO: can we do better ?
std::vector<KeyMatrix> QF;

2
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -47,7 +47,7 @@ Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane,
Vector e = n_hat_p.error(n_hat_q, H_p);
H->resize(2, 3);
H->block<2, 2>(0, 0) << H_p;
H->block<2, 1>(0, 2) << Matrix::Zero(2, 1);
H->block<2, 1>(0, 2) << Z_2x1;
return e;
} else {
Unit3 n_hat_p = measured_p_.normal();

2
gtsam/slam/PoseRotationPrior.h
View File

@ -75,7 +75,7 @@ public:
Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const {
const Rotation& newR = pose.rotation();
if (H) {
*H = gtsam::zeros(rDim, xDim);
*H = Matrix::Zero(rDim, xDim);
std::pair<size_t, size_t> rotInterval = POSE::rotationInterval();
(*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim);
}

2
gtsam/slam/PoseTranslationPrior.h
View File

@ -65,7 +65,7 @@ public:
const int tDim = traits<Translation>::GetDimension(newTrans);
const int xDim = traits<Pose>::GetDimension(pose);
if (H) {
*H = gtsam::zeros(tDim, xDim);
*H = Matrix::Zero(tDim, xDim);
std::pair<size_t, size_t> transInterval = POSE::translationInterval();
(*H).middleCols(transInterval.first, tDim) = R.matrix();
}

4
gtsam/slam/ProjectionFactor.h
View File

@ -146,8 +146,8 @@ namespace gtsam {
return reprojectionError.vector();
}
} catch( CheiralityException& e) {
if (H1) *H1 = zeros(2,6);
if (H2) *H2 = zeros(2,3);
if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;

2
gtsam/slam/ReferenceFrameFactor.h
View File

@ -100,7 +100,7 @@ public:
boost::optional<Matrix&> Dlocal = boost::none) const {
Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
if (Dlocal)
*Dlocal = -1* gtsam::eye(Point::dimension);
*Dlocal = -1* Matrix::Identity(Point::dimension,Point::dimension);
return traits<Point>::Local(local,newlocal);
}

2
gtsam/slam/SmartProjectionFactor.h
View File

@ -291,7 +291,7 @@ public:
if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
// failed: return"empty" Hessian
BOOST_FOREACH(Matrix& m, Gs)
m = zeros(Base::Dim, Base::Dim);
m = Matrix::Zero(Base::Dim, Base::Dim);
BOOST_FOREACH(Vector& v, gs)
v = zero(Base::Dim);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,

4
gtsam/slam/StereoFactor.h
View File

@ -138,8 +138,8 @@ public:
return (stereoCam.project(point, H1, H2) - measured_).vector();
}
} catch(StereoCheiralityException& e) {
if (H1) *H1 = zeros(3,6);
if (H2) *H2 = zeros(3,3);
if (H1) *H1 = Matrix::Zero(3,6);
if (H2) *H2 = Z_3x3;
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;

2
gtsam/slam/TriangulationFactor.h
View File

@ -124,7 +124,7 @@ public:
return error.vector();
} catch (CheiralityException& e) {
if (H2)
*H2 = zeros(Measurement::dimension, 3);
*H2 = Matrix::Zero(Measurement::dimension, 3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "
<< DefaultKeyFormatter(this->key()) << " moved behind camera"

8
gtsam/slam/dataset.cpp
View File

@ -472,7 +472,7 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
<< p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x()
<< " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w();
Matrix InfoG2o = Matrix::Identity(6,6);
Matrix InfoG2o = I_6x6;
InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation
InfoG2o.block(3,3,3,3) = Info.block(0,0,3,3); // cov rotation
InfoG2o.block(0,3,3,3) = Info.block(0,3,3,3); // off diagonal
@ -539,7 +539,7 @@ GraphAndValues load3D(const string& filename) {
ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
Rot3 R = Rot3::Ypr(yaw,pitch,roll);
Point3 t = Point3(x, y, z);
Matrix m = Matrix::Identity(6,6);
Matrix m = I_6x6;
for (int i = 0; i < 6; i++)
for (int j = i; j < 6; j++)
ls >> m(i, j);
@ -549,7 +549,7 @@ GraphAndValues load3D(const string& filename) {
graph->push_back(factor);
}
if (tag == "EDGE_SE3:QUAT") {
Matrix m = Matrix::Identity(6,6);
Matrix m = I_6x6;
Key id1, id2;
double x, y, z, qx, qy, qz, qw;
ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
@ -563,7 +563,7 @@ GraphAndValues load3D(const string& filename) {
m(j, i) = mij;
}
}
Matrix mgtsam = Matrix::Identity(6,6);
Matrix mgtsam = I_6x6;
mgtsam.block(0,0,3,3) = m.block(3,3,3,3); // cov rotation
mgtsam.block(3,3,3,3) = m.block(0,0,3,3); // cov translation
mgtsam.block(0,3,3,3) = m.block(0,3,3,3); // off diagonal

4
gtsam/slam/lago.cpp
View File

@ -30,8 +30,8 @@ using namespace std;
namespace gtsam {
namespace lago {
static const Matrix I = Matrix::Identity(1,1);
static const Matrix I3 = Matrix::Identity(3,3);
static const Matrix I = I_1x1;
static const Matrix I3 = I_3x3;
static const Key keyAnchor = symbol('Z', 9999999);
static const noiseModel::Diagonal::shared_ptr priorOrientationNoise =

18
gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
View File

@ -49,9 +49,9 @@ const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished();
//*************************************************************************************
TEST( regularImplicitSchurFactor, creation ) {
// Matrix E = Matrix::Ones(6,3);
Matrix E = zeros(6, 3);
E.block<2,2>(0, 0) = eye(2);
E.block<2,3>(2, 0) = 2 * ones(2, 3);
Matrix E = Matrix::Zero(6, 3);
E.block<2,2>(0, 0) = I_2x2;
E.block<2,3>(2, 0) = 2 * Matrix::Ones(2, 3);
Matrix3 P = (E.transpose() * E).inverse();
RegularImplicitSchurFactor<CalibratedCamera> expected(keys, FBlocks, E, P, b);
Matrix expectedP = expected.getPointCovariance();
@ -61,10 +61,10 @@ TEST( regularImplicitSchurFactor, creation ) {
/* ************************************************************************* */
TEST( regularImplicitSchurFactor, addHessianMultiply ) {
Matrix E = zeros(6, 3);
E.block<2,2>(0, 0) = eye(2);
E.block<2,3>(2, 0) = 2 * ones(2, 3);
E.block<2,2>(4, 1) = eye(2);
Matrix E = Matrix::Zero(6, 3);
E.block<2,2>(0, 0) = I_2x2;
E.block<2,3>(2, 0) = 2 * Matrix::Ones(2, 3);
E.block<2,2>(4, 1) = I_2x2;
Matrix3 P = (E.transpose() * E).inverse();
double alpha = 0.5;
@ -83,7 +83,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
// Create full F
size_t M=4, m = 3, d = 6;
Matrix F(2 * m, d * M);
F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3;
F << F0, Matrix::Zero(2, d * 3), Matrix::Zero(2, d), F1, Matrix::Zero(2, d*2), Matrix::Zero(2, d * 3), F3;
// Calculate expected result F'*alpha*(I - E*P*E')*F*x
FastVector<Key> keys2;
@ -246,7 +246,7 @@ TEST(regularImplicitSchurFactor, hessianDiagonal)
EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
// variant two
Matrix I2 = eye(2);
Matrix I2 = I_2x2;
Matrix E0 = E.block<2,3>(0, 0);
Matrix F0t = F0.transpose();
EXPECT(assert_equal(F0t*F0-F0t*E0*P*E0.transpose()*F0,actualBD[0]));

6
gtsam_unstable/dynamics/DynamicsPriors.h
View File

@ -57,7 +57,7 @@ struct VelocityPrior : public gtsam::PartialPriorFactor<PoseRTV> {
this->mask_[0] = 6;
this->mask_[1] = 7;
this->mask_[2] = 8;
this->H_ = zeros(3, 9);
this->H_ = Matrix::Zero(3, 9);
this->fillH();
}
};
@ -81,7 +81,7 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor<PoseRTV> {
this->mask_[1] = 8; // vz
this->mask_[2] = 0; // roll
this->mask_[3] = 1; // pitch
this->H_ = zeros(3, 9);
this->H_ = Matrix::Zero(3, 9);
this->fillH();
}
@ -97,7 +97,7 @@ struct DGroundConstraint : public gtsam::PartialPriorFactor<PoseRTV> {
this->mask_[1] = 8; // vz
this->mask_[2] = 0; // roll
this->mask_[3] = 1; // pitch
this->H_ = zeros(3, 9);
this->H_ = Matrix::Zero(3, 9);
this->fillH();
}
};

24
gtsam_unstable/dynamics/Pendulum.h
View File

@ -50,9 +50,9 @@ public:
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = 1;
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p);
if (H3) *H3 = eye(p)*h_;
if (H1) *H1 = -Matrix::Zero(p,p);
if (H2) *H2 = Matrix::Zero(p,p);
if (H3) *H3 = Matrix::Zero(p,p)*h_;
return (Vector(1) << qk+v*h_-qk1).finished();
}
@ -98,9 +98,9 @@ public:
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = 1;
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p);
if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q);
if (H1) *H1 = -Matrix::Zero(p,p);
if (H2) *H2 = Matrix::Zero(p,p);
if (H3) *H3 = -Matrix::Zero(p,p)*h_*g_/r_*cos(q);
return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1).finished();
}
@ -154,9 +154,9 @@ public:
double mr2_h = 1/h_*m_*r_*r_;
double mgrh = m_*g_*r_*h_;
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
if (H1) *H1 = -Matrix::Zero(p,p);
if (H2) *H2 = Matrix::Zero(p,p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
if (H3) *H3 = Matrix::Zero(p,p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk).finished();
}
@ -210,9 +210,9 @@ public:
double mr2_h = 1/h_*m_*r_*r_;
double mgrh = m_*g_*r_*h_;
if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
if (H1) *H1 = -Matrix::Zero(p,p);
if (H2) *H2 = Matrix::Zero(p,p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
if (H3) *H3 = Matrix::Zero(p,p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1).finished();
}

2
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -140,7 +140,7 @@ public:
}
if (H3) {
*H3 = zeros(6,6);
*H3 = Z_6x6;
insertSub(*H3, -h_*D_gravityBody_gk, 3, 0);
}

6
gtsam_unstable/dynamics/VelocityConstraint3.h
View File

@ -41,9 +41,9 @@ public:
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = 1;
if (H1) *H1 = eye(p);
if (H2) *H2 = -eye(p);
if (H3) *H3 = eye(p)*dt_;
if (H1) *H1 = Matrix::Zero(p,p);
if (H2) *H2 = -Matrix::Zero(p,p);
if (H3) *H3 = Matrix::Zero(p,p)*dt_;
return (Vector(1) << x1+v*dt_-x2).finished();
}

8
gtsam_unstable/dynamics/tests/testPoseRTV.cpp
View File

@ -227,15 +227,15 @@ TEST( testPoseRTV, transformed_from_2 ) {
/* ************************************************************************* */
TEST(testPoseRTV, RRTMbn) {
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(kZero3)));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3())));
EXPECT(assert_equal(I_3x3, PoseRTV::RRTMbn(kZero3)));
EXPECT(assert_equal(I_3x3, PoseRTV::RRTMbn(Rot3())));
EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::Ypr(0.1, 0.2, 0.3))));
}
/* ************************************************************************* */
TEST(testPoseRTV, RRTMnb) {
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(kZero3)));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3())));
EXPECT(assert_equal(I_3x3, PoseRTV::RRTMnb(kZero3)));
EXPECT(assert_equal(I_3x3, PoseRTV::RRTMnb(Rot3())));
EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::Ypr(0.1, 0.2, 0.3))));
}

4
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

@ -29,8 +29,8 @@ Vector gamma2 = Vector2(0.0, 0.0); // no shape
Vector u2 = Vector2(0.0, 0.0); // no control at time 2
double distT = 1.0; // distance from the body-centered x axis to the big top motor
double distR = 5.0; // distance from the body-centered z axis to the small motor
Matrix Mass = diag((Vector(3) << mass, mass, mass).finished());
Matrix Inertia = diag((Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass).finished());
Matrix Mass = ((Vector(3) << mass, mass, mass).finished()).asDiagonal();
Matrix Inertia = (Vector(6) << 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass).finished().asDiagonal();
Vector computeFu(const Vector& gamma, const Vector& control) {
double gamma_r = gamma(0), gamma_p = gamma(1);

2
gtsam_unstable/geometry/InvDepthCamera3.h
View File

@ -102,7 +102,7 @@ public:
gtsam::Matrix J2;
gtsam::Point2 uv= camera.project(landmark,H1, J2, boost::none);
if (H1) {
*H1 = (*H1) * gtsam::eye(6);
*H1 = (*H1) * I_6x6;
}
double cos_theta = cos(theta);

10
gtsam_unstable/geometry/Pose3Upright.cpp
View File

@ -81,7 +81,7 @@ Pose3 Pose3Upright::pose() const {
Pose3Upright Pose3Upright::inverse(boost::optional<Matrix&> H1) const {
Pose3Upright result(T_.inverse(H1), -z_);
if (H1) {
Matrix H1_ = -eye(4,4);
Matrix H1_ = -I_4x4;
H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
*H1 = H1_;
@ -96,12 +96,12 @@ Pose3Upright Pose3Upright::compose(const Pose3Upright& p2,
return Pose3Upright(T_.compose(p2.T_), z_ + p2.z_);
Pose3Upright result(T_.compose(p2.T_, H1), z_ + p2.z_);
if (H1) {
Matrix H1_ = eye(4,4);
Matrix H1_ = I_4x4;
H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
*H1 = H1_;
}
if (H2) *H2 = eye(4,4);
if (H2) *H2 = I_4x4;
return result;
}
@ -112,12 +112,12 @@ Pose3Upright Pose3Upright::between(const Pose3Upright& p2,
return Pose3Upright(T_.between(p2.T_), p2.z_ - z_);
Pose3Upright result(T_.between(p2.T_, H1, H2), p2.z_ - z_);
if (H1) {
Matrix H1_ = -eye(4,4);
Matrix H1_ = -I_4x4;
H1_.topLeftCorner(2,2) = H1->topLeftCorner(2,2);
H1_.topRightCorner(2, 1) = H1->topRightCorner(2, 1);
*H1 = H1_;
}
if (H2) *H2 = eye(4,4);
if (H2) *H2 = I_4x4;
return result;
}

2
gtsam_unstable/linear/tests/testLinearEquality.cpp
View File

@ -186,7 +186,7 @@ TEST(LinearEquality, matrices)
/* ************************************************************************* */
TEST(LinearEquality, operators )
{
Matrix I = eye(2);
Matrix I = I_2x2;
Vector b = (Vector(2) << 0.2,-0.1).finished();
LinearEquality lf(1, -I, 2, I, b, 0);

34
gtsam_unstable/linear/tests/testQPSolver.cpp
View File

@ -26,7 +26,7 @@ using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
const Matrix One = ones(1,1);
const Matrix One = Matrix::Ones(1,1);
/* ************************************************************************* */
// Create test graph according to Forst10book_pg171Ex5
@ -38,14 +38,14 @@ QP createTestCase() {
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10
qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1),
2.0 * ones(1, 1), zero(1), 10.0));
HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * ones(1),
2.0 * Matrix::Ones(1, 1), zero(1), 10.0));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
qp.inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0
qp.inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0
qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3)); // x1 <= 3/2
qp.inequalities.push_back(LinearInequality(X(1), Matrix::Ones(1,1), X(2), Matrix::Ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
qp.inequalities.push_back(LinearInequality(X(1), -Matrix::Ones(1,1), 0, 1)); // -x1 <= 0
qp.inequalities.push_back(LinearInequality(X(2), -Matrix::Ones(1,1), 0, 2)); // -x2 <= 0
qp.inequalities.push_back(LinearInequality(X(1), Matrix::Ones(1,1), 1.5, 3)); // x1 <= 3/2
return qp;
}
@ -53,8 +53,8 @@ QP createTestCase() {
TEST(QPSolver, TestCase) {
VectorValues values;
double x1 = 5, x2 = 7;
values.insert(X(1), x1 * ones(1, 1));
values.insert(X(2), x2 * ones(1, 1));
values.insert(X(1), x1 * Matrix::Ones(1, 1));
values.insert(X(2), x2 * Matrix::Ones(1, 1));
QP qp = createTestCase();
DOUBLES_EQUAL(29, x1 * x1 - x1 * x2 + x2 * x2 - 3 * x1 + 5, 1e-9);
DOUBLES_EQUAL(29, qp.cost[0]->error(values), 1e-9);
@ -92,8 +92,8 @@ QP createEqualityConstrainedTest() {
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0
qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * ones(1, 1), zeros(1, 1), zero(1),
2.0 * ones(1, 1), zero(1), 0.0));
HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), Z_1x1, zero(1),
2.0 * Matrix::Ones(1, 1), zero(1), 0.0));
// Equality constraints
// x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector
@ -219,8 +219,8 @@ QP createTestMatlabQPEx() {
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0
qp.cost.push_back(
HessianFactor(X(1), X(2), 1.0 * ones(1, 1), -ones(1, 1), 2.0 * ones(1),
2.0 * ones(1, 1), 6 * ones(1), 1000.0));
HessianFactor(X(1), X(2), 1.0 * Matrix::Ones(1,1), -Matrix::Ones(1, 1), 2.0 * ones(1),
2.0 * Matrix::Ones(1, 1), 6 * ones(1), 1000.0));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2
@ -251,8 +251,8 @@ TEST(QPSolver, optimizeMatlabEx) {
QP createTestNocedal06bookEx16_4() {
QP qp;
qp.cost.push_back(JacobianFactor(X(1), ones(1, 1), ones(1)));
qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1)));
qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), ones(1)));
qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * ones(1)));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0));
@ -283,8 +283,8 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) {
TEST(QPSolver, failedSubproblem) {
QP qp;
qp.cost.push_back(JacobianFactor(X(1), eye(2), zero(2)));
qp.cost.push_back(HessianFactor(X(1), zeros(2, 2), zero(2), 100.0));
qp.cost.push_back(JacobianFactor(X(1), I_2x2, zero(2)));
qp.cost.push_back(HessianFactor(X(1), Z_2x2, zero(2), 100.0));
qp.inequalities.push_back(
LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0).finished(), -1.0, 0));

22
gtsam_unstable/nonlinear/tests/testParticleFactor.cpp
View File

@ -126,33 +126,33 @@ TEST( ParticleFilter, constructor) {
TEST( ParticleFilter, linear1 ) {
// Create the controls and measurement properties for our example
Matrix F = eye(2, 2);
Matrix B = eye(2, 2);
Matrix F = I_2x2;
Matrix B = I_2x2;
Vector u = Vector2(1.0, 0.0);
SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1);
Matrix Q = 0.01 * eye(2, 2);
Matrix H = eye(2, 2);
Matrix Q = 0.01 * I_2x2;
Matrix H = I_2x2;
State z1(1.0, 0.0);
State z2(2.0, 0.0);
State z3(3.0, 0.0);
SharedDiagonal modelR = noiseModel::Isotropic::Sigma(2, 0.1);
Matrix R = 0.01 * eye(2, 2);
Matrix R = 0.01 * I_2x2;
// Create the set of expected output TestValues
State expected0(0.0, 0.0);
Matrix P00 = 0.01 * eye(2, 2);
Matrix P00 = 0.01 * I_2x2;
State expected1(1.0, 0.0);
Matrix P01 = P00 + Q;
Matrix I11 = inverse(P01) + inverse(R);
Matrix I11 = P01.inverse() + R.inverse();
State expected2(2.0, 0.0);
Matrix P12 = inverse(I11) + Q;
Matrix I22 = inverse(P12) + inverse(R);
Matrix P12 = I11.inverse() + Q;
Matrix I22 = P12.inverse() + R.inverse();
State expected3(3.0, 0.0);
Matrix P23 = inverse(I22) + Q;
Matrix I33 = inverse(P23) + inverse(R);
Matrix P23 = I22.inverse() + Q;
Matrix I33 = P23.inverse() + R.inverse();
// Create a Kalman filter of dimension 2
KalmanFilter kf(2);

12
gtsam_unstable/slam/AHRS.cpp
View File

@ -83,7 +83,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::initialize(double g_e)
0.0, 0.0, 0.0).finished(); // we don't know anything on yaw
// Calculate the initial covariance matrix for the error state dx, Farrell08book eq. 10.66
Matrix Pa = 0.025 * 0.025 * eye(3);
Matrix Pa = 0.025 * 0.025 * I_3x3;
Matrix P11 = Omega_T * (H_g * (Pa + Pa_) * trans(H_g)) * trans(Omega_T);
P11(2, 2) = 0.0001;
Matrix P12 = -Omega_T * H_g * Pa;
@ -171,7 +171,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
// calculate residual gravity measurement
z = n_g_ - trans(bRn) * measured_b_g;
H = collect(3, &n_g_cross_, &Z_3x3, &bRn);
R = trans(bRn) * diag(sigmas_v_a_.array().square()) * bRn;
R = trans(bRn) * ((Vector3) sigmas_v_a_.array().square()).asDiagonal() * bRn;
} else {
// my measurement prediction (in body frame):
// F(:,k) = bias - b_g
@ -186,7 +186,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
Matrix b_g = bRn * n_g_cross_;
H = collect(3, &b_g, &Z_3x3, &I_3x3);
// And the measurement noise, TODO: should be created once where sigmas_v_a is given
R = diag(sigmas_v_a_.array().square());
R = ((Vector3) sigmas_v_a_.array().square()).asDiagonal();
}
// update the Kalman filter
@ -196,7 +196,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
Mechanization_bRn2 newMech = mech.correct(updatedState->mean());
// reset KF state
Vector dx = zeros(9, 1);
Vector dx = Z_9x1;
updatedState = KF_.init(dx, updatedState->covariance());
return make_pair(newMech, updatedState);
@ -217,7 +217,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aidGeneral(
Matrix H = collect(3, &b_g, &I_3x3, &Z_3x3);
// Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_));
// Matrix R = diag(Vector3(1.0, 0.2, 1.0)); // good for L_twice
Matrix R = diag(Vector3(0.01, 0.0001, 0.01));
Matrix R = Vector3(0.01, 0.0001, 0.01).asDiagonal();
// update the Kalman filter
KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R);
@ -226,7 +226,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aidGeneral(
Mechanization_bRn2 newMech = mech.correct(updatedState->mean());
// reset KF state
Vector dx = zeros(9, 1);
Vector dx = Z_9x1;
updatedState = KF_.init(dx, updatedState->covariance());
return make_pair(newMech, updatedState);

2
gtsam_unstable/slam/DummyFactor.cpp
View File

@ -50,7 +50,7 @@ DummyFactor::linearize(const Values& c) const {
std::vector<std::pair<Key, Matrix> > terms(this->size());
for(size_t j=0; j<this->size(); ++j) {
terms[j].first = keys()[j];
terms[j].second = zeros(rowDim_, dims_[j]);
terms[j].second = Matrix::Zero(rowDim_, dims_[j]);
}
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(rowDim_);

18
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -434,8 +434,8 @@ public:
delta_t += msr_dt;
// Update EquivCov_Overall
Matrix Z_3x3 = zeros(3,3);
Matrix I_3x3 = eye(3,3);
Matrix Z_3x3 = Z_3x3;
Matrix I_3x3 = I_3x3;
Matrix H_pos_pos = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
Matrix H_pos_vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
@ -461,7 +461,7 @@ public:
noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt );
Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() );
Matrix Q_d = (model_discrete_curr->R().transpose() * model_discrete_curr->R()).inverse();
EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d;
// Luca: force identity covariance matrix (for testing purposes)
@ -536,9 +536,9 @@ public:
static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
const noiseModel::Gaussian::shared_ptr& gaussian_process){
Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() );
Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() );
Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
Matrix cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse();
Matrix cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse();
Matrix cov_process = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse();
cov_process.block(0,0, 3,3) += cov_gyro;
cov_process.block(6,6, 3,3) += cov_acc;
@ -550,9 +550,9 @@ public:
const noiseModel::Gaussian::shared_ptr& gaussian_process,
Matrix& cov_acc, Matrix& cov_gyro, Matrix& cov_process_without_acc_gyro){
cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() );
cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() );
cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse();
cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse();
cov_process_without_acc_gyro = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse();
}
static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial,

4
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -367,8 +367,8 @@ public:
delta_t += msr_dt;
// Update EquivCov_Overall
Matrix Z_3x3 = zeros(3,3);
Matrix I_3x3 = eye(3,3);
Matrix Z_3x3 = Z_3x3;
Matrix I_3x3 = I_3x3;
Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);

4
gtsam_unstable/slam/GaussMarkov1stOrderFactor.h
View File

@ -103,8 +103,8 @@ public:
Vector hx(v2 - alpha_v1);
if(H1) *H1 = - diag(alpha);
if(H2) *H2 = eye(v2.size());
if(H1) *H1 = -1 * alpha.asDiagonal();
if(H2) *H2 = Matrix::Identity(v2.size(),v2.size());
return hx;
}

6
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -278,9 +278,9 @@ public:
static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
const noiseModel::Gaussian::shared_ptr& gaussian_process){
Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() );
Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() );
Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
Matrix cov_acc = ( gaussian_acc->R().transpose() * gaussian_acc->R() ).inverse();
Matrix cov_gyro = ( gaussian_gyro->R().transpose() * gaussian_gyro->R() ).inverse();
Matrix cov_process = ( gaussian_process->R().transpose() * gaussian_process->R() ).inverse();
cov_process.block(0,0, 3,3) += cov_gyro;
cov_process.block(6,6, 3,3) += cov_acc;

6
gtsam_unstable/slam/InvDepthFactor3.h
View File

@ -89,9 +89,9 @@ public:
gtsam::Point2 reprojectionError(camera.project(point, invDepth, H1, H2, H3) - measured_);
return reprojectionError.vector();
} catch( CheiralityException& e) {
if (H1) *H1 = gtsam::zeros(2,6);
if (H2) *H2 = gtsam::zeros(2,5);
if (H3) *H2 = gtsam::zeros(2,1);
if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,5);
if (H3) *H2 = Matrix::Zero(2,1);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
return gtsam::ones(2) * 2.0 * K_->fx();

4
gtsam_unstable/slam/MultiProjectionFactor.h
View File

@ -184,8 +184,8 @@ namespace gtsam {
return reprojectionError.vector();
}
} catch( CheiralityException& e) {
if (H1) *H1 = zeros(2,6);
if (H2) *H2 = zeros(2,3);
if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;

4
gtsam_unstable/slam/PartialPriorFactor.h
View File

@ -70,7 +70,7 @@ namespace gtsam {
/** Single Element Constructor: acts on a single parameter specified by idx */
PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) :
Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(zeros(1, T::dimension)) {
Base(model, key), prior_((Vector(1) << prior).finished()), mask_(1, idx), H_(Matrix::Zero(1, T::dimension)) {
assert(model->dim() == 1);
this->fillH();
}
@ -78,7 +78,7 @@ namespace gtsam {
/** Indices Constructor: specify the mask with a set of indices */
PartialPriorFactor(Key key, const std::vector<size_t>& mask, const Vector& prior,
const SharedNoiseModel& model) :
Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::dimension)) {
Base(model, key), prior_(prior), mask_(mask), H_(Matrix::Zero(mask.size(), T::dimension)) {
assert((size_t)prior_.size() == mask.size());
assert(model->dim() == (size_t) prior.size());
this->fillH();

6
gtsam_unstable/slam/ProjectionFactorPPP.h
View File

@ -137,9 +137,9 @@ namespace gtsam {
return reprojectionError.vector();
}
} catch( CheiralityException& e) {
if (H1) *H1 = zeros(2,6);
if (H2) *H2 = zeros(2,6);
if (H3) *H3 = zeros(2,3);
if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,6);
if (H3) *H3 = Matrix::Zero(2,3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;

8
gtsam_unstable/slam/ProjectionFactorPPPC.h
View File

@ -132,10 +132,10 @@ namespace gtsam {
return reprojectionError.vector();
}
} catch( CheiralityException& e) {
if (H1) *H1 = zeros(2,6);
if (H2) *H2 = zeros(2,6);
if (H3) *H3 = zeros(2,3);
if (H4) *H4 = zeros(2,CALIBRATION::Dim());
if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,6);
if (H3) *H3 = Matrix::Zero(2,3);
if (H4) *H4 = Matrix::Zero(2,CALIBRATION::Dim());
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;

4
gtsam_unstable/slam/RelativeElevationFactor.cpp
View File

@ -21,7 +21,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
double hx = pose.z() - point.z();
if (H1) {
*H1 = zeros(1, 6);
*H1 = Matrix::Zero(1,6);
// Use bottom row of rotation matrix for derivative of translation
(*H1)(0, 3) = pose.rotation().r1().z();
(*H1)(0, 4) = pose.rotation().r2().z();
@ -29,7 +29,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p
}
if (H2) {
*H2 = zeros(1, 3);
*H2 = Matrix::Zero(1,3);
(*H2)(0, 2) = -1.0;
}
return (Vector(1) << hx - measured_).finished();

2
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -141,7 +141,7 @@ public:
if (H)
// set Jacobians to zero for n<3
for (size_t j = 0; j < n; j++)
(*H)[j] = zeros(3, 1);
(*H)[j] = Matrix::Zero(3, 1);
return zero(1);
} else {
Vector error = zero(1);

2
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -340,7 +340,7 @@ public:
if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
// failed: return"empty" Hessian
BOOST_FOREACH(Matrix& m, Gs)
m = zeros(Base::Dim, Base::Dim);
m = Matrix::Zero(Base::Dim, Base::Dim);
BOOST_FOREACH(Vector& v, gs)
v = zero(Base::Dim);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,

2
gtsam_unstable/slam/tests/testDummyFactor.cpp
View File

@ -47,7 +47,7 @@ TEST( testDummyFactor, basics ) {
CHECK(actLinearization);
noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3);
GaussianFactor::shared_ptr expLinearization(new JacobianFactor(
key1, zeros(3,3), key2, zeros(3,3), zero(3), model3));
key1, Matrix::Zero(3,3), key2, Matrix::Zero(3,3), zero(3), model3));
EXPECT(assert_equal(*expLinearization, *actLinearization, tol));
}

4
gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp
View File

@ -41,8 +41,8 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor)
Vector delta_vel_in_t0 = Vector3(0.0, 0.0, 0.0);
Vector delta_angles = Vector3(0.0, 0.0, 0.0);
double delta_t = 0.0;
Matrix EquivCov_Overall = zeros(15,15);
Matrix Jacobian_wrt_t0_Overall = eye(15);
Matrix EquivCov_Overall = Matrix::Zero(15,15);
Matrix Jacobian_wrt_t0_Overall = Matrix::Identity(15,15);
imuBias::ConstantBias bias1 = imuBias::ConstantBias();
// Earth Terms (gravity, etc)

2
gtsam_unstable/slam/tests/testSmartRangeFactor.cpp
View File

@ -70,7 +70,7 @@ TEST( SmartRangeFactor, unwhitenedError ) {
// Check Jacobian for n==1
vector<Matrix> H1(1);
f.unwhitenedError(values, H1); // with H now !
CHECK(assert_equal(zeros(3,1), H1.front()));
CHECK(assert_equal(Matrix::Zero(3,1), H1.front()));
// Whenever there are two ranges or less, error should be zero
Vector actual1 = f.unwhitenedError(values);

4
gtsam_unstable/testing_tools/base/cholScalingTest.m
View File

@ -19,8 +19,8 @@ badscale = 1e-8;
Acoeffs = [
1 11 badscale
(1:10)' (1:10)' -ones(10,1)
(1:10)' (2:11)' ones(10,1)
(1:10)' (1:10)' -Matrix::Ones(10,1)
(1:10)' (2:11)' Matrix::Ones(10,1)
]';
A = full(sparse(Acoeffs(1,:), Acoeffs(2,:), Acoeffs(3,:)));
b = zeros(size(A,1), 1);

2
matlab/gtsam_examples/IMUKittiExampleGPS.m
View File

@ -38,7 +38,7 @@ currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_x = noiseModel.Isotropic.Precisions([ 0.0; 0.0; 0.0; 1; 1; 1 ]);
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0);
sigma_init_b = noiseModel.Isotropic.Sigmas([ 0.100; 0.100; 0.100; 5.00e-05; 5.00e-05; 5.00e-05 ]);
sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadata.GyroscopeBiasSigma * ones(3,1) ];
sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * Matrix::Ones(3,1); IMU_metadata.GyroscopeBiasSigma * Matrix::Ones(3,1) ];
g = [0;0;-9.8];
w_coriolis = [0;0;0];

2
matlab/gtsam_examples/SBAExample.m
View File

@ -29,7 +29,7 @@ options.showImages = false;
measurementNoiseSigma = 1.0;
pointNoiseSigma = 0.1;
cameraNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1 ...
0.001*ones(1,5)]';
0.001*Matrix::Ones(1,5)]';
%% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph)
graph = NonlinearFactorGraph;

2
matlab/unstable_examples/FlightCameraTransformIMU.m
View File

@ -62,7 +62,7 @@ currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0);
sigma_init_b = noiseModel.Isotropic.Sigmas([ 0.100; 0.100; 0.100; 5.00e-05; 5.00e-05; 5.00e-05 ]);
sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadata.GyroscopeBiasSigma * ones(3,1) ];
sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * Matrix::Ones(3,1); IMU_metadata.GyroscopeBiasSigma * Matrix::Ones(3,1) ];
g = [0;0;-9.8];
w_coriolis = [0;0;0];

2
matlab/unstable_examples/IMUKittiExampleAdvanced.m
View File

@ -155,7 +155,7 @@ for measurementIndex = 1:length(timestamps)
%% Create GPS factor
if type == 2
newFactors.add(PriorFactorPose3(currentPoseKey, Pose3(currentPoseGlobal.rotation, GPS_data(measurementIndex).Position), ...
noiseModel.Diagonal.Precisions([ zeros(3,1); 1./(GPS_data(measurementIndex).PositionSigma).^2*ones(3,1) ])));
noiseModel.Diagonal.Precisions([ zeros(3,1); 1./(GPS_data(measurementIndex).PositionSigma).^2*Matrix::Ones(3,1) ])));
end
%% Create VO factor

2
matlab/unstable_examples/IMUKittiExampleVO.m
View File

@ -51,7 +51,7 @@ currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_x = noiseModel.Isotropic.Sigmas([ 1.0; 1.0; 0.01; 0.01; 0.01; 0.01 ]);
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0);
sigma_init_b = noiseModel.Isotropic.Sigmas([ 0.100; 0.100; 0.100; 5.00e-05; 5.00e-05; 5.00e-05 ]);
sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadata.GyroscopeBiasSigma * ones(3,1) ];
sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * Matrix::Ones(3,1); IMU_metadata.GyroscopeBiasSigma * Matrix::Ones(3,1) ];
g = [0;0;-9.8];
w_coriolis = [0;0;0];

2
matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m
View File

@ -94,7 +94,7 @@ currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0);
sigma_init_b = noiseModel.Isotropic.Sigmas([ 0.100; 0.100; 0.100; 5.00e-05; 5.00e-05; 5.00e-05 ]);
sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadata.GyroscopeBiasSigma * ones(3,1) ];
sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * Matrix::Ones(3,1); IMU_metadata.GyroscopeBiasSigma * Matrix::Ones(3,1) ];
g = [0;0;-9.8];
w_coriolis = [0;0;0];

10
tests/simulated2D.h
View File

@ -90,7 +90,7 @@ namespace simulated2D {
/// Prior on a single pose, optionally returns derivative
inline Point2 prior(const Point2& x, boost::optional<Matrix&> H = boost::none) {
if (H) *H = gtsam::eye(2);
if (H) *H = I_2x2;
return x;
}
@ -102,8 +102,8 @@ namespace simulated2D {
/// odometry between two poses, optionally returns derivative
inline Point2 odo(const Point2& x1, const Point2& x2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) {
if (H1) *H1 = -gtsam::eye(2);
if (H2) *H2 = gtsam::eye(2);
if (H1) *H1 = -I_2x2;
if (H2) *H2 = I_2x2;
return x2 - x1;
}
@ -115,8 +115,8 @@ namespace simulated2D {
/// measurement between landmark and pose, optionally returns derivative
inline Point2 mea(const Point2& x, const Point2& l, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) {
if (H1) *H1 = -gtsam::eye(2);
if (H2) *H2 = gtsam::eye(2);
if (H1) *H1 = -I_2x2;
if (H2) *H2 = I_2x2;
return l - x;
}

2
tests/simulated2DConstraints.h
View File

@ -90,7 +90,7 @@ namespace simulated2D {
virtual double value(const Point& x, boost::optional<Matrix&> H =
boost::none) const {
if (H) {
Matrix D = zeros(1, traits<Point>::GetDimension(x));
Matrix D = Matrix::Zero(1, traits<Point>::GetDimension(x));
D(0, IDX) = 1.0;
*H = D;
}

2
tests/simulated2DOriented.h
View File

@ -63,7 +63,7 @@ namespace simulated2DOriented {
/// Prior on a single pose, optional derivative version
Pose2 prior(const Pose2& x, boost::optional<Matrix&> H = boost::none) {
if (H) *H = gtsam::eye(3);
if (H) *H = I_3x3;
return x;
}

10
tests/simulated3D.h
View File

@ -39,7 +39,7 @@ namespace simulated3D {
* Prior on a single pose
*/
Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none) {
if (H) *H = eye(3);
if (H) *H = I_3x3;
return x;
}
@ -49,8 +49,8 @@ Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none) {
Point3 odo(const Point3& x1, const Point3& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) {
if (H1) *H1 = -1 * eye(3);
if (H2) *H2 = eye(3);
if (H1) *H1 = -1 * I_3x3;
if (H2) *H2 = I_3x3;
return x2 - x1;
}
@ -60,8 +60,8 @@ Point3 odo(const Point3& x1, const Point3& x2,
Point3 mea(const Point3& x, const Point3& l,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) {
if (H1) *H1 = -1 * eye(3);
if (H2) *H2 = eye(3);
if (H1) *H1 = -1 * I_3x3;
if (H2) *H2 = I_3x3;
return l - x;
}

20
tests/smallExample.h
View File

@ -274,16 +274,16 @@ inline GaussianFactorGraph createGaussianFactorGraph() {
GaussianFactorGraph fg;
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
fg += JacobianFactor(X(1), 10*eye(2), -1.0*ones(2));
fg += JacobianFactor(X(1), 10*I_2x2, -1.0*Matrix::Ones(2,2));
// odometry between x1 and x2: x2-x1=[0.2;-0.1]
fg += JacobianFactor(X(1), -10*eye(2), X(2), 10*eye(2), Vector2(2.0, -1.0));
fg += JacobianFactor(X(1), -10*I_2x2, X(2), 10*I_2x2, Vector2(2.0, -1.0));
// measurement between x1 and l1: l1-x1=[0.0;0.2]
fg += JacobianFactor(X(1), -5*eye(2), L(1), 5*eye(2), Vector2(0.0, 1.0));
fg += JacobianFactor(X(1), -5*I_2x2, L(1), 5*I_2x2, Vector2(0.0, 1.0));
// measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg += JacobianFactor(X(2), -5*eye(2), L(1), 5*eye(2), Vector2(-1.0, 1.5));
fg += JacobianFactor(X(2), -5*I_2x2, L(1), 5*I_2x2, Vector2(-1.0, 1.5));
return fg;
}
@ -409,7 +409,7 @@ inline GaussianFactorGraph createSimpleConstraintGraph() {
using namespace impl;
// create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1
Matrix Ax = eye(2);
Matrix Ax = I_2x2;
Vector b1(2);
b1(0) = 1.0;
b1(1) = -1.0;
@ -419,8 +419,8 @@ inline GaussianFactorGraph createSimpleConstraintGraph() {
// between _x_ and _y_, that is going to be the only factor on _y_
// |1 0||x_1| + |-1 0||y_1| = |0|
// |0 1||x_2| | 0 -1||y_2| |0|
Matrix Ax1 = eye(2);
Matrix Ay1 = eye(2) * -1;
Matrix Ax1 = I_2x2;
Matrix Ay1 = I_2x2 * -1;
Vector b2 = Vector2(0.0, 0.0);
JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
constraintModel));
@ -450,7 +450,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() {
using namespace impl;
// create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1
Matrix Ax = eye(2);
Matrix Ax = I_2x2;
Vector b1(2);
b1(0) = 1.0;
b1(1) = -1.0;
@ -466,7 +466,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() {
Ax1(0, 1) = 2.0;
Ax1(1, 0) = 2.0;
Ax1(1, 1) = 1.0;
Matrix Ay1 = eye(2) * 10;
Matrix Ay1 = I_2x2 * 10;
Vector b2 = Vector2(1.0, 2.0);
JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2,
constraintModel));
@ -492,7 +492,7 @@ inline VectorValues createSingleConstraintValues() {
inline GaussianFactorGraph createMultiConstraintGraph() {
using namespace impl;
// unary factor 1
Matrix A = eye(2);
Matrix A = I_2x2;
Vector b = Vector2(-2.0, 2.0);
JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1));

30
tests/testExpressionFactor.cpp
View File

@ -140,7 +140,7 @@ typedef Eigen::Matrix<double,9,3> Matrix93;
Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) {
Vector9 v;
v << p, p, p;
if (H) *H << eye(3), eye(3), eye(3);
if (H) *H << I_3x3, I_3x3, I_3x3;
return v;
}
typedef Eigen::Matrix<double,9,9> Matrix9;
@ -334,11 +334,11 @@ TEST(ExpressionFactor, Compose1) {
// Check unwhitenedError
std::vector<Matrix> H(2);
Vector actual = f.unwhitenedError(values, H);
EXPECT( assert_equal(eye(3), H[0],1e-9));
EXPECT( assert_equal(eye(3), H[1],1e-9));
EXPECT( assert_equal(I_3x3, H[0],1e-9));
EXPECT( assert_equal(I_3x3, H[1],1e-9));
// Check linearization
JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
JacobianFactor expected(1, I_3x3, 2, I_3x3, zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
@ -364,10 +364,10 @@ TEST(ExpressionFactor, compose2) {
std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(2*eye(3), H[0],1e-9));
EXPECT( assert_equal(2*I_3x3, H[0],1e-9));
// Check linearization
JacobianFactor expected(1, 2 * eye(3), zero(3));
JacobianFactor expected(1, 2 * I_3x3, zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
@ -393,10 +393,10 @@ TEST(ExpressionFactor, compose3) {
std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(eye(3), H[0],1e-9));
EXPECT( assert_equal(I_3x3, H[0],1e-9));
// Check linearization
JacobianFactor expected(3, eye(3), zero(3));
JacobianFactor expected(3, I_3x3, zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
@ -409,11 +409,11 @@ Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
// return dummy derivatives (not correct, but that's ok for testing here)
if (H1)
*H1 = eye(3);
*H1 = I_3x3;
if (H2)
*H2 = eye(3);
*H2 = I_3x3;
if (H3)
*H3 = eye(3);
*H3 = I_3x3;
return R1 * (R2 * R3);
}
@ -436,12 +436,12 @@ TEST(ExpressionFactor, composeTernary) {
std::vector<Matrix> H(3);
Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(3, H.size());
EXPECT( assert_equal(eye(3), H[0],1e-9));
EXPECT( assert_equal(eye(3), H[1],1e-9));
EXPECT( assert_equal(eye(3), H[2],1e-9));
EXPECT( assert_equal(I_3x3, H[0],1e-9));
EXPECT( assert_equal(I_3x3, H[1],1e-9));
EXPECT( assert_equal(I_3x3, H[2],1e-9));
// Check linearization
JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3));
JacobianFactor expected(1, I_3x3, 2, I_3x3, 3, I_3x3, zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);

2
tests/testExtendedKalmanFilter.cpp
View File

@ -223,7 +223,7 @@ public:
*H1 = -F(p1);
if(H2)
*H2 = eye(dim());
*H2 = Matrix::Identity(dim(),dim());
// Return the error between the prediction and the supplied value of p2
return (p2 - prediction).vector();

8
tests/testGaussianBayesTreeB.cpp
View File

@ -79,7 +79,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
double sigma3 = 0.61808;
Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished();
GaussianBayesNet expected3;
expected3 += GaussianConditional(X(5), zero(2), eye(2)/sigma3, X(6), A56/sigma3);
expected3 += GaussianConditional(X(5), zero(2), I_2x2/sigma3, X(6), A56/sigma3);
GaussianBayesTree::sharedClique C3 = bayesTree[X(4)];
GaussianBayesNet actual3 = C3->shortcut(R);
EXPECT(assert_equal(expected3,actual3,tol));
@ -88,7 +88,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
double sigma4 = 0.661968;
Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished();
GaussianBayesNet expected4;
expected4 += GaussianConditional(X(4), zero(2), eye(2)/sigma4, X(6), A46/sigma4);
expected4 += GaussianConditional(X(4), zero(2), I_2x2/sigma4, X(6), A46/sigma4);
GaussianBayesTree::sharedClique C4 = bayesTree[X(3)];
GaussianBayesNet actual4 = C4->shortcut(R);
EXPECT(assert_equal(expected4,actual4,tol));
@ -134,7 +134,7 @@ TEST( GaussianBayesTree, balanced_smoother_marginals )
// Check marginal on x1
JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), zero(2), sigmax1);
JacobianFactor actual1 = *bayesTree.marginalFactor(X(1));
Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1);
Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1);
Matrix actualCovarianceX1;
GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky);
actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse();
@ -243,7 +243,7 @@ TEST( GaussianBayesTree, balanced_smoother_joint )
GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
// Conditional density elements reused by both tests
const Matrix I = eye(2), A = -0.00429185*I;
const Matrix I = I_2x2, A = -0.00429185*I;
// Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
GaussianBayesNet expected1 = list_of

24
tests/testGaussianFactorGraphB.cpp
View File

@ -78,7 +78,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1 )
conditional = result.first->front();
// create expected Conditional Gaussian
Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
Matrix I = 15*I_2x2, R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
Vector d = Vector2(-0.133333, -0.0222222);
GaussianConditional expected(X(1),15*d,R11,L(1),S12,X(2),S13);
@ -96,7 +96,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2 )
// create expected Conditional Gaussian
double sig = 0.0894427;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2);
GaussianConditional expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma);
@ -112,7 +112,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1 )
// create expected Conditional Gaussian
double sig = sqrt(2.0)/10.;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2);
GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);
@ -129,7 +129,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast )
boost::tie(conditional,remaining) = fg.eliminateOne(ordering[X(1)], EliminateQR);
// create expected Conditional Gaussian
Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
Matrix I = 15*I_2x2, R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
Vector d = Vector2(-0.133333, -0.0222222), sigma = ones(2);
GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);
@ -159,7 +159,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2_fast )
// create expected Conditional Gaussian
double sig = 0.0894427;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Matrix I = I_2x2/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2);
GaussianConditional expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma);
@ -175,7 +175,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast )
// create expected Conditional Gaussian
double sig = sqrt(2.0)/10.;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Matrix I = I_2x2/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2);
GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);
@ -186,7 +186,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast )
TEST( GaussianFactorGraph, eliminateAll )
{
// create expected Chordal bayes Net
Matrix I = eye(2);
Matrix I = I_2x2;
Ordering ordering;
ordering += X(2),L(1),X(1);
@ -389,7 +389,7 @@ TEST( GaussianFactorGraph, elimination )
ord += X(1), X(2);
// Create Gaussian Factor Graph
GaussianFactorGraph fg;
Matrix Ap = eye(1), An = eye(1) * -1;
Matrix Ap = I_2x2, An = I_2x2 * -1;
Vector b = (Vector(1) << 0.0).finished();
SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,2.0);
fg += ord[X(1)], An, ord[X(2)], Ap, b, sigma;
@ -473,13 +473,13 @@ TEST(GaussianFactorGraph, replace)
SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0));
GaussianFactorGraph::sharedFactor f1(new JacobianFactor(
ord[X(1)], eye(3,3), ord[X(2)], eye(3,3), zero(3), noise));
ord[X(1)], I_3x3, ord[X(2)], I_3x3, Z_3x3, noise));
GaussianFactorGraph::sharedFactor f2(new JacobianFactor(
ord[X(2)], eye(3,3), ord[X(3)], eye(3,3), zero(3), noise));
ord[X(2)], I_3x3, ord[X(3)], I_3x3, Z_3x3, noise));
GaussianFactorGraph::sharedFactor f3(new JacobianFactor(
ord[X(3)], eye(3,3), ord[X(4)], eye(3,3), zero(3), noise));
ord[X(3)], I_3x3, ord[X(4)], I_3x3, Z_3x3, noise));
GaussianFactorGraph::sharedFactor f4(new JacobianFactor(
ord[X(5)], eye(3,3), ord[X(6)], eye(3,3), zero(3), noise));
ord[X(5)], I_3x3, ord[X(6)], I_3x3, Z_3x3, noise));
GaussianFactorGraph actual;
actual.push_back(f1);

2
tests/testGraph.cpp
View File

@ -112,7 +112,7 @@ TEST( Graph, composePoses )
TEST( GaussianFactorGraph, findMinimumSpanningTree )
{
GaussianFactorGraph g;
Matrix I = eye(2);
Matrix I = Z_2x2;
Vector2 b(0, 0);
const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5));
using namespace symbol_shorthand;

12
tests/testNonlinearEquality.cpp
View File

@ -56,7 +56,7 @@ TEST ( NonlinearEquality, linearization ) {
// check linearize
SharedDiagonal constraintModel = noiseModel::Constrained::All(3);
JacobianFactor expLF(key, eye(3), zero(3), constraintModel);
JacobianFactor expLF(key, Z_3x3, zero(3), constraintModel);
GaussianFactor::shared_ptr actualLF = nle->linearize(linearize);
EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF));
}
@ -180,7 +180,7 @@ TEST ( NonlinearEquality, allow_error_pose ) {
// check linearization
GaussianFactor::shared_ptr actLinFactor = nle.linearize(config);
Matrix A1 = eye(3, 3);
Matrix A1 =Z_3x3;
Vector b = expVec;
SharedDiagonal model = noiseModel::Constrained::All(3);
GaussianFactor::shared_ptr expLinFactor(
@ -289,7 +289,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) {
config1.insert(key, pt);
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
GaussianFactor::shared_ptr expected1(
new JacobianFactor(key, eye(2, 2), zero(2), hard_model));
new JacobianFactor(key, Z_2x2, Matrix::Zero(2,2), hard_model));
EXPECT(assert_equal(*expected1, *actual1, tol));
Values config2;
@ -297,7 +297,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) {
config2.insert(key, ptBad);
GaussianFactor::shared_ptr actual2 = constraint.linearize(config2);
GaussianFactor::shared_ptr expected2(
new JacobianFactor(key, eye(2, 2), Vector2(-1.0, 0.0), hard_model));
new JacobianFactor(key, Z_2x2, Vector2(-1.0, 0.0), hard_model));
EXPECT(assert_equal(*expected2, *actual2, tol));
}
@ -374,7 +374,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
config1.insert(key2, x2);
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1);
GaussianFactor::shared_ptr expected1(
new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), zero(2),
new JacobianFactor(key1, -Z_2x2, key2, Z_2x2, zero(2),
hard_model));
EXPECT(assert_equal(*expected1, *actual1, tol));
@ -385,7 +385,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
config2.insert(key2, x2bad);
GaussianFactor::shared_ptr actual2 = constraint.linearize(config2);
GaussianFactor::shared_ptr expected2(
new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), Vector2(1.0, 1.0),
new JacobianFactor(key1, -Z_2x2, key2, Z_2x2, Vector2(1.0, 1.0),
hard_model));
EXPECT(assert_equal(*expected2, *actual2, tol));
}

6
timing/timeMatrix.cpp
View File

@ -46,7 +46,7 @@ double timeCollect(size_t p, size_t m, size_t n, bool passDims, size_t reps) {
vector<const Matrix *> matrices;
for (size_t i=0; i<p;++i) {
Matrix * M = new Matrix;
(*M) = eye(m,n);
(*M) = Matrix::Identity(m,n);
matrices.push_back(M);
}
@ -216,8 +216,8 @@ double timeHouseholder(size_t reps) {
*/
double timeMatrixInsert(size_t reps) {
// create a matrix
Matrix bigBase = zeros(100, 100);
Matrix small = eye(5,5);
Matrix bigBase = Matrix::Zero(100, 100);
Matrix small = Matrix::Identity(5,5);
// perform timing
double elapsed;

2
timing/timePose2.cpp
View File

@ -63,7 +63,7 @@ Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2,
s, -c, dt2,
0.0, 0.0,-1.0).finished();
}
if (H2) *H2 = Matrix::Identity(3,3);
if (H2) *H2 = I_3x3;
return Pose2(R,t);
}

8
timing/timeRot2.cpp
View File

@ -55,8 +55,8 @@ Vector Rot2BetweenFactorEvaluateErrorOptimizedBetween(const Rot2& measured, cons
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2)
{
Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x)
if (H1) *H1 = -eye(1);
if (H2) *H2 = eye(1);
if (H1) *H1 = -I_1x1;
if (H2) *H2 = I_1x1;
// manifold equivalent of h(x)-z -> log(z,h(x))
return Rot2::Logmap(Rot2betweenOptimized(measured, hx));
}
@ -67,8 +67,8 @@ Vector Rot2BetweenFactorEvaluateErrorOptimizedBetweenNoeye(const Rot2& measured,
{
// TODO: Implement
Rot2 hx = Rot2betweenOptimized(p1, p2); // h(x)
if (H1) *H1 = -Matrix::Identity(1,1);
if (H2) *H2 = Matrix::Identity(1,1);
if (H1) *H1 = -I_1x1;
if (H2) *H2 = I_1x1;
// manifold equivalent of h(x)-z -> log(z,h(x))
return Rot2::Logmap(Rot2betweenOptimized(measured, hx));
}