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

Got rid of some concats

release/4.3a0
dellaert 2014-11-04 15:44:41 +01:00
parent c332a44c5e
commit efc2dc69fe
3 changed files with 22 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -276,8 +276,8 @@ TEST(HessianFactor, CombineAndEliminate)
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = (Vector(3) << 1.5, 1.5, 1.5);
Vector s0 = (Vector(3) << 1.6, 1.6, 1.6);
Vector3 b0(1.5, 1.5, 1.5);
Vector3 s0(1.6, 1.6, 1.6);
Matrix A10 = (Matrix(3,3) <<
2.0, 0.0, 0.0,
@ -287,15 +287,15 @@ TEST(HessianFactor, CombineAndEliminate)
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = (Vector(3) << 2.5, 2.5, 2.5);
Vector s1 = (Vector(3) << 2.6, 2.6, 2.6);
Vector3 b1(2.5, 2.5, 2.5);
Vector3 s1(2.6, 2.6, 2.6);
Matrix A21 = (Matrix(3,3) <<
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = (Vector(3) << 3.5, 3.5, 3.5);
Vector s2 = (Vector(3) << 3.6, 3.6, 3.6);
Vector3 b2(3.5, 3.5, 3.5);
Vector3 s2(3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@ -305,8 +305,8 @@ TEST(HessianFactor, CombineAndEliminate)
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2);
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
Vector9 b; b << b1, b0, b2;
Vector9 sigmas; sigmas << s1, s0, s2;
// create a full, uneliminated version of the factor
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));

16
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -369,8 +369,8 @@ TEST(JacobianFactor, eliminate)
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = (Vector(3) << 1.5, 1.5, 1.5);
Vector s0 = (Vector(3) << 1.6, 1.6, 1.6);
Vector3 b0(1.5, 1.5, 1.5);
Vector3 s0(1.6, 1.6, 1.6);
Matrix A10 = (Matrix(3, 3) <<
2.0, 0.0, 0.0,
@ -380,15 +380,15 @@ TEST(JacobianFactor, eliminate)
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = (Vector(3) << 2.5, 2.5, 2.5);
Vector s1 = (Vector(3) << 2.6, 2.6, 2.6);
Vector3 b1(2.5, 2.5, 2.5);
Vector3 s1(2.6, 2.6, 2.6);
Matrix A21 = (Matrix(3, 3) <<
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = (Vector(3) << 3.5, 3.5, 3.5);
Vector s2 = (Vector(3) << 3.6, 3.6, 3.6);
Vector3 b2(3.5, 3.5, 3.5);
Vector3 s2(3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@ -398,8 +398,8 @@ TEST(JacobianFactor, eliminate)
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2);
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
Vector9 b; b << b1, b0, b2;
Vector9 sigmas; sigmas << s1, s0, s2;
JacobianFactor combinedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
GaussianFactorGraph::EliminationResult expected = combinedFactor.eliminate(list_of(0));

12
gtsam_unstable/slam/AHRS.cpp
View File

@ -48,12 +48,12 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
F_g_ = -I3 / tau_g;
F_a_ = -I3 / tau_a;
Vector var_omega_w = 0 * ones(3); // TODO
Vector var_omega_g = (0.0034 * 0.0034) * ones(3);
Vector var_omega_a = (0.034 * 0.034) * ones(3);
Vector sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g);
Vector vars = concatVectors(4, &var_omega_w, &var_omega_g, &sigmas_v_g_sq,
&var_omega_a);
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 = emul(sigmas_v_g, sigmas_v_g);
Vector vars(12);
vars << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a;
var_w_ = diag(vars);
// Quantities needed for aiding