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

Merged in fix/EigenAllTheWay (pull request #239) 

Eigen All The Way
release/4.3a0
Frank Dellaert 2016-03-13 14:03:11 -07:00
parent 83eeb58c7a 3a222cf8c4
commit 990d9bd5da
23 changed files with 83 additions and 260 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;
Vector pseudo(m); // allocate storage for pseudo-inverse
Vector weights = reciprocal(emul(sigmas,sigmas)); // calculate weights once
Vector weights = sigmas.array().square().inverse(); // 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

59
gtsam/base/Vector.cpp
View File

@ -42,11 +42,6 @@ bool zero(const Vector& v) {
return result;
}
/* ************************************************************************* */
Vector repeat(size_t n, double value) {
return Vector::Constant(n, value);
}
/* ************************************************************************* */
Vector delta(size_t n, size_t i, double value) {
return Vector::Unit(n, i) * value;
@ -176,28 +171,6 @@ bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) {
return flag;
}
/* ************************************************************************* */
ConstSubVector sub(const Vector &v, size_t i1, size_t i2) {
return v.segment(i1,i2-i1);
}
/* ************************************************************************* */
void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {
fullVector.segment(i, subVector.size()) = subVector;
}
/* ************************************************************************* */
Vector emul(const Vector &a, const Vector &b) {
assert (b.size()==a.size());
return a.cwiseProduct(b);
}
/* ************************************************************************* */
Vector ediv(const Vector &a, const Vector &b) {
assert (b.size()==a.size());
return a.cwiseQuotient(b);
}
/* ************************************************************************* */
Vector ediv_(const Vector &a, const Vector &b) {
size_t n = a.size();
@ -210,36 +183,6 @@ Vector ediv_(const Vector &a, const Vector &b) {
return c;
}
/* ************************************************************************* */
double sum(const Vector &a) {
return a.sum();
}
/* ************************************************************************* */
double norm_2(const Vector& v) {
return v.norm();
}
/* ************************************************************************* */
Vector reciprocal(const Vector &a) {
return a.array().inverse();
}
/* ************************************************************************* */
Vector esqrt(const Vector& v) {
return v.cwiseSqrt();
}
/* ************************************************************************* */
Vector abs(const Vector& v) {
return v.cwiseAbs();
}
/* ************************************************************************* */
double max(const Vector &a) {
return a.maxCoeff();
}
/* ************************************************************************* */
// imperative version, pass in x
double houseInPlace(Vector &v) {
@ -363,6 +306,4 @@ Vector concatVectors(size_t nrVectors, ...)
return concatVectors(vs);
}
/* ************************************************************************* */
} // namespace gtsam

99
gtsam/base/Vector.h
View File

@ -20,7 +20,6 @@
#pragma once
#ifndef MKL_BLAS
#define MKL_BLAS MKL_DOMAIN_BLAS
#endif
@ -64,13 +63,6 @@ GTSAM_MAKE_VECTOR_DEFS(12);
typedef Eigen::VectorBlock<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> ConstSubVector;
/**
* Create vector initialized to a constant value
* @param n is the size of the vector
* @param value is a constant value to insert into the vector
*/
GTSAM_EXPORT Vector repeat(size_t n, double value);
/**
* Create basis vector of dimension n,
* with a constant in spot i
@ -196,39 +188,6 @@ GTSAM_EXPORT bool assert_equal(const ConstSubVector& vec1, const ConstSubVector&
*/
GTSAM_EXPORT bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol=1e-9);
/**
* extract subvector, slice semantics, i.e. range = [i1,i2[ excluding i2
* @param v Vector
* @param i1 first row index
* @param i2 last row index + 1
* @return subvector v(i1:i2)
*/
GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2);
/**
* Inserts a subvector into a vector IN PLACE
* @param fullVector is the vector to be changed
* @param subVector is the vector to insert
* @param i is the index where the subvector should be inserted
*/
GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i);
/**
* 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);
/**
* elementwise division
* @param a first vector
* @param b second vector
* @return vector [a(i)/b(i)]
*/
GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b);
/**
* elementwise division, but 0/0 = 0, not inf
* @param a first vector
@ -237,49 +196,6 @@ GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b);
*/
GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b);
/**
* sum vector elements
* @param a vector
* @return sum_i a(i)
*/
GTSAM_EXPORT double sum(const Vector &a);
/**
* Calculates L2 norm for a vector
* modeled after boost.ublas for compatibility
* @param v vector
* @return the L2 norm
*/
GTSAM_EXPORT double norm_2(const Vector& v);
/**
* Elementwise reciprocal of vector elements
* @param a vector
* @return [1/a(i)]
*/
GTSAM_EXPORT Vector reciprocal(const Vector &a);
/**
* Elementwise sqrt of vector elements
* @param v is a vector
* @return [sqrt(a(i))]
*/
GTSAM_EXPORT Vector esqrt(const Vector& v);
/**
* Absolute values of vector elements
* @param v is a vector
* @return [abs(a(i))]
*/
GTSAM_EXPORT Vector abs(const Vector& v);
/**
* Return the max element of a vector
* @param a is a vector
* @return max(a)
*/
GTSAM_EXPORT double max(const Vector &a);
/**
* Dot product
*/
@ -356,6 +272,21 @@ GTSAM_EXPORT Vector concatVectors(const std::list<Vector>& vs);
*/
GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
GTSAM_EXPORT inline Vector abs(const Vector& v){return v.cwiseAbs();}
GTSAM_EXPORT inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);}
GTSAM_EXPORT inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();}
GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);}
GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();}
GTSAM_EXPORT inline double norm_2(const Vector& v) {return v.norm();}
GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();}
GTSAM_EXPORT inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);}
GTSAM_EXPORT inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);}
GTSAM_EXPORT inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;}
GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();}
#endif
} // namespace gtsam
#include <boost/serialization/nvp.hpp>

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

@ -119,36 +119,6 @@ TEST(Vector, negate )
EXPECT(assert_equal(b, -a));
}
/* ************************************************************************* */
TEST(Vector, sub )
{
Vector a(6);
a(0) = 10; a(1) = 20; a(2) = 3;
a(3) = 34; a(4) = 11; a(5) = 2;
Vector result(sub(a,2,5));
Vector b(3);
b(0) = 3; b(1) = 34; b(2) =11;
EXPECT(b==result);
EXPECT(assert_equal(b, result));
}
/* ************************************************************************* */
TEST(Vector, subInsert )
{
Vector big = zero(6),
small = ones(3);
size_t i = 2;
subInsert(big, small, i);
Vector expected = (Vector(6) << 0.0, 0.0, 1.0, 1.0, 1.0, 0.0).finished();
EXPECT(assert_equal(expected, big));
}
/* ************************************************************************* */
TEST(Vector, householder )
{
@ -198,7 +168,7 @@ TEST(Vector, weightedPseudoinverse )
// create sigmas
Vector sigmas(2);
sigmas(0) = 0.1; sigmas(1) = 0.2;
Vector weights = reciprocal(emul(sigmas,sigmas));
Vector weights = sigmas.array().square().inverse();
// perform solve
Vector actual; double precision;
@ -224,8 +194,7 @@ TEST(Vector, weightedPseudoinverse_constraint )
// create sigmas
Vector sigmas(2);
sigmas(0) = 0.0; sigmas(1) = 0.2;
Vector weights = reciprocal(emul(sigmas,sigmas));
Vector weights = sigmas.array().square().inverse();
// perform solve
Vector actual; double precision;
boost::tie(actual, precision) = weightedPseudoinverse(x, weights);
@ -244,7 +213,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(emul(sigmas,sigmas));
Vector weights = sigmas.array().square().inverse();
Vector pseudo; double precision;
boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);
@ -253,17 +222,6 @@ TEST(Vector, weightedPseudoinverse_nan )
DOUBLES_EQUAL(100, precision, 1e-5);
}
/* ************************************************************************* */
TEST(Vector, ediv )
{
Vector a = Vector3(10., 20., 30.);
Vector b = Vector3(2.0, 5.0, 6.0);
Vector actual(ediv(a,b));
Vector c = Vector3(5.0, 4.0, 5.0);
EXPECT(assert_equal(c,actual));
}
/* ************************************************************************* */
TEST(Vector, dot )
{
@ -303,13 +261,6 @@ TEST(Vector, greater_than )
EXPECT(greaterThanOrEqual(v1, v2)); // test equals
}
/* ************************************************************************* */
TEST(Vector, reciprocal )
{
Vector v = Vector3(1.0, 2.0, 4.0);
EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25),reciprocal(v)));
}
/* ************************************************************************* */
TEST(Vector, linear_dependent )
{

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

@ -148,7 +148,7 @@ TEST(Pose3, Adjoint_full)
Pose3 Agrawal06iros(const Vector& xi) {
Vector w = xi.head(3);
Vector v = xi.tail(3);
double t = norm_2(w);
double t = w.norm();
if (t < 1e-5)
return Pose3(Rot3(), Point3(v));
else {
@ -538,7 +538,7 @@ TEST(Pose3, retract_localCoordinates)
/* ************************************************************************* */
TEST(Pose3, expmap_logmap)
{
Vector d12 = repeat(6,0.1);
Vector d12 = Vector6::Constant(0.1);
Pose3 t1 = T, t2 = t1.expmap(d12);
EXPECT(assert_equal(d12, t1.logmap(t2)));
}

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

@ -96,7 +96,7 @@ TEST( Rot3, equals)
/* ************************************************************************* */
// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
Rot3 slow_but_correct_Rodrigues(const Vector& w) {
double t = norm_2(w);
double t = w.norm();
Matrix3 J = skewSymmetric(w / t);
if (t < 1e-5) return Rot3();
Matrix3 R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
@ -130,7 +130,7 @@ TEST( Rot3, Rodrigues2)
TEST( Rot3, Rodrigues3)
{
Vector w = Vector3(0.1, 0.2, 0.3);
Rot3 R1 = Rot3::AxisAngle(w / norm_2(w), norm_2(w));
Rot3 R1 = Rot3::AxisAngle(w / w.norm(), w.norm());
Rot3 R2 = slow_but_correct_Rodrigues(w);
CHECK(assert_equal(R2,R1));
}
@ -224,16 +224,16 @@ TEST(Rot3, log)
/* ************************************************************************* */
TEST(Rot3, retract_localCoordinates)
{
Vector3 d12 = repeat(3,0.1);
Vector3 d12 = Vector3::Constant(0.1);
Rot3 R2 = R.retract(d12);
EXPECT(assert_equal(d12, R.localCoordinates(R2)));
}
/* ************************************************************************* */
TEST(Rot3, expmap_logmap)
{
Vector3 d12 = repeat(3,0.1);
Vector3 d12 = Vector3::Constant(0.1);
Rot3 R2 = R.expmap(d12);
EXPECT(assert_equal(d12, R.logmap(R2)));
EXPECT(assert_equal(d12, (Vector) R.logmap(R2)));
}
/* ************************************************************************* */

4
gtsam/linear/NoiseModel.cpp
View File

@ -258,7 +258,7 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
if (variances(j) != variances(0)) goto full;
return Isotropic::Variance(n, variances(0), true);
}
full: return shared_ptr(new Diagonal(esqrt(variances)));
full: return shared_ptr(new Diagonal(variances.cwiseSqrt()));
}
/* ************************************************************************* */
@ -326,7 +326,7 @@ static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) {
/* ************************************************************************* */
Constrained::Constrained(const Vector& sigmas)
: Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) {
: Diagonal(sigmas), mu_(Vector::Constant(sigmas.size(), 1000.0)) {
internal::fix(sigmas, precisions_, invsigmas_);
}

24
gtsam/linear/NoiseModel.h
View File

@ -309,7 +309,7 @@ namespace gtsam {
* i.e. the diagonal of the information matrix, i.e., weights
*/
static shared_ptr Precisions(const Vector& precisions, bool smart = true) {
return Variances(reciprocal(precisions), smart);
return Variances(precisions.array().inverse(), smart);
}
virtual void print(const std::string& name) const;
@ -416,7 +416,7 @@ namespace gtsam {
* standard devations, some of which might be zero
*/
static shared_ptr MixedSigmas(const Vector& sigmas) {
return MixedSigmas(repeat(sigmas.size(), 1000.0), sigmas);
return MixedSigmas(Vector::Constant(sigmas.size(), 1000.0), sigmas);
}
/**
@ -424,7 +424,7 @@ namespace gtsam {
* standard devations, some of which might be zero
*/
static shared_ptr MixedSigmas(double m, const Vector& sigmas) {
return MixedSigmas(repeat(sigmas.size(), m), sigmas);
return MixedSigmas(Vector::Constant(sigmas.size(), m), sigmas);
}
/**
@ -432,10 +432,10 @@ namespace gtsam {
* standard devations, some of which might be zero
*/
static shared_ptr MixedVariances(const Vector& mu, const Vector& variances) {
return shared_ptr(new Constrained(mu, esqrt(variances)));
return shared_ptr(new Constrained(mu, variances.cwiseSqrt()));
}
static shared_ptr MixedVariances(const Vector& variances) {
return shared_ptr(new Constrained(esqrt(variances)));
return shared_ptr(new Constrained(variances.cwiseSqrt()));
}
/**
@ -443,10 +443,10 @@ namespace gtsam {
* precisions, some of which might be inf
*/
static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions) {
return MixedVariances(mu, reciprocal(precisions));
return MixedVariances(mu, precisions.array().inverse());
}
static shared_ptr MixedPrecisions(const Vector& precisions) {
return MixedVariances(reciprocal(precisions));
return MixedVariances(precisions.array().inverse());
}
/**
@ -458,17 +458,17 @@ namespace gtsam {
/** Fully constrained variations */
static shared_ptr All(size_t dim) {
return shared_ptr(new Constrained(repeat(dim, 1000.0), repeat(dim,0)));
return shared_ptr(new Constrained(Vector::Constant(dim, 1000.0), Vector::Constant(dim,0)));
}
/** Fully constrained variations */
static shared_ptr All(size_t dim, const Vector& mu) {
return shared_ptr(new Constrained(mu, repeat(dim,0)));
return shared_ptr(new Constrained(mu, Vector::Constant(dim,0)));
}
/** Fully constrained variations with a mu parameter */
static shared_ptr All(size_t dim, double mu) {
return shared_ptr(new Constrained(repeat(dim, mu), repeat(dim,0)));
return shared_ptr(new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0)));
}
virtual void print(const std::string& name) const;
@ -522,10 +522,10 @@ namespace gtsam {
/** protected constructor takes sigma */
Isotropic(size_t dim, double sigma) :
Diagonal(repeat(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
/* dummy constructor to allow for serialization */
Isotropic() : Diagonal(repeat(1, 1.0)),sigma_(1.0),invsigma_(1.0) {}
Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {}
public:

2
gtsam/linear/VectorValues.cpp
View File

@ -52,7 +52,7 @@ namespace gtsam {
Key key;
size_t n;
boost::tie(key, n) = v;
values_.insert(make_pair(key, sub(x, j, j + n)));
values_.insert(make_pair(key, x.segment(j,n)));
j += n;
}
}

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

@ -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(emul(sigmas,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);

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

@ -155,13 +155,13 @@ TEST(NoiseModel, ConstrainedConstructors )
Vector3 mu(200.0, 300.0, 400.0);
actual = Constrained::All(d);
// TODO: why should this be a thousand ??? Dummy variable?
EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu()));
EXPECT(assert_equal(gtsam::repeat(d, 0), actual->sigmas()));
EXPECT(assert_equal(gtsam::repeat(d, 0), actual->invsigmas())); // Actually zero as dummy value
EXPECT(assert_equal(gtsam::repeat(d, 0), actual->precisions())); // Actually zero as dummy value
EXPECT(assert_equal(Vector::Constant(d, 1000.0), actual->mu()));
EXPECT(assert_equal(Vector::Constant(d, 0), actual->sigmas()));
EXPECT(assert_equal(Vector::Constant(d, 0), actual->invsigmas())); // Actually zero as dummy value
EXPECT(assert_equal(Vector::Constant(d, 0), actual->precisions())); // Actually zero as dummy value
actual = Constrained::All(d, m);
EXPECT(assert_equal(gtsam::repeat(d, m), actual->mu()));
EXPECT(assert_equal(Vector::Constant(d, m), actual->mu()));
actual = Constrained::All(d, mu);
EXPECT(assert_equal(mu, actual->mu()));
@ -170,7 +170,7 @@ TEST(NoiseModel, ConstrainedConstructors )
EXPECT(assert_equal(mu, actual->mu()));
actual = Constrained::MixedSigmas(m, sigmas);
EXPECT(assert_equal( gtsam::repeat(d, m), actual->mu()));
EXPECT(assert_equal(Vector::Constant(d, m), actual->mu()));
}
/* ************************************************************************* */

2
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -268,7 +268,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) {
const Vector3 x = thetahat; // parametrization of so(3)
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
double normx = norm_2(x);
double normx = x.norm();
const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X
+ (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X
* X;

2
gtsam/nonlinear/NonlinearEquality.h
View File

@ -153,7 +153,7 @@ public:
throw std::invalid_argument(
"Linearization point not feasible for "
+ DefaultKeyFormatter(this->key()) + "!");
return repeat(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
return Vector::Constant(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
}
}

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

@ -69,16 +69,16 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
double alpha = 0.5;
VectorValues xvalues = map_list_of //
(0, gtsam::repeat(6, 2))//
(1, gtsam::repeat(6, 4))//
(2, gtsam::repeat(6, 0))// distractor
(3, gtsam::repeat(6, 8));
(0, Vector::Constant(6, 2))//
(1, Vector::Constant(6, 4))//
(2, Vector::Constant(6, 0))// distractor
(3, Vector::Constant(6, 8));
VectorValues yExpected = map_list_of//
(0, gtsam::repeat(6, 27))//
(1, gtsam::repeat(6, -40))//
(2, gtsam::repeat(6, 0))// distractor
(3, gtsam::repeat(6, 279));
(0, Vector::Constant(6, 27))//
(1, Vector::Constant(6, -40))//
(2, Vector::Constant(6, 0))// distractor
(3, Vector::Constant(6, 279));
// Create full F
size_t M=4, m = 3, d = 6;

2
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -652,7 +652,7 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
Vector e1 = sfm1.evaluateError(values.at<Camera>(c1), values.at<Point3>(l1));
Vector e2 = sfm2.evaluateError(values.at<Camera>(c2), values.at<Point3>(l1));
double actualError = 0.5
* (norm_2(e1) * norm_2(e1) + norm_2(e2) * norm_2(e2));
* (e1.norm() * e1.norm() + e2.norm() * e2.norm());
double actualErrorGraph = generalGraph.error(values);
DOUBLES_EQUAL(expectedErrorGraph, actualErrorGraph, 1e-7);

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

@ -90,7 +90,7 @@ Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) {
TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
Vector Fu = computeFu(gamma2, u2);
Vector fGravity_g1 = zero(6);
subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)), 3); // gravity force in g1 frame
fGravity_g1.segment<3>(3) = g1.rotation().unrotate(Vector3(0, 0, -mass*9.81)); // gravity force in g1 frame
Vector Fb = Fu+fGravity_g1;
Vector dV = newtonEuler(V1_g1, Fb, Inertia);

2
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -167,7 +167,7 @@ TEST( Similarity3, retract_first_order) {
//******************************************************************************
TEST(Similarity3, localCoordinates_first_order) {
Vector d12 = repeat(7, 0.1);
Vector7 d12 = Vector7::Constant(0.1);
d12(6) = 1.0;
Similarity3 t1 = T1, t2 = t1.retract(d12);
EXPECT(assert_equal(d12, t1.localCoordinates(t2)));

10
gtsam_unstable/slam/AHRS.cpp
View File

@ -32,7 +32,7 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
// estimate standard deviation on gyroscope readings
Pg_ = Cov(stationaryU);
Vector3 sigmas_v_g = esqrt(Pg_.diagonal() * T);
Vector3 sigmas_v_g = (Pg_.diagonal() * T).cwiseSqrt();
// estimate standard deviation on accelerometer readings
Pa_ = Cov(stationaryF);
@ -48,11 +48,11 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
Vector3 var_omega_w = 0 * ones(3); // TODO
Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3);
Vector3 var_omega_a = (0.034 * 0.034) * ones(3);
Vector3 sigmas_v_g_sq = sigmas_v_g.cwiseProduct(sigmas_v_g);
Vector3 sigmas_v_g_sq = sigmas_v_g.array().square();
var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a;
// Quantities needed for aiding
sigmas_v_a_ = esqrt(T * Pa_.diagonal());
sigmas_v_a_ = (T * Pa_.diagonal()).cwiseSqrt();
// gravity in nav frame
n_g_ = (Vector(3) << 0.0, 0.0, g_e).finished();
@ -171,7 +171,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
// calculate residual gravity measurement
z = n_g_ - trans(bRn) * measured_b_g;
H = collect(3, &n_g_cross_, &Z_3x3, &bRn);
R = trans(bRn) * diag(emul(sigmas_v_a_, sigmas_v_a_)) * bRn;
R = trans(bRn) * diag(sigmas_v_a_.array().square()) * bRn;
} else {
// my measurement prediction (in body frame):
// F(:,k) = bias - b_g
@ -186,7 +186,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
Matrix b_g = bRn * n_g_cross_;
H = collect(3, &b_g, &Z_3x3, &I_3x3);
// And the measurement noise, TODO: should be created once where sigmas_v_a is given
R = diag(emul(sigmas_v_a_, sigmas_v_a_));
R = diag(sigmas_v_a_.array().square());
}
// update the Kalman filter

10
gtsam_unstable/slam/Mechanization_bRn2.cpp
View File

@ -33,7 +33,7 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U,
if(g_e == 0) {
if (flat)
// acceleration measured is along the z-axis.
b_g = (Vector3(3) << 0.0, 0.0, norm_2(meanF)).finished();
b_g = (Vector3(3) << 0.0, 0.0, meanF.norm()).finished();
else
// acceleration measured is the opposite of gravity (10.13)
b_g = -meanF;
@ -66,14 +66,14 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U,
}
/* ************************************************************************* */
Mechanization_bRn2 Mechanization_bRn2::correct(const Vector3& dx) const {
Vector3 rho = sub(dx, 0, 3);
Mechanization_bRn2 Mechanization_bRn2::correct(const Vector9& dx) const {
Vector3 rho = dx.segment<3>(0);
Rot3 delta_nRn = Rot3::Rodrigues(rho);
Rot3 bRn = bRn_ * delta_nRn;
Vector3 x_g = x_g_ + sub(dx, 3, 6);
Vector3 x_a = x_a_ + sub(dx, 6, 9);
Vector3 x_g = x_g_ + dx.segment<3>(3);
Vector3 x_a = x_a_ + dx.segment<3>(6);
return Mechanization_bRn2(bRn, x_g, x_a);
}

2
gtsam_unstable/slam/Mechanization_bRn2.h
View File

@ -68,7 +68,7 @@ public:
* @param obj The current state
* @param dx The error used to correct and return a new state
*/
Mechanization_bRn2 correct(const Vector3& dx) const;
Mechanization_bRn2 correct(const Vector9& dx) const;
/**
* Integrate to get new state

16
tests/testBoundingConstraint.cpp
View File

@ -73,8 +73,8 @@ TEST( testBoundingConstraint, unary_basics_inactive2 ) {
EXPECT_DOUBLES_EQUAL(2.0, constraint4.threshold(), tol);
EXPECT(!constraint3.isGreaterThan());
EXPECT(!constraint4.isGreaterThan());
EXPECT(assert_equal(repeat(1, 3.0), constraint3.evaluateError(pt2), tol));
EXPECT(assert_equal(repeat(1, 5.0), constraint4.evaluateError(pt2), tol));
EXPECT(assert_equal(Vector::Constant(1, 3.0), constraint3.evaluateError(pt2), tol));
EXPECT(assert_equal(Vector::Constant(1, 5.0), constraint4.evaluateError(pt2), tol));
EXPECT(assert_equal(zero(1), constraint3.unwhitenedError(config), tol));
EXPECT(assert_equal(zero(1), constraint4.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(0.0, constraint3.error(config), tol);
@ -88,10 +88,10 @@ TEST( testBoundingConstraint, unary_basics_active1 ) {
config.insert(key, pt2);
EXPECT(constraint1.active(config));
EXPECT(constraint2.active(config));
EXPECT(assert_equal(repeat(1,-3.0), constraint1.evaluateError(pt2), tol));
EXPECT(assert_equal(repeat(1,-5.0), constraint2.evaluateError(pt2), tol));
EXPECT(assert_equal(repeat(1,-3.0), constraint1.unwhitenedError(config), tol));
EXPECT(assert_equal(repeat(1,-5.0), constraint2.unwhitenedError(config), tol));
EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.evaluateError(pt2), tol));
EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.evaluateError(pt2), tol));
EXPECT(assert_equal(Vector::Constant(1,-3.0), constraint1.unwhitenedError(config), tol));
EXPECT(assert_equal(Vector::Constant(1,-5.0), constraint2.unwhitenedError(config), tol));
EXPECT_DOUBLES_EQUAL(45.0, constraint1.error(config), tol);
EXPECT_DOUBLES_EQUAL(125.0, constraint2.error(config), tol);
}
@ -129,8 +129,8 @@ TEST( testBoundingConstraint, unary_linearization_active) {
config2.insert(key, pt2);
GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2);
GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2);
JacobianFactor expected1(key, (Matrix(1, 2) << 1.0, 0.0).finished(), repeat(1, 3.0), hard_model1);
JacobianFactor expected2(key, (Matrix(1, 2) << 0.0, 1.0).finished(), repeat(1, 5.0), hard_model1);
JacobianFactor expected1(key, (Matrix(1, 2) << 1.0, 0.0).finished(), Vector::Constant(1, 3.0), hard_model1);
JacobianFactor expected2(key, (Matrix(1, 2) << 0.0, 1.0).finished(), Vector::Constant(1, 5.0), hard_model1);
EXPECT(assert_equal((const GaussianFactor&)expected1, *actual1, tol));
EXPECT(assert_equal((const GaussianFactor&)expected2, *actual2, tol));
}

2
tests/testNonlinearEquality.cpp
View File

@ -137,7 +137,7 @@ TEST ( NonlinearEquality, error ) {
actual = nle->unwhitenedError(bad_linearize);
EXPECT(
assert_equal(actual, repeat(3, std::numeric_limits<double>::infinity())));
assert_equal(actual, Vector::Constant(3, std::numeric_limits<double>::infinity())));
}
//******************************************************************************

4
timing/timeSchurFactors.cpp
View File

@ -55,14 +55,14 @@ void timeAll(size_t m, size_t N) {
Matrix P = (E.transpose() * E).inverse();
// RHS and sigmas
const Vector b = gtsam::repeat(2 * m, 1);
const Vector b = Vector::Constant(2*m,1);
const SharedDiagonal model;
// parameters for multiplyHessianAdd
double alpha = 0.5;
VectorValues xvalues, yvalues;
for (size_t i = 0; i < m; i++)
xvalues.insert(i, gtsam::repeat(D, 2));
xvalues.insert(i, Vector::Constant(D,2));
// Implicit
RegularImplicitSchurFactor<CAMERA> implicitFactor(keys, Fblocks, E, P, b);