From 70b2aab35209bbd4d8957a5be91292474ef27c09 Mon Sep 17 00:00:00 2001 From: Alex Hagiopol Date: Mon, 11 Apr 2016 15:11:29 -0400 Subject: [PATCH] Deprecated all inline functions in Matrix.h. --- CMakeLists.txt | 2 +- gtsam/base/Matrix.h | 49 +---------------- gtsam/base/numericalDerivative.h | 2 +- gtsam/base/tests/testMatrix.cpp | 52 +++++++++---------- gtsam/base/tests/testSymmetricBlockMatrix.cpp | 2 +- gtsam/geometry/OrientedPlane3.cpp | 2 +- gtsam/geometry/Pose2.cpp | 2 +- gtsam/geometry/tests/testCal3_S2.cpp | 4 +- gtsam/geometry/tests/testPinholeCamera.cpp | 2 +- gtsam/geometry/tests/testPinholePose.cpp | 2 +- gtsam/geometry/tests/testPoint2.cpp | 8 +-- gtsam/geometry/tests/testPoint3.cpp | 8 +-- gtsam/geometry/tests/testPose2.cpp | 4 +- gtsam/geometry/tests/testPose3.cpp | 8 +-- gtsam/geometry/tests/testRot2.cpp | 8 +-- gtsam/geometry/tests/testRot3.cpp | 4 +- gtsam/geometry/tests/testSimpleCamera.cpp | 2 +- gtsam/linear/GaussianConditional.cpp | 2 +- gtsam/linear/JacobianFactor.cpp | 3 +- gtsam/linear/KalmanFilter.cpp | 4 +- gtsam/linear/iterative.h | 2 +- gtsam/linear/tests/testGaussianBayesTree.cpp | 4 +- .../linear/tests/testGaussianFactorGraph.cpp | 20 +++---- gtsam/linear/tests/testHessianFactor.cpp | 6 +-- gtsam/linear/tests/testJacobianFactor.cpp | 34 ++++++------ gtsam/linear/tests/testKalmanFilter.cpp | 30 +++++------ gtsam/linear/tests/testNoiseModel.cpp | 16 +++--- .../linear/tests/testSerializationLinear.cpp | 8 +-- gtsam/navigation/AttitudeFactor.h | 2 +- gtsam/navigation/MagFactor.h | 4 +- gtsam/navigation/PreintegrationBase.cpp | 2 +- gtsam/nonlinear/NonlinearEquality.h | 10 ++-- gtsam/nonlinear/tests/testExpression.cpp | 6 +-- .../tests/testLinearContainerFactor.cpp | 2 +- gtsam/slam/GeneralSFMFactor.h | 6 +-- gtsam/slam/InitializePose3.cpp | 11 ++-- gtsam/slam/JacobianFactorQ.h | 2 +- gtsam/slam/OrientedPlane3Factor.cpp | 2 +- gtsam/slam/PoseRotationPrior.h | 2 +- gtsam/slam/PoseTranslationPrior.h | 2 +- gtsam/slam/ProjectionFactor.h | 4 +- gtsam/slam/ReferenceFrameFactor.h | 2 +- gtsam/slam/SmartProjectionFactor.h | 2 +- gtsam/slam/StereoFactor.h | 4 +- gtsam/slam/TriangulationFactor.h | 2 +- gtsam/slam/dataset.cpp | 8 +-- gtsam/slam/lago.cpp | 4 +- .../tests/testRegularImplicitSchurFactor.cpp | 18 +++---- gtsam_unstable/dynamics/DynamicsPriors.h | 6 +-- gtsam_unstable/dynamics/Pendulum.h | 24 ++++----- gtsam_unstable/dynamics/SimpleHelicopter.h | 2 +- gtsam_unstable/dynamics/VelocityConstraint3.h | 6 +-- gtsam_unstable/dynamics/tests/testPoseRTV.cpp | 8 +-- .../dynamics/tests/testSimpleHelicopter.cpp | 4 +- gtsam_unstable/geometry/InvDepthCamera3.h | 2 +- gtsam_unstable/geometry/Pose3Upright.cpp | 10 ++-- .../linear/tests/testLinearEquality.cpp | 2 +- gtsam_unstable/linear/tests/testQPSolver.cpp | 34 ++++++------ .../nonlinear/tests/testParticleFactor.cpp | 22 ++++---- gtsam_unstable/slam/AHRS.cpp | 12 ++--- gtsam_unstable/slam/DummyFactor.cpp | 2 +- .../slam/EquivInertialNavFactor_GlobalVel.h | 18 +++---- .../EquivInertialNavFactor_GlobalVel_NoBias.h | 4 +- .../slam/GaussMarkov1stOrderFactor.h | 4 +- .../slam/InertialNavFactor_GlobalVelocity.h | 6 +-- gtsam_unstable/slam/InvDepthFactor3.h | 6 +-- gtsam_unstable/slam/MultiProjectionFactor.h | 4 +- gtsam_unstable/slam/PartialPriorFactor.h | 4 +- gtsam_unstable/slam/ProjectionFactorPPP.h | 6 +-- gtsam_unstable/slam/ProjectionFactorPPPC.h | 8 +-- .../slam/RelativeElevationFactor.cpp | 4 +- gtsam_unstable/slam/SmartRangeFactor.h | 2 +- .../slam/SmartStereoProjectionFactor.h | 2 +- gtsam_unstable/slam/tests/testDummyFactor.cpp | 2 +- .../testEquivInertialNavFactor_GlobalVel.cpp | 4 +- .../slam/tests/testSmartRangeFactor.cpp | 2 +- .../testing_tools/base/cholScalingTest.m | 4 +- matlab/gtsam_examples/IMUKittiExampleGPS.m | 2 +- matlab/gtsam_examples/SBAExample.m | 2 +- .../FlightCameraTransformIMU.m | 2 +- .../IMUKittiExampleAdvanced.m | 2 +- matlab/unstable_examples/IMUKittiExampleVO.m | 2 +- ...ansformCalProjectionFactorIMUExampleISAM.m | 2 +- tests/simulated2D.h | 10 ++-- tests/simulated2DConstraints.h | 2 +- tests/simulated2DOriented.h | 2 +- tests/simulated3D.h | 10 ++-- tests/smallExample.h | 20 +++---- tests/testExpressionFactor.cpp | 30 +++++------ tests/testExtendedKalmanFilter.cpp | 2 +- tests/testGaussianBayesTreeB.cpp | 8 +-- tests/testGaussianFactorGraphB.cpp | 24 ++++----- tests/testGraph.cpp | 2 +- tests/testNonlinearEquality.cpp | 12 ++--- timing/timeMatrix.cpp | 6 +-- timing/timePose2.cpp | 2 +- timing/timeRot2.cpp | 8 +-- 97 files changed, 348 insertions(+), 401 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8c4229ed5..16fa5d4c5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 - 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 diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 84f201c51..0c7a3f9bd 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -74,7 +74,7 @@ typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; /** - * equals with an tolerance + * equals with a tolerance */ template bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBase& 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 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::ReshapedTy return Reshape::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 diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 9a42db3ce..1bd62c257 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -142,7 +142,7 @@ typename internal::FixedSizeMatrix::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; diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 13d32794e..b2d512e4a 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -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 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 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)); diff --git a/gtsam/base/tests/testSymmetricBlockMatrix.cpp b/gtsam/base/tests/testSymmetricBlockMatrix.cpp index df3403cf4..da193aec5 100644 --- a/gtsam/base/tests/testSymmetricBlockMatrix.cpp +++ b/gtsam/base/tests/testSymmetricBlockMatrix.cpp @@ -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() = testBlockMatrix(1, 1).triangularView(); EXPECT(assert_equal(expected1, actual1)); EXPECT(assert_equal(Matrix(expected1.triangularView()), actual1t)); diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp index 514328483..4af0257ec 100644 --- a/gtsam/geometry/OrientedPlane3.cpp +++ b/gtsam/geometry/OrientedPlane3.cpp @@ -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; } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 383c94dd1..8a0e8fbf5 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -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]; diff --git a/gtsam/geometry/tests/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 3e93dedc1..4ccb6075b 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -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)); } diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 4fbdbcbe1..99dcb95bf 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -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)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index c5137e3b3..0d840de7e 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -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)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 6f8d45b3e..3a636b9bf 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -73,12 +73,12 @@ TEST(Point2, Lie) { Matrix H1, H2; EXPECT(assert_equal(Point2(5,7), traits::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::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::Retract(p1, Vector2(4., 5.)))); EXPECT(assert_equal(Vector2(3.,3.), traits::Local(p1,p2))); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index da7696d0d..9b6e53323 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -47,12 +47,12 @@ TEST(Point3, Lie) { Matrix H1, H2; EXPECT(assert_equal(Point3(5, 7, 9), traits::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::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::Retract(p1, Vector3(4,5,6)))); EXPECT(assert_equal(Vector3(3, 3, 3), traits::Local(p1,p2))); diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 5b835200a..d1f68fe28 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -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(testing::compose, pose1, pose2); Matrix numericalH2 = numericalDerivative22(testing::compose, pose1, pose2); EXPECT(assert_equal(expectedH1,actualDcompose1)); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 648197ced..957c7621a 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -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, 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)); diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 6ead22860..4394103c9 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -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)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 876897ab3..0c78cc426 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -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 ) { diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 71dcabd4e..70b3069f2 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -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)); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index feb477a4e..eefb6302f 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -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_) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index fc1a7c841..4bc141798 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -565,8 +565,7 @@ void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e, pair 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; } } diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 68e6a00f1..c0d294adf 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -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); diff --git a/gtsam/linear/iterative.h b/gtsam/linear/iterative.h index 1ba0a7423..22f65b8de 100644 --- a/gtsam/linear/iterative.h +++ b/gtsam/linear/iterative.h @@ -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; } }; diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 4f7c5c335..738a6d59f 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -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)); } diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 7404caa14..26e6b1925 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -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; } diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 61083a926..4b824d818 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -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); diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index d57f896ef..7ae28264c 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -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 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 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 @@ -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)); } diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index 16be98306..b095c5a31 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -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); diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 434d94d7f..43ce271f3 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -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)); } diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index 71fdbe6a6..669aa30e8 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -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(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); diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index b61a861d6..21f74ac06 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -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; diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index bee3e8944..3875391d0 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -154,7 +154,7 @@ public: // measured bM = nRb� * 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_); } }; diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index fefd4b23c..c243ca860 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -105,7 +105,7 @@ pair 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(); } diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index e24c0d610..ccdcb86be 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -142,11 +142,11 @@ public: const size_t nj = traits::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::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 H = boost::none) const { if (H) - (*H) = eye(traits::GetDimension(x1)); + (*H) = Matrix::Identity(traits::GetDimension(x1),traits::GetDimension(x1)); // manifold equivalent of h(x)-z -> log(z,h(x)) return traits::Local(value_,x1); } @@ -322,8 +322,8 @@ public: Vector evaluateError(const X& x1, const X& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { static const size_t p = traits::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::Local(x1,x2); } diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 43f1cd017..0b0511e40 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -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); } diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index bd862ef94..7dd4a1f8b 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -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; diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index d2b042fed..db7dc4e74 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -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; } diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 7d7a86405..180e5cd24 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -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& 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; diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 16560a43e..f9f258055 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -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 QF; diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 3728e53b1..5b0c6a6b9 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -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(); diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 94c19a9d0..44f317a49 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -75,7 +75,7 @@ public: Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const { const Rotation& newR = pose.rotation(); if (H) { - *H = gtsam::zeros(rDim, xDim); + *H = Matrix::Zero(rDim, xDim); std::pair rotInterval = POSE::rotationInterval(); (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim); } diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index a24421b34..0b1c0cf63 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -65,7 +65,7 @@ public: const int tDim = traits::GetDimension(newTrans); const int xDim = traits::GetDimension(pose); if (H) { - *H = gtsam::zeros(tDim, xDim); + *H = Matrix::Zero(tDim, xDim); std::pair transInterval = POSE::translationInterval(); (*H).middleCols(transInterval.first, tDim) = R.matrix(); } diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 1056527d1..211e299e3 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -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; diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 90ceeafc8..966ade343 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -100,7 +100,7 @@ public: boost::optional Dlocal = boost::none) const { Point newlocal = transform_point(trans, global, Dtrans, Dforeign); if (Dlocal) - *Dlocal = -1* gtsam::eye(Point::dimension); + *Dlocal = -1* Matrix::Identity(Point::dimension,Point::dimension); return traits::Local(local,newlocal); } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index a5d5d1883..9d0f9c554 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -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 >(this->keys_, diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 578422eaf..528479f80 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -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; diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index aa50929a5..2748599c4 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -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" diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index c1c919a58..cbfa1ad31 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -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 diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index a29d3e8b1..fa6fce37a 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -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 = diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 3664de9c5..261cc1716 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -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 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 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])); diff --git a/gtsam_unstable/dynamics/DynamicsPriors.h b/gtsam_unstable/dynamics/DynamicsPriors.h index 5b986415e..b4dfa7dc8 100644 --- a/gtsam_unstable/dynamics/DynamicsPriors.h +++ b/gtsam_unstable/dynamics/DynamicsPriors.h @@ -57,7 +57,7 @@ struct VelocityPrior : public gtsam::PartialPriorFactor { 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 { 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 { 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(); } }; diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index ecc43ad79..28a26edce 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -50,9 +50,9 @@ public: boost::optional H2 = boost::none, boost::optional 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 H2 = boost::none, boost::optional 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(); } diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index a3dd6327b..f38c256b1 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -140,7 +140,7 @@ public: } if (H3) { - *H3 = zeros(6,6); + *H3 = Z_6x6; insertSub(*H3, -h_*D_gravityBody_gk, 3, 0); } diff --git a/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index 9ecc7619e..63a2c43bf 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/gtsam_unstable/dynamics/VelocityConstraint3.h @@ -41,9 +41,9 @@ public: boost::optional H2 = boost::none, boost::optional 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(); } diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 855131042..1e52988c0 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -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)))); } diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 6f75c264c..db5c2bc6e 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -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); diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 4ce0eaedb..4f1b42610 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -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); diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index f83cb19fb..4237170f0 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -81,7 +81,7 @@ Pose3 Pose3Upright::pose() const { Pose3Upright Pose3Upright::inverse(boost::optional 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; } diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp index bf41743a2..90f3de586 100644 --- a/gtsam_unstable/linear/tests/testLinearEquality.cpp +++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp @@ -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); diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 8fca61ca4..d643223f8 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -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)); diff --git a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp index bfd3c9168..1cf94b161 100644 --- a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp @@ -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); diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index ce8fd29f3..a16a4f9c0 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -83,7 +83,7 @@ std::pair 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 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 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 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 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 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); diff --git a/gtsam_unstable/slam/DummyFactor.cpp b/gtsam_unstable/slam/DummyFactor.cpp index 1ce3a878d..70aff0596 100644 --- a/gtsam_unstable/slam/DummyFactor.cpp +++ b/gtsam_unstable/slam/DummyFactor.cpp @@ -50,7 +50,7 @@ DummyFactor::linearize(const Values& c) const { std::vector > terms(this->size()); for(size_t j=0; jsize(); ++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_); diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 79a37c2ff..58284c3a6 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -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(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); Matrix H_pos_vel = numericalDerivative11(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, diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 8216942cd..acca233d9 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -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(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 7f9564ee3..68c972a86 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -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; } diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 30d3a216d..c3a67232a 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -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; diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index ac481f979..3de6a1824 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/gtsam_unstable/slam/InvDepthFactor3.h @@ -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(); diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index f1487f562..84d4ae0db 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -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; diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index e3080466f..a848620e3 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -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& 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(); diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index 5b1540c83..3b331d66b 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -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; diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index 069274065..1bd352a33 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -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; diff --git a/gtsam_unstable/slam/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp index 941a1db89..db077994f 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp @@ -21,7 +21,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p boost::optional H1, boost::optional 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(); diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 84aed6ca8..6e638cc77 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -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); diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 7cbeaae65..7f1bf544a 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -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 >(this->keys_, diff --git a/gtsam_unstable/slam/tests/testDummyFactor.cpp b/gtsam_unstable/slam/tests/testDummyFactor.cpp index 86b4e5584..03a57b352 100644 --- a/gtsam_unstable/slam/tests/testDummyFactor.cpp +++ b/gtsam_unstable/slam/tests/testDummyFactor.cpp @@ -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)); } diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 476447f8b..35bf5790e 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -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) diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index ee06dbce2..9ae4aa928 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -70,7 +70,7 @@ TEST( SmartRangeFactor, unwhitenedError ) { // Check Jacobian for n==1 vector 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); diff --git a/gtsam_unstable/testing_tools/base/cholScalingTest.m b/gtsam_unstable/testing_tools/base/cholScalingTest.m index ed347a523..24f597ed7 100644 --- a/gtsam_unstable/testing_tools/base/cholScalingTest.m +++ b/gtsam_unstable/testing_tools/base/cholScalingTest.m @@ -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); diff --git a/matlab/gtsam_examples/IMUKittiExampleGPS.m b/matlab/gtsam_examples/IMUKittiExampleGPS.m index 32f61e47f..b335b54be 100644 --- a/matlab/gtsam_examples/IMUKittiExampleGPS.m +++ b/matlab/gtsam_examples/IMUKittiExampleGPS.m @@ -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]; diff --git a/matlab/gtsam_examples/SBAExample.m b/matlab/gtsam_examples/SBAExample.m index b0f754044..808064f4c 100644 --- a/matlab/gtsam_examples/SBAExample.m +++ b/matlab/gtsam_examples/SBAExample.m @@ -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; diff --git a/matlab/unstable_examples/FlightCameraTransformIMU.m b/matlab/unstable_examples/FlightCameraTransformIMU.m index b0b37ee33..25d6e69bd 100644 --- a/matlab/unstable_examples/FlightCameraTransformIMU.m +++ b/matlab/unstable_examples/FlightCameraTransformIMU.m @@ -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]; diff --git a/matlab/unstable_examples/IMUKittiExampleAdvanced.m b/matlab/unstable_examples/IMUKittiExampleAdvanced.m index cb13eacee..8e2295159 100644 --- a/matlab/unstable_examples/IMUKittiExampleAdvanced.m +++ b/matlab/unstable_examples/IMUKittiExampleAdvanced.m @@ -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 diff --git a/matlab/unstable_examples/IMUKittiExampleVO.m b/matlab/unstable_examples/IMUKittiExampleVO.m index 6434e750a..91f187301 100644 --- a/matlab/unstable_examples/IMUKittiExampleVO.m +++ b/matlab/unstable_examples/IMUKittiExampleVO.m @@ -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]; diff --git a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m index fd4a17469..0999596c9 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m @@ -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]; diff --git a/tests/simulated2D.h b/tests/simulated2D.h index a1080a6de..0012f5f45 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -90,7 +90,7 @@ namespace simulated2D { /// Prior on a single pose, optionally returns derivative inline Point2 prior(const Point2& x, boost::optional 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 H1 = boost::none, boost::optional 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 H1 = boost::none, boost::optional 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; } diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index f593ab23a..ccc734cfd 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -90,7 +90,7 @@ namespace simulated2D { virtual double value(const Point& x, boost::optional H = boost::none) const { if (H) { - Matrix D = zeros(1, traits::GetDimension(x)); + Matrix D = Matrix::Zero(1, traits::GetDimension(x)); D(0, IDX) = 1.0; *H = D; } diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h index b05fb45c1..9e604cb3c 100644 --- a/tests/simulated2DOriented.h +++ b/tests/simulated2DOriented.h @@ -63,7 +63,7 @@ namespace simulated2DOriented { /// Prior on a single pose, optional derivative version Pose2 prior(const Pose2& x, boost::optional H = boost::none) { - if (H) *H = gtsam::eye(3); + if (H) *H = I_3x3; return x; } diff --git a/tests/simulated3D.h b/tests/simulated3D.h index 84d9ec8cd..3c92e733e 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.h @@ -39,7 +39,7 @@ namespace simulated3D { * Prior on a single pose */ Point3 prior(const Point3& x, boost::optional 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 H = boost::none) { Point3 odo(const Point3& x1, const Point3& x2, boost::optional H1 = boost::none, boost::optional 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 H1 = boost::none, boost::optional 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; } diff --git a/tests/smallExample.h b/tests/smallExample.h index 9866d22aa..33f42da7f 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -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)); diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 282a6b816..c20eca34a 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -140,7 +140,7 @@ typedef Eigen::Matrix 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 Matrix9; @@ -334,11 +334,11 @@ TEST(ExpressionFactor, Compose1) { // Check unwhitenedError std::vector 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 gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -364,10 +364,10 @@ TEST(ExpressionFactor, compose2) { std::vector 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 gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); @@ -393,10 +393,10 @@ TEST(ExpressionFactor, compose3) { std::vector 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 gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(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 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 gf = f.linearize(values); boost::shared_ptr jf = // boost::dynamic_pointer_cast(gf); diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index a7bcf7153..00ab4a16c 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -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(); diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index cf54ce96e..9b172d572 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -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 diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 88dc91ce7..cde79e637 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -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); diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 6c1fd7dd5..fdb93c29c 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -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; diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 7142f72d5..5e4f832e4 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -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)); } diff --git a/timing/timeMatrix.cpp b/timing/timeMatrix.cpp index d51b0abd2..a094f9628 100644 --- a/timing/timeMatrix.cpp +++ b/timing/timeMatrix.cpp @@ -46,7 +46,7 @@ double timeCollect(size_t p, size_t m, size_t n, bool passDims, size_t reps) { vector matrices; for (size_t i=0; i H1, boost::optional 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)); }