diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 3542d06d9..213b6bdf1 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -339,7 +339,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) { list > results; Vector pseudo(m); // allocate storage for pseudo-inverse - Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); // calculate weights once + Vector weights = reciprocal(sigmas.array().square()); // calculate weights once // We loop over all columns, because the columns that can be eliminated // are not necessarily contiguous. For each one, estimate the corresponding diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 527068a6d..d7994da83 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -213,12 +213,6 @@ GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2); GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i); #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 -/** - * elementwise multiplication - * @param a first vector - * @param b second vector - * @return vector [a(i)*b(i)] - */ GTSAM_EXPORT Vector emul(const Vector &a, const Vector &b); #endif diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index dd3527b46..6ea1627a5 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -198,7 +198,7 @@ TEST(Vector, weightedPseudoinverse ) // create sigmas Vector sigmas(2); sigmas(0) = 0.1; sigmas(1) = 0.2; - Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); + Vector weights = reciprocal(sigmas.array().square()); // perform solve Vector actual; double precision; @@ -224,7 +224,7 @@ TEST(Vector, weightedPseudoinverse_constraint ) // create sigmas Vector sigmas(2); sigmas(0) = 0.0; sigmas(1) = 0.2; - Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); + Vector weights = reciprocal(sigmas.array().square()); // perform solve Vector actual; double precision; boost::tie(actual, precision) = weightedPseudoinverse(x, weights); @@ -243,7 +243,7 @@ TEST(Vector, weightedPseudoinverse_nan ) { Vector a = (Vector(4) << 1., 0., 0., 0.).finished(); Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished(); - Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); + Vector weights = reciprocal(sigmas.array().square()); Vector pseudo; double precision; boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights); diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index c14f88129..16be98306 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -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.cwiseProduct(sigmas)); + Matrix modelQ = diag(sigmas.array().square()); KalmanFilter::State pa3 = kfa.updateQ(pa, H, z, modelQ); KalmanFilter::State pb3 = kfb.updateQ(pb, H, z, modelQ); diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 9ea923ebc..511514347 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -186,8 +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_.cwiseQuotient(sigmas_v_a_)); - + R = diag(sigmas_v_a_.array().square()); } // update the Kalman filter