From efc2dc69fed0bfab04e377d120658cd28afd7122 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 4 Nov 2014 15:44:41 +0100
Subject: [PATCH] Got rid of some concats

---
 gtsam/linear/tests/testHessianFactor.cpp  | 16 ++++++++--------
 gtsam/linear/tests/testJacobianFactor.cpp | 16 ++++++++--------
 gtsam_unstable/slam/AHRS.cpp              | 12 ++++++------
 3 files changed, 22 insertions(+), 22 deletions(-)

diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp
index 17ad0bf42..90a9cceda 100644
--- a/gtsam/linear/tests/testHessianFactor.cpp
+++ b/gtsam/linear/tests/testHessianFactor.cpp
@@ -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));
diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp
index f70c3496a..1650df0ec 100644
--- a/gtsam/linear/tests/testJacobianFactor.cpp
+++ b/gtsam/linear/tests/testJacobianFactor.cpp
@@ -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));
diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp
index af562c1b2..1719abc86 100644
--- a/gtsam_unstable/slam/AHRS.cpp
+++ b/gtsam_unstable/slam/AHRS.cpp
@@ -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