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

Deprecated emul() in Vector.h.

release/4.3a0
Alex Hagiopol 2016-03-05 18:49:34 -05:00
parent 83eeb58c7a
commit 22c3af906e
5 changed files with 10 additions and 9 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(emul(sigmas,sigmas)); // calculate weights once Vector weights = reciprocal(sigmas.cwiseProduct(sigmas)); // 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

3
gtsam/base/Vector.h
View File

@ -20,7 +20,6 @@
#pragma once #pragma once
#ifndef MKL_BLAS #ifndef MKL_BLAS
#define MKL_BLAS MKL_DOMAIN_BLAS #define MKL_BLAS MKL_DOMAIN_BLAS
#endif #endif
@ -213,6 +212,7 @@ 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
/** /**
* elementwise multiplication * elementwise multiplication
* @param a first vector * @param a first vector
@ -220,6 +220,7 @@ GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t
* @return vector [a(i)*b(i)] * @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
/** /**
* elementwise division * elementwise division

7
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(emul(sigmas,sigmas)); Vector weights = reciprocal(sigmas.cwiseProduct(sigmas));
// perform solve // perform solve
Vector actual; double precision; Vector actual; double precision;
@ -224,8 +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(emul(sigmas,sigmas)); Vector weights = reciprocal(sigmas.cwiseProduct(sigmas));
// 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);
@ -244,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(emul(sigmas,sigmas)); Vector weights = reciprocal(sigmas.cwiseProduct(sigmas));
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(emul(sigmas,sigmas)); Matrix modelQ = diag(sigmas.cwiseProduct(sigmas));
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);

5
gtsam_unstable/slam/AHRS.cpp
View File

@ -171,7 +171,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
// calculate residual gravity measurement // calculate residual gravity measurement
z = n_g_ - trans(bRn) * measured_b_g; z = n_g_ - trans(bRn) * measured_b_g;
H = collect(3, &n_g_cross_, &Z_3x3, &bRn); H = collect(3, &n_g_cross_, &Z_3x3, &bRn);
R = trans(bRn) * diag(emul(sigmas_v_a_, sigmas_v_a_)) * bRn; R = trans(bRn) * diag(sigmas_v_a_.cwiseProduct(sigmas_v_a_)) * bRn;
} else { } else {
// my measurement prediction (in body frame): // my measurement prediction (in body frame):
// F(:,k) = bias - b_g // F(:,k) = bias - b_g
@ -186,7 +186,8 @@ 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(emul(sigmas_v_a_, sigmas_v_a_)); R = diag(sigmas_v_a_.cwiseQuotient(sigmas_v_a_));
} }
// update the Kalman filter // update the Kalman filter