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

Replaced cwiseProduct() with array().square(). Killed deprecated comments.

release/4.3a0
Alex Hagiopol 2016-03-05 21:08:24 -05:00
parent 22c3af906e
commit caa45ad67b
5 changed files with 6 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/base/Matrix.cpp
View File

@ -339,7 +339,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
list<boost::tuple<Vector, double, double> > results; list<boost::tuple<Vector, double, double> > results;
Vector pseudo(m); // allocate storage for pseudo-inverse 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 // We loop over all columns, because the columns that can be eliminated
// are not necessarily contiguous. For each one, estimate the corresponding // are not necessarily contiguous. For each one, estimate the corresponding

6
gtsam/base/Vector.h
View File

@ -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); GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 #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); GTSAM_EXPORT Vector emul(const Vector &a, const Vector &b);
#endif #endif

6
gtsam/base/tests/testVector.cpp
View File

@ -198,7 +198,7 @@ TEST(Vector, weightedPseudoinverse )
// create sigmas // create sigmas
Vector sigmas(2); Vector sigmas(2);
sigmas(0) = 0.1; sigmas(1) = 0.2; sigmas(0) = 0.1; sigmas(1) = 0.2;
Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); Vector weights = reciprocal(sigmas.array().square());
// perform solve // perform solve
Vector actual; double precision; Vector actual; double precision;
@ -224,7 +224,7 @@ TEST(Vector, weightedPseudoinverse_constraint )
// create sigmas // create sigmas
Vector sigmas(2); Vector sigmas(2);
sigmas(0) = 0.0; sigmas(1) = 0.2; sigmas(0) = 0.0; sigmas(1) = 0.2;
Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); Vector weights = reciprocal(sigmas.array().square());
// perform solve // perform solve
Vector actual; double precision; Vector actual; double precision;
boost::tie(actual, precision) = weightedPseudoinverse(x, weights); 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 a = (Vector(4) << 1., 0., 0., 0.).finished();
Vector sigmas = (Vector(4) << 0.1, 0.1, 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; Vector pseudo; double precision;
boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights); boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);

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

@ -270,7 +270,7 @@ TEST( KalmanFilter, QRvsCholesky ) {
EXPECT(assert_equal(expected2, pb2->covariance(), 1e-7)); EXPECT(assert_equal(expected2, pb2->covariance(), 1e-7));
// do the above update again, this time with a full Matrix Q // 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 pa3 = kfa.updateQ(pa, H, z, modelQ);
KalmanFilter::State pb3 = kfb.updateQ(pb, H, z, modelQ); KalmanFilter::State pb3 = kfb.updateQ(pb, H, z, modelQ);

3
gtsam_unstable/slam/AHRS.cpp
View File

@ -186,8 +186,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
Matrix b_g = bRn * n_g_cross_; Matrix b_g = bRn * n_g_cross_;
H = collect(3, &b_g, &Z_3x3, &I_3x3); H = collect(3, &b_g, &Z_3x3, &I_3x3);
// And the measurement noise, TODO: should be created once where sigmas_v_a is given // 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 // update the Kalman filter