diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index f3755c180..43814731f 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -70,7 +70,7 @@ int main(int argc, char* argv[]) { /* 2. add factors to the graph */ // add measurement factors - SharedDiagonal measurementNoise = Diagonal::Sigmas((Vector(2) << 0.5, 0.5).finished()); + SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5)); boost::shared_ptr factor; graph.push_back( boost::make_shared(measurementNoise, X(1), calib, diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index b6da40978..84bca65f3 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -118,14 +118,14 @@ int main(int argc, char** argv) { // 2a. Add odometry factors // For simplicity, we will use the same noise model for each odometry factor - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished()); + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses graph.add(BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); // 2b. Add "GPS-like" measurements // We will use our custom UnaryFactor for this. - noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1).finished()); // 10cm std on x,y + noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y graph.add(boost::make_shared(1, 0.0, 0.0, unaryNoise)); graph.add(boost::make_shared(2, 2.0, 0.0, unaryNoise)); graph.add(boost::make_shared(3, 4.0, 0.0, unaryNoise)); diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index 33c217a09..b5cd681e5 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -64,13 +64,13 @@ int main(int argc, char** argv) { // Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1).finished()); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.add(PriorFactor(1, priorMean, priorNoise)); // Add odometry factors Pose2 odometry(2.0, 0.0, 0.0); // For simplicity, we will use the same noise model for each odometry factor - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished()); + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index 7c9e50459..84f9be3a1 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -80,18 +80,18 @@ int main(int argc, char** argv) { // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1).finished()); // 30cm std on x,y, 0.1 rad on theta + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta graph.add(PriorFactor(x1, prior, priorNoise)); // add directly to graph // Add two odometry factors Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished()); // 20cm std on x,y, 0.1 rad on theta + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta graph.add(BetweenFactor(x1, x2, odometry, odometryNoise)); graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); // Add Range-Bearing measurements to two different landmarks // create a noise model for the landmark measurements - noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2).finished()); // 0.1 rad std on bearing, 20cm on range + noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range // create the measurement values - indices are (pose id, landmark id) Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90), diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 76064196d..7991f7fbf 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -71,11 +71,11 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1).finished()); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.add(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); // For simplicity, we will use the same noise model for odometry and loop closures - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished()); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors // Create odometry (Between) factors between consecutive poses diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 2855d2fca..46ca02ee4 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -38,7 +38,7 @@ int main (int argc, char** argv) { // Add a Gaussian prior on first poses Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.01, 0.01, 0.01).finished()); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01)); graph->push_back(PriorFactor(0, priorMean, priorNoise)); // Single Step Optimization using Levenberg-Marquardt diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 939790f7a..818a9e577 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -32,11 +32,11 @@ int main (int argc, char** argv) { NonlinearFactorGraph graph; // 2a. Add a prior on the first pose, setting it to the origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1).finished()); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); // For simplicity, we will use the same noise model for odometry and loop closures - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished()); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors graph.push_back(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 7e522f16e..4422586b0 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -68,12 +68,12 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 prior(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1).finished()); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, prior, priorNoise)); // 2b. Add odometry factors // For simplicity, we will use the same noise model for each odometry factor - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished()); + noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // Create odometry (Between) factors between consecutive poses graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); @@ -85,7 +85,7 @@ int main(int argc, char** argv) { // these constraints may be identified in many ways, such as appearance-based techniques // with camera images. // We will use another Between Factor to enforce this constraint, with the distance set to zero, - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished()); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.push_back(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); graph.print("\nFactor Graph:\n"); // print diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index be31e5802..201ec188b 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -37,7 +37,7 @@ int main() { // Create the Kalman Filter initialization point Point2 x_initial(0.0, 0.0); - SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1).finished()); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // Create Key for initial pose Symbol x0('x',0); @@ -57,8 +57,8 @@ int main() { // For the purposes of this example, let us assume we are using a constant-position model and // the controls are driving the point to the right at 1 m/s. Then, F = [1 0 ; 0 1], B = [1 0 ; 0 1] // and u = [1 ; 0]. Let us also assume that the process noise Q = [0.1 0 ; 0 0.1]. - Vector u = (Vector(2) << 1.0, 0.0).finished(); - SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1).finished(), true); + Vector u = Vector2(1.0, 0.0); + SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1), true); // This simple motion can be modeled with a BetweenFactor // Create Key for next pose @@ -83,7 +83,7 @@ int main() { // For the purposes of this example, let us assume we have something like a GPS that returns // the current position of the robot. Then H = [1 0 ; 0 1]. Let us also assume that the measurement noise // R = [0.25 0 ; 0 0.25]. - SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vector(2) << 0.25, 0.25).finished(), true); + SharedDiagonal R = noiseModel::Diagonal::Sigmas(Vector2(0.25, 0.25), true); // This simple measurement can be modeled with a PriorFactor Point2 z1(1.0, 0.0); diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index 7bb291a2f..c1dface2e 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -27,7 +27,7 @@ GTSAM_CONCEPT_LIE_INST(LieVector) /* ************************************************************************* */ TEST( testLieVector, construction ) { - Vector v = (Vector(3) << 1.0, 2.0, 3.0).finished(); + Vector v = Vector3(1.0, 2.0, 3.0); LieVector lie1(v), lie2(v); EXPECT(lie1.dim() == 3); @@ -37,7 +37,7 @@ TEST( testLieVector, construction ) { /* ************************************************************************* */ TEST( testLieVector, other_constructors ) { - Vector init = (Vector(2) << 10.0, 20.0).finished(); + Vector init = Vector2(10.0, 20.0); LieVector exp(init); double data[] = {10,20}; LieVector b(2,data); diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index df70324eb..5b36d2b02 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -400,7 +400,7 @@ TEST( matrix, scale_rows ) A(2, 2) = 1.; A(2, 3) = 1.; - Vector v = (Vector(3) << 2., 3., 4.).finished(); + Vector v = Vector3(2., 3., 4.); Matrix actual = vector_scale(v, A); @@ -618,9 +618,9 @@ TEST( matrix, matrix_vector_multiplication ) Vector result(2); Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); - Vector v = (Vector(3) << 1., 2., 3.).finished(); - Vector Av = (Vector(2) << 14., 32.).finished(); - Vector AtAv = (Vector(3) << 142., 188., 234.).finished(); + Vector v = Vector3(1., 2., 3.); + Vector Av = Vector2(14., 32.); + Vector AtAv = Vector3(142., 188., 234.); EQUALITY(A*v,Av); EQUALITY(A^Av,AtAv); @@ -787,19 +787,19 @@ TEST( matrix, inverse2 ) TEST( matrix, backsubtitution ) { // TEST ONE 2x2 matrix U1*x=b1 - Vector expected1 = (Vector(2) << 3.6250, -0.75).finished(); + Vector expected1 = Vector2(3.6250, -0.75); Matrix U22 = (Matrix(2, 2) << 2., 3., 0., 4.).finished(); Vector b1 = U22 * expected1; EXPECT( assert_equal(expected1 , backSubstituteUpper(U22, b1), 0.000001)); // TEST TWO 3x3 matrix U2*x=b2 - Vector expected2 = (Vector(3) << 5.5, -8.5, 5.).finished(); + Vector expected2 = Vector3(5.5, -8.5, 5.); Matrix U33 = (Matrix(3, 3) << 3., 5., 6., 0., 2., 3., 0., 0., 1.).finished(); Vector b2 = U33 * expected2; EXPECT( assert_equal(expected2 , backSubstituteUpper(U33, b2), 0.000001)); // TEST THREE Lower triangular 3x3 matrix L3*x=b3 - Vector expected3 = (Vector(3) << 1., 1., 1.).finished(); + Vector expected3 = Vector3(1., 1., 1.); Matrix L3 = trans(U33); Vector b3 = L3 * expected3; EXPECT( assert_equal(expected3 , backSubstituteLower(L3, b3), 0.000001)); @@ -1054,7 +1054,7 @@ TEST( matrix, cholesky_inverse ) TEST( matrix, multiplyAdd ) { Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); - Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = (Vector(3) << 5., 6., 7.).finished(), + Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), expected = e + A * x; multiplyAdd(1, A, x, e); @@ -1065,7 +1065,7 @@ TEST( matrix, multiplyAdd ) TEST( matrix, transposeMultiplyAdd ) { Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); - Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = (Vector(3) << 5., 6., 7.).finished(), + Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), expected = x + trans(A) * e; transposeMultiplyAdd(1, A, e, x); @@ -1099,7 +1099,7 @@ TEST( matrix, linear_dependent3 ) /* ************************************************************************* */ TEST( matrix, svd1 ) { - Vector v = (Vector(3) << 2., 1., 0.).finished(); + Vector v = Vector3(2., 1., 0.); Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1) * Matrix(trans(V1)); Matrix U, V; @@ -1122,7 +1122,7 @@ TEST( matrix, svd2 ) Vector s; Matrix expectedU = (Matrix(3, 2) << 0.,-1.,0.,0.,1.,0.).finished(); - Vector expected_s = (Vector(2) << 3.,2.).finished(); + Vector expected_s = Vector2(3.,2.); Matrix expectedV = (Matrix(2, 2) << 1.,0.,0.,1.).finished(); svd(sampleA, U, s, V); @@ -1145,7 +1145,7 @@ TEST( matrix, svd3 ) Vector s; Matrix expectedU = (Matrix(2, 2) << -1.,0.,0.,-1.).finished(); - Vector expected_s = (Vector(2) << 3.0, 2.0).finished(); + Vector expected_s = Vector2(3.0, 2.0); Matrix expectedV = (Matrix(3, 2) << 0.,1.,0.,0.,-1.,0.).finished(); svd(sampleAt, U, s, V); @@ -1182,7 +1182,7 @@ TEST( matrix, svd4 ) 0.6659, -0.7370, 0.0970, -0.0689).finished(); - Vector expected_s = (Vector(2) << 1.6455, 0.1910).finished(); + Vector expected_s = Vector2(1.6455, 0.1910); Matrix expectedV = (Matrix(2, 2) << 0.7403, -0.6723, diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp index 5aaf779ca..fceb2f4b4 100644 --- a/gtsam/base/tests/testSerializationBase.cpp +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -30,9 +30,9 @@ using namespace std; using namespace gtsam; using namespace gtsam::serializationTestHelpers; -Vector v1 = (Vector(2) << 1.0, 2.0).finished(); -Vector v2 = (Vector(2) << 3.0, 4.0).finished(); -Vector v3 = (Vector(2) << 5.0, 6.0).finished(); +Vector v1 = Vector2(1.0, 2.0); +Vector v2 = Vector2(3.0, 4.0); +Vector v3 = Vector2(5.0, 6.0); /* ************************************************************************* */ TEST (Serialization, FastList) { diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 9eac8136b..460ff6cd6 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -47,17 +47,17 @@ TEST( TestVector, special_comma_initializer) expected(1) = 2; expected(2) = 3; - Vector actual1 = (Vector(3) << 1, 2, 3).finished(); - Vector actual2((Vector(3) << 1, 2, 3).finished()); + Vector actual1 = Vector3(1, 2, 3); + Vector actual2(Vector3(1, 2, 3)); - Vector subvec1 = (Vector(2) << 2, 3).finished(); + Vector subvec1 = Vector2(2, 3); Vector actual4 = (Vector(3) << 1, subvec1).finished(); - Vector subvec2 = (Vector(2) << 1, 2).finished(); + Vector subvec2 = Vector2(1, 2); Vector actual5 = (Vector(3) << subvec2, 3).finished(); - Vector actual6 = testFcn1((Vector(3) << 1, 2, 3).finished()); - Vector actual7 = testFcn2((Vector(3) << 1, 2, 3).finished()); + Vector actual6 = testFcn1(Vector3(1, 2, 3)); + Vector actual7 = testFcn2(Vector3(1, 2, 3)); EXPECT(assert_equal(expected, actual1)); EXPECT(assert_equal(expected, actual2)); @@ -254,31 +254,31 @@ TEST( TestVector, weightedPseudoinverse_nan ) /* ************************************************************************* */ TEST( TestVector, ediv ) { - Vector a = (Vector(3) << 10., 20., 30.).finished(); - Vector b = (Vector(3) << 2.0, 5.0, 6.0).finished(); + Vector a = Vector3(10., 20., 30.); + Vector b = Vector3(2.0, 5.0, 6.0); Vector actual(ediv(a,b)); - Vector c = (Vector(3) << 5.0, 4.0, 5.0).finished(); + Vector c = Vector3(5.0, 4.0, 5.0); EXPECT(assert_equal(c,actual)); } /* ************************************************************************* */ TEST( TestVector, dot ) { - Vector a = (Vector(3) << 10., 20., 30.).finished(); - Vector b = (Vector(3) << 2.0, 5.0, 6.0).finished(); + Vector a = Vector3(10., 20., 30.); + Vector b = Vector3(2.0, 5.0, 6.0); DOUBLES_EQUAL(20+100+180,dot(a,b),1e-9); } /* ************************************************************************* */ TEST( TestVector, axpy ) { - Vector x = (Vector(3) << 10., 20., 30.).finished(); - Vector y0 = (Vector(3) << 2.0, 5.0, 6.0).finished(); + Vector x = Vector3(10., 20., 30.); + Vector y0 = Vector3(2.0, 5.0, 6.0); Vector y1 = y0, y2 = y0; axpy(0.1,x,y1); axpy(0.1,x,y2.head(3)); - Vector expected = (Vector(3) << 3.0, 7.0, 9.0).finished(); + Vector expected = Vector3(3.0, 7.0, 9.0); EXPECT(assert_equal(expected,y1)); EXPECT(assert_equal(expected,Vector(y2))); } @@ -295,7 +295,7 @@ TEST( TestVector, equals ) /* ************************************************************************* */ TEST( TestVector, greater_than ) { - Vector v1 = (Vector(3) << 1.0, 2.0, 3.0).finished(), + Vector v1 = Vector3(1.0, 2.0, 3.0), v2 = zero(3); EXPECT(greaterThanOrEqual(v1, v1)); // test basic greater than EXPECT(greaterThanOrEqual(v1, v2)); // test equals @@ -304,31 +304,31 @@ TEST( TestVector, greater_than ) /* ************************************************************************* */ TEST( TestVector, reciprocal ) { - Vector v = (Vector(3) << 1.0, 2.0, 4.0).finished(); - EXPECT(assert_equal((Vector(3) << 1.0, 0.5, 0.25).finished(),reciprocal(v))); + Vector v = Vector3(1.0, 2.0, 4.0); + EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25),reciprocal(v))); } /* ************************************************************************* */ TEST( TestVector, linear_dependent ) { - Vector v1 = (Vector(3) << 1.0, 2.0, 3.0).finished(); - Vector v2 = (Vector(3) << -2.0, -4.0, -6.0).finished(); + Vector v1 = Vector3(1.0, 2.0, 3.0); + Vector v2 = Vector3(-2.0, -4.0, -6.0); EXPECT(linear_dependent(v1, v2)); } /* ************************************************************************* */ TEST( TestVector, linear_dependent2 ) { - Vector v1 = (Vector(3) << 0.0, 2.0, 0.0).finished(); - Vector v2 = (Vector(3) << 0.0, -4.0, 0.0).finished(); + Vector v1 = Vector3(0.0, 2.0, 0.0); + Vector v2 = Vector3(0.0, -4.0, 0.0); EXPECT(linear_dependent(v1, v2)); } /* ************************************************************************* */ TEST( TestVector, linear_dependent3 ) { - Vector v1 = (Vector(3) << 0.0, 2.0, 0.0).finished(); - Vector v2 = (Vector(3) << 0.1, -4.1, 0.0).finished(); + Vector v1 = Vector3(0.0, 2.0, 0.0); + Vector v2 = Vector3(0.1, -4.1, 0.0); EXPECT(!linear_dependent(v1, v2)); } diff --git a/gtsam/discrete/tests/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp index 240f09cc2..24bec4fa1 100644 --- a/gtsam/discrete/tests/testDiscreteMarginals.cpp +++ b/gtsam/discrete/tests/testDiscreteMarginals.cpp @@ -53,10 +53,10 @@ TEST_UNSAFE( DiscreteMarginals, UGM_small ) { EXPECT_DOUBLES_EQUAL( 0.359631, (*actualC)(values), 1e-6); Vector actualCvector = marginals.marginalProbabilities(Cathy); - EXPECT(assert_equal((Vector(2) << 0.359631, 0.640369).finished(), actualCvector, 1e-6)); + EXPECT(assert_equal(Vector2(0.359631, 0.640369), actualCvector, 1e-6)); actualCvector = marginals.marginalProbabilities(Mark); - EXPECT(assert_equal((Vector(2) << 0.48628, 0.51372).finished(), actualCvector, 1e-6)); + EXPECT(assert_equal(Vector2(0.48628, 0.51372), actualCvector, 1e-6)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index ff22d883c..5e13129cc 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -162,7 +162,7 @@ Vector Unit3::localCoordinates(const Unit3& y) const { // Check for special cases if (std::abs(dot - 1.0) < 1e-16) - return (Vector(2) << 0, 0).finished(); + return Vector2(0, 0); else if (std::abs(dot + 1.0) < 1e-16) return (Vector(2) << M_PI, 0).finished(); else { diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 1b67e4af3..d2990a747 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -62,7 +62,7 @@ TEST (EssentialMatrix, retract0) { //************************************************************************* TEST (EssentialMatrix, retract1) { - EssentialMatrix expected(c1Rc2.retract((Vector(3) << 0.1, 0, 0).finished()), Unit3(c1Tc2)); + EssentialMatrix expected(c1Rc2.retract(Vector3(0.1, 0, 0)), Unit3(c1Tc2)); EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0).finished()); EXPECT(assert_equal(expected, actual)); } @@ -70,7 +70,7 @@ TEST (EssentialMatrix, retract1) { //************************************************************************* TEST (EssentialMatrix, retract2) { EssentialMatrix expected(c1Rc2, - Unit3(c1Tc2).retract((Vector(2) << 0.1, 0).finished())); + Unit3(c1Tc2).retract(Vector2(0.1, 0))); EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0).finished()); EXPECT(assert_equal(expected, actual)); } @@ -85,7 +85,7 @@ TEST (EssentialMatrix, transform_to) { * Rot3::roll(M_PI / 6.0); Point3 aTb2(19.2, 3.7, 5.9); EssentialMatrix E(aRb2, Unit3(aTb2)); - //EssentialMatrix E(aRb, Unit3(aTb).retract((Vector(2) << 0.1, 0).finished())); + //EssentialMatrix E(aRb, Unit3(aTb).retract(Vector2(0.1, 0))); static Point3 P(0.2, 0.7, -2); Matrix actH1, actH2; E.transform_to(P, actH1, actH2); diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 3c0b15252..ab40928ab 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -46,8 +46,8 @@ TEST(Point2, Lie) { EXPECT(assert_equal(-eye(2), H1)); EXPECT(assert_equal(eye(2), H2)); - EXPECT(assert_equal(Point2(5,7), p1.retract((Vector(2) << 4., 5.).finished()))); - EXPECT(assert_equal((Vector(2) << 3.,3.).finished(), p1.localCoordinates(p2))); + EXPECT(assert_equal(Point2(5,7), p1.retract(Vector2(4., 5.)))); + EXPECT(assert_equal(Vector2(3.,3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 611db57e1..24c2bab49 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -40,8 +40,8 @@ TEST(Point3, Lie) { EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(5, 7, 9), p1.retract((Vector(3) << 4., 5., 6.).finished()))); - EXPECT(assert_equal((Vector)(Vector(3) << 3.,3.,3.).finished(), p1.localCoordinates(p2))); + EXPECT(assert_equal(Point3(5, 7, 9), p1.retract(Vector3(4., 5., 6.)))); + EXPECT(assert_equal((Vector)Vector3(3.,3.,3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 2cfc389b8..b4de6eb7c 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -68,7 +68,7 @@ TEST(Pose2, retract) { #else Pose2 expected(M_PI/2.0+0.99, Point2(1.015, 2.01)); #endif - Pose2 actual = pose.retract((Vector(3) << 0.01, -0.015, 0.99).finished()); + Pose2 actual = pose.retract(Vector3(0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -76,7 +76,7 @@ TEST(Pose2, retract) { TEST(Pose2, expmap) { Pose2 pose(M_PI/2.0, Point2(1, 2)); Pose2 expected(1.00811, 2.01528, 2.5608); - Pose2 actual = expmap_default(pose, (Vector(3) << 0.01, -0.015, 0.99).finished()); + Pose2 actual = expmap_default(pose, Vector3(0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -84,7 +84,7 @@ TEST(Pose2, expmap) { TEST(Pose2, expmap2) { Pose2 pose(M_PI/2.0, Point2(1, 2)); Pose2 expected(1.00811, 2.01528, 2.5608); - Pose2 actual = expmap_default(pose, (Vector(3) << 0.01, -0.015, 0.99).finished()); + Pose2 actual = expmap_default(pose, Vector3(0.01, -0.015, 0.99)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -99,7 +99,7 @@ TEST(Pose2, expmap3) { Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0; Matrix expected = eye(3) + A + A2 + A3 + A4; - Vector v = (Vector(3) << 0.01, -0.015, 0.99).finished(); + Vector v = Vector3(0.01, -0.015, 0.99); Pose2 pose = Pose2::Expmap(v); Pose2 pose2(v); EXPECT(assert_equal(pose, pose2)); @@ -110,7 +110,7 @@ TEST(Pose2, expmap3) { /* ************************************************************************* */ TEST(Pose2, expmap0a) { Pose2 expected(0.0101345, -0.0149092, 0.018); - Pose2 actual = Pose2::Expmap((Vector(3) << 0.01, -0.015, 0.018).finished()); + Pose2 actual = Pose2::Expmap(Vector3(0.01, -0.015, 0.018)); EXPECT(assert_equal(expected, actual, 1e-5)); } @@ -175,9 +175,9 @@ TEST(Pose2, logmap) { Pose2 pose0(M_PI/2.0, Point2(1, 2)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01)); #ifdef SLOW_BUT_CORRECT_EXPMAP - Vector expected = (Vector(3) << 0.00986473, -0.0150896, 0.018).finished(); + Vector expected = Vector3(0.00986473, -0.0150896, 0.018); #else - Vector expected = (Vector(3) << 0.01, -0.015, 0.018).finished(); + Vector expected = Vector3(0.01, -0.015, 0.018); #endif Vector actual = pose0.localCoordinates(pose); EXPECT(assert_equal(expected, actual, 1e-5)); @@ -187,7 +187,7 @@ TEST(Pose2, logmap) { TEST(Pose2, logmap_full) { Pose2 pose0(M_PI/2.0, Point2(1, 2)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01)); - Vector expected = (Vector(3) << 0.00986473, -0.0150896, 0.018).finished(); + Vector expected = Vector3(0.00986473, -0.0150896, 0.018); Vector actual = logmap_default(pose0, pose); EXPECT(assert_equal(expected, actual, 1e-5)); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 263e8cdf3..2737aa38a 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -102,7 +102,7 @@ TEST( Rot3, rodriguez) /* ************************************************************************* */ TEST( Rot3, rodriguez2) { - Vector axis = (Vector(3) << 0., 1., 0.).finished(); // rotation around Y + Vector axis = Vector3(0., 1., 0.); // rotation around Y double angle = 3.14 / 4.0; Rot3 actual = Rot3::rodriguez(axis, angle); Rot3 expected(0.707388, 0, 0.706825, @@ -114,7 +114,7 @@ TEST( Rot3, rodriguez2) /* ************************************************************************* */ TEST( Rot3, rodriguez3) { - Vector w = (Vector(3) << 0.1, 0.2, 0.3).finished(); + Vector w = Vector3(0.1, 0.2, 0.3); Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); Rot3 R2 = slow_but_correct_rodriguez(w); CHECK(assert_equal(R2,R1)); @@ -123,7 +123,7 @@ TEST( Rot3, rodriguez3) /* ************************************************************************* */ TEST( Rot3, rodriguez4) { - Vector axis = (Vector(3) << 0., 0., 1.).finished(); // rotation around Z + Vector axis = Vector3(0., 0., 1.); // rotation around Z double angle = M_PI/2.0; Rot3 actual = Rot3::rodriguez(axis, angle); double c=cos(angle),s=sin(angle); @@ -256,7 +256,7 @@ TEST(Rot3, manifold_expmap) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = (Vector(3) << 0.1, 0.2, 0.3).finished(); + Vector d = Vector3(0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -393,7 +393,7 @@ TEST( Rot3, between ) } /* ************************************************************************* */ -Vector w = (Vector(3) << 0.1, 0.27, -0.2).finished(); +Vector w = Vector3(0.1, 0.27, -0.2); // Left trivialization Derivative of exp(w) wrpt w: // How does exp(w) change when w changes? @@ -462,7 +462,7 @@ TEST( Rot3, yaw_pitch_roll ) Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3); CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3))); - CHECK(assert_equal((Vector)(Vector(3) << 0.1, 0.2, 0.3).finished(),expected.ypr())); + CHECK(assert_equal((Vector)Vector3(0.1, 0.2, 0.3),expected.ypr())); } /* ************************************************************************* */ @@ -472,22 +472,22 @@ TEST( Rot3, RQ) Matrix actualK; Vector actual; boost::tie(actualK, actual) = RQ(R.matrix()); - Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671).finished(); + Vector expected = Vector3(0.14715, 0.385821, 0.231671); CHECK(assert_equal(I3,actualK)); CHECK(assert_equal(expected,actual,1e-6)); // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] CHECK(assert_equal(expected,R.xyz(),1e-6)); - CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3).finished(),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); + CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3).finished(),Rot3::ypr(0.1,0.2,0.3).ypr())); - CHECK(assert_equal((Vector)(Vector(3) << 0.3,0.2,0.1).finished(),Rot3::ypr(0.1,0.2,0.3).rpy())); + CHECK(assert_equal((Vector)Vector3(0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); + CHECK(assert_equal((Vector)Vector3(0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.0,0.0).finished(),Rot3::yaw (0.1).ypr())); - CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.1,0.0).finished(),Rot3::pitch(0.1).ypr())); - CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.0,0.1).finished(),Rot3::roll (0.1).ypr())); + CHECK(assert_equal((Vector)Vector3(0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); + CHECK(assert_equal((Vector)Vector3(0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); + CHECK(assert_equal((Vector)Vector3(0.0,0.0,0.1),Rot3::roll (0.1).ypr())); // Try RQ to recover calibration from 3*3 sub-block of projection matrix Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0).finished(); diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index b3d9d523b..1313a3be5 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -56,7 +56,7 @@ TEST(Rot3, manifold_cayley) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = (Vector(3) << 0.1, 0.2, 0.3).finished(); + Vector d = Vector3(0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) @@ -84,7 +84,7 @@ TEST(Rot3, manifold_slow_cayley) CHECK(assert_equal(d12,-d21)); // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = (Vector(3) << 0.1, 0.2, 0.3).finished(); + Vector d = Vector3(0.1, 0.2, 0.3); // exp(-d)=inverse(exp(d)) CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index f8fef39a8..8e504ba0e 100644 --- a/gtsam/geometry/tests/testStereoPoint2.cpp +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -44,8 +44,8 @@ TEST(StereoPoint2, Lie) { EXPECT(assert_equal(StereoPoint2(3,3,3), p1.between(p2))); - EXPECT(assert_equal(StereoPoint2(5,7,9), p1.retract((Vector(3) << 4., 5., 6.).finished()))); - EXPECT(assert_equal((Vector(3) << 3., 3., 3.).finished(), p1.localCoordinates(p2))); + EXPECT(assert_equal(StereoPoint2(5,7,9), p1.retract(Vector3(4., 5., 6.)))); + EXPECT(assert_equal(Vector3(3., 3., 3.), p1.localCoordinates(p2))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index a5ea14c1b..0102477b3 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -106,11 +106,11 @@ TEST(Unit3, unrotate) { //******************************************************************************* TEST(Unit3, error) { - Unit3 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0).finished()), // - r = p.retract((Vector(2) << 0.8, 0).finished()); - EXPECT(assert_equal((Vector(2) << 0, 0).finished(), p.error(p), 1e-8)); - EXPECT(assert_equal((Vector(2) << 0.479426, 0).finished(), p.error(q), 1e-5)); - EXPECT(assert_equal((Vector(2) << 0.717356, 0).finished(), p.error(r), 1e-5)); + Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // + r = p.retract(Vector2(0.8, 0)); + EXPECT(assert_equal(Vector2(0, 0), p.error(p), 1e-8)); + EXPECT(assert_equal(Vector2(0.479426, 0), p.error(q), 1e-5)); + EXPECT(assert_equal(Vector2(0.717356, 0), p.error(r), 1e-5)); Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian @@ -130,8 +130,8 @@ TEST(Unit3, error) { //******************************************************************************* TEST(Unit3, distance) { - Unit3 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0).finished()), // - r = p.retract((Vector(2) << 0.8, 0).finished()); + Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // + r = p.retract(Vector2(0.8, 0)); EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8); EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-8); EXPECT_DOUBLES_EQUAL(0.71735609089952279, p.distance(r), 1e-8); @@ -228,9 +228,9 @@ inline static Vector randomVector(const Vector& minLimits, TEST(Unit3, localCoordinates_retract) { size_t numIterations = 10000; - Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0).finished(), maxSphereLimit = - (Vector(3) << 1.0, 1.0, 1.0).finished(); - Vector minXiLimit = (Vector(2) << -1.0, -1.0).finished(), maxXiLimit = (Vector(2) << 1.0, 1.0).finished(); + Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit = + Vector3(1.0, 1.0, 1.0); + Vector minXiLimit = Vector2(-1.0, -1.0), maxXiLimit = Vector2(1.0, 1.0); for (size_t i = 0; i < numIterations; i++) { // Sleep for the random number generator (TODO?: Better create all of them first). @@ -258,8 +258,8 @@ TEST(Unit3, localCoordinates_retract) { TEST(Unit3, localCoordinates_retract_expmap) { size_t numIterations = 10000; - Vector minSphereLimit = (Vector(3) << -1.0, -1.0, -1.0).finished(), maxSphereLimit = - (Vector(3) << 1.0, 1.0, 1.0).finished(); + Vector minSphereLimit = Vector3(-1.0, -1.0, -1.0), maxSphereLimit = + Vector3(1.0, 1.0, 1.0); Vector minXiLimit = (Vector(2) << -M_PI, -M_PI).finished(), maxXiLimit = (Vector(2) << M_PI, M_PI).finished(); for (size_t i = 0; i < numIterations; i++) { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index d12d23041..290249b68 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -49,15 +49,15 @@ namespace gtsam { * Example: * \code VectorValues values; - values.insert(3, (Vector(3) << 1.0, 2.0, 3.0).finished()); - values.insert(4, (Vector(2) << 4.0, 5.0).finished()); + values.insert(3, Vector3(1.0, 2.0, 3.0)); + values.insert(4, Vector2(4.0, 5.0)); values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished()); // Prints [ 3.0 4.0 ] gtsam::print(values[1]); // Prints [ 8.0 9.0 ] - values[1] = (Vector(2) << 8.0, 9.0).finished(); + values[1] = Vector2(8.0, 9.0); gtsam::print(values[1]); \endcode * diff --git a/gtsam/linear/tests/testErrors.cpp b/gtsam/linear/tests/testErrors.cpp index 4133f4769..9c8eb468d 100644 --- a/gtsam/linear/tests/testErrors.cpp +++ b/gtsam/linear/tests/testErrors.cpp @@ -29,12 +29,12 @@ using namespace gtsam; TEST( Errors, arithmetic ) { Errors e; - e += (Vector(2) << 1.0,2.0).finished(), (Vector(3) << 3.0,4.0,5.0).finished(); + e += Vector2(1.0,2.0), Vector3(3.0,4.0,5.0); DOUBLES_EQUAL(1+4+9+16+25,dot(e,e),1e-9); axpy(2.0,e,e); Errors expected; - expected += (Vector(2) << 3.0,6.0).finished(), (Vector(3) << 9.0,12.0,15.0).finished(); + expected += Vector2(3.0,6.0), Vector3(9.0,12.0,15.0); CHECK(assert_equal(expected,e)); } diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index a8d86e94b..fb3884542 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -51,7 +51,7 @@ TEST( GaussianBayesNet, matrix ) 1.0, 1.0, 0.0, 1.0 ).finished(); - Vector d1 = (Vector(2) << 9.0, 5.0).finished(); + Vector d1 = Vector2(9.0, 5.0); EXPECT(assert_equal(R,R1)); EXPECT(assert_equal(d,d1)); @@ -131,15 +131,15 @@ TEST( GaussianBayesNet, DeterminantTest ) { GaussianBayesNet cbn; cbn += GaussianConditional( - 0, (Vector(2) << 3.0, 4.0 ).finished(), (Matrix(2, 2) << 1.0, 3.0, 0.0, 4.0 ).finished(), + 0, Vector2(3.0, 4.0 ), (Matrix(2, 2) << 1.0, 3.0, 0.0, 4.0 ).finished(), 1, (Matrix(2, 2) << 2.0, 1.0, 2.0, 3.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); cbn += GaussianConditional( - 1, (Vector(2) << 5.0, 6.0 ).finished(), (Matrix(2, 2) << 1.0, 1.0, 0.0, 3.0 ).finished(), + 1, Vector2(5.0, 6.0 ), (Matrix(2, 2) << 1.0, 1.0, 0.0, 3.0 ).finished(), 2, (Matrix(2, 2) << 1.0, 0.0, 5.0, 2.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); cbn += GaussianConditional( - 3, (Vector(2) << 7.0, 8.0 ).finished(), (Matrix(2, 2) << 1.0, 1.0, 0.0, 5.0 ).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); + 3, Vector2(7.0, 8.0 ), (Matrix(2, 2) << 1.0, 1.0, 0.0, 5.0 ).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); double expectedDeterminant = 60.0 / 64.0; double actualDeterminant = cbn.determinant(); @@ -163,21 +163,21 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 0, (Vector(2) << 1.0,2.0).finished(), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(), + 0, Vector2(1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(), 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(), 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 1, (Vector(2) << 15.0,16.0).finished(), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(), + 1, Vector2(15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(), 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(), 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 2, (Vector(2) << 29.0,30.0).finished(), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(), + 2, Vector2(29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(), 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 3, (Vector(2) << 39.0,40.0).finished(), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(), + 3, Vector2(39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(), 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, (Vector(2) << 49.0,50.0).finished(), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished())); + 4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished())); // Compute the Hessian numerically Matrix hessian = numericalHessian( diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 101ce9834..78fe5187a 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -88,7 +88,7 @@ TEST( GaussianBayesTree, eliminate ) GaussianBayesTree bayesTree_expected; bayesTree_expected.insertRoot( - MakeClique(GaussianConditional(pair_list_of(x3, (Matrix(2, 1) << 2., 0.).finished()) (x4, (Matrix(2, 1) << 2., 2.).finished()), 2, (Vector(2) << 2., 2.).finished()), list_of + MakeClique(GaussianConditional(pair_list_of(x3, (Matrix(2, 1) << 2., 0.).finished()) (x4, (Matrix(2, 1) << 2., 2.).finished()), 2, Vector2(2., 2.)), list_of (MakeClique(GaussianConditional(pair_list_of(x2, (Matrix(2, 1) << -2.*sqrt(2.), 0.).finished()) (x1, (Matrix(2, 1) << -sqrt(2.), -sqrt(2.)).finished()) (x3, (Matrix(2, 1) << -sqrt(2.), sqrt(2.)).finished()), 2, (Vector(2) << -2.*sqrt(2.), 0.).finished()))))); EXPECT(assert_equal(bayesTree_expected, bt)); @@ -115,16 +115,16 @@ TEST(GaussianBayesTree, complicatedMarginal) { bt.insertRoot( MakeClique(GaussianConditional(pair_list_of (11, (Matrix(3,1) << 0.0971, 0, 0).finished()) (12, (Matrix(3,2) << 0.3171, 0.4387, 0.9502, 0.3816, 0, 0.7655).finished()), - 2, (Vector(3) << 0.2638, 0.1455, 0.1361).finished()), list_of + 2, Vector3(0.2638, 0.1455, 0.1361)), list_of (MakeClique(GaussianConditional(pair_list_of (9, (Matrix(3,1) << 0.7952, 0, 0).finished()) (10, (Matrix(3,2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797).finished()) (11, (Matrix(3,1) << 0.6551, 0.1626, 0.1190).finished()) (12, (Matrix(3,2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513).finished()), - 2, (Vector(3) << 0.4314, 0.9106, 0.1818).finished()))) + 2, Vector3(0.4314, 0.9106, 0.1818)))) (MakeClique(GaussianConditional(pair_list_of (7, (Matrix(3,1) << 0.2551, 0, 0).finished()) (8, (Matrix(3,2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575).finished()) (11, (Matrix(3,1) << 0.8407, 0.2543, 0.8143).finished()), - 2, (Vector(3) << 0.3998, 0.2599, 0.8001).finished()), list_of + 2, Vector3(0.3998, 0.2599, 0.8001)), list_of (MakeClique(GaussianConditional(pair_list_of (5, (Matrix(3,1) << 0.2435, 0, 0).finished()) (6, (Matrix(3,2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308, 0.0).finished()) // NOTE the non-upper-triangular form @@ -134,15 +134,15 @@ TEST(GaussianBayesTree, complicatedMarginal) { // redone if this stops working in the future (7, (Matrix(3,1) << 0.5853, 0.5497, 0.9172).finished()) (8, (Matrix(3,2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759).finished()), - 2, (Vector(3) << 0.8173, 0.8687, 0.0844).finished()), list_of + 2, Vector3(0.8173, 0.8687, 0.0844)), list_of (MakeClique(GaussianConditional(pair_list_of (3, (Matrix(3,1) << 0.0540, 0, 0).finished()) (4, (Matrix(3,2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371).finished()) (6, (Matrix(3,2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020).finished()), - 2, (Vector(3) << 0.9619, 0.0046, 0.7749).finished()))) + 2, Vector3(0.9619, 0.0046, 0.7749)))) (MakeClique(GaussianConditional(pair_list_of (1, (Matrix(3,1) << 0.2630, 0, 0).finished()) (2, (Matrix(3,2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524).finished()) (5, (Matrix(3,1) << 0.8258, 0.5383, 0.9961).finished()), - 2, (Vector(3) << 0.0782, 0.4427, 0.1067).finished()))))))))); + 2, Vector3(0.0782, 0.4427, 0.1067)))))))))); // Marginal on 5 Matrix expectedCov = (Matrix(1,1) << 236.5166).finished(); @@ -261,11 +261,11 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { // Known steepest descent point from Bayes' net version VectorValues expectedFromBN = pair_list_of - (0, (Vector(2) << 0.000129034, 0.000688183).finished()) - (1, (Vector(2) << 0.0109679, 0.0253767).finished()) - (2, (Vector(2) << 0.0680441, 0.114496).finished()) - (3, (Vector(2) << 0.16125, 0.241294).finished()) - (4, (Vector(2) << 0.300134, 0.423233).finished()); + (0, Vector2(0.000129034, 0.000688183)) + (1, Vector2(0.0109679, 0.0253767)) + (2, Vector2(0.0680441, 0.114496)) + (3, Vector2(0.16125, 0.241294)) + (4, Vector2(0.300134, 0.423233)); // Compute the steepest descent point with the dogleg function VectorValues actual = bt.optimizeGradientSearch(); diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index c3e1d3ddc..60387694e 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -58,8 +58,8 @@ TEST(GaussianConditional, constructor) -11.3820, -7.2581, -3.0153, -3.5635).finished(); - Vector d = (Vector(2) << 1.0, 2.0).finished(); - SharedDiagonal s = noiseModel::Diagonal::Sigmas((Vector(2) << 3.0, 4.0).finished()); + Vector d = Vector2(1.0, 2.0); + SharedDiagonal s = noiseModel::Diagonal::Sigmas(Vector2(3.0, 4.0)); vector > terms = pair_list_of (1, R) @@ -116,9 +116,9 @@ TEST( GaussianConditional, equals ) R(0,0) = 0.1 ; R(1,0) = 0.3; R(0,1) = 0.0 ; R(1,1) = 0.34; - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 0.34).finished()); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34)); - Vector d = (Vector(2) << 0.2, 0.5).finished(); + Vector d = Vector2(0.2, 0.5); GaussianConditional expected(1, d, R, 2, A1, 10, A2, model), @@ -179,7 +179,7 @@ TEST( GaussianConditional, solve_simple ) GaussianConditional cg(list_of(1)(2), 1, blockMatrix); // partial solution - Vector sx1 = (Vector(2) << 9.0, 10.0).finished(); + Vector sx1 = Vector2(9.0, 10.0); // elimination order: 1, 2 VectorValues actual = map_list_of @@ -215,15 +215,15 @@ TEST( GaussianConditional, solve_multifrontal ) EXPECT(assert_equal(Vector(blockMatrix.full().rightCols(1)), cg.get_d())); // partial solution - Vector sl1 = (Vector(2) << 9.0, 10.0).finished(); + Vector sl1 = Vector2(9.0, 10.0); // elimination order; _x_, _x1_, _l1_ VectorValues actual = map_list_of (10, sl1); // parent VectorValues expected = map_list_of - (1, (Vector(2) << -3.1,-3.4).finished()) - (2, (Vector(2) << -11.9,-13.2).finished()) + (1, Vector2(-3.1,-3.4)) + (2, Vector2(-11.9,-13.2)) (10, sl1); // verify indices/size diff --git a/gtsam/linear/tests/testGaussianDensity.cpp b/gtsam/linear/tests/testGaussianDensity.cpp index b502752e4..081b1d3d1 100644 --- a/gtsam/linear/tests/testGaussianDensity.cpp +++ b/gtsam/linear/tests/testGaussianDensity.cpp @@ -29,7 +29,7 @@ TEST(GaussianDensity, constructor) -12.1244, -5.1962, 0., 4.6904).finished(); - Vector d = (Vector(2) << 1.0, 2.0).finished(), s = (Vector(2) << 3.0, 4.0).finished(); + Vector d = Vector2(1.0, 2.0), s = Vector2(3.0, 4.0); GaussianConditional conditional(1, d, R, noiseModel::Diagonal::Sigmas(s)); GaussianDensity copied(conditional); diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 722d32810..d87d842de 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -48,9 +48,9 @@ TEST(GaussianFactorGraph, initialization) { fg += JacobianFactor(0, 10*eye(2), -1.0*ones(2), unit2), - JacobianFactor(0, -10*eye(2),1, 10*eye(2), (Vector(2) << 2.0, -1.0).finished(), unit2), - JacobianFactor(0, -5*eye(2), 2, 5*eye(2), (Vector(2) << 0.0, 1.0).finished(), unit2), - JacobianFactor(1, -5*eye(2), 2, 5*eye(2), (Vector(2) << -1.0, 1.5).finished(), unit2); + JacobianFactor(0, -10*eye(2),1, 10*eye(2), Vector2(2.0, -1.0), unit2), + JacobianFactor(0, -5*eye(2), 2, 5*eye(2), Vector2(0.0, 1.0), unit2), + JacobianFactor(1, -5*eye(2), 2, 5*eye(2), Vector2(-1.0, 1.5), unit2); EXPECT_LONGS_EQUAL(4, (long)fg.size()); @@ -97,8 +97,8 @@ TEST(GaussianFactorGraph, sparseJacobian) { GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); - gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.).finished(), (Vector(2) << 4., 8.).finished(), model); - gfg.add(0, (Matrix(2, 3) << 9.,10., 0., 0., 0., 0.).finished(), 1, (Matrix(2, 2) << 11., 12., 14., 15.).finished(), (Vector(2) << 13.,16.).finished(), model); + gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.).finished(), Vector2(4., 8.), model); + gfg.add(0, (Matrix(2, 3) << 9.,10., 0., 0., 0., 0.).finished(), 1, (Matrix(2, 2) << 11., 12., 14., 15.).finished(), Vector2(13.,16.), model); Matrix actual = gfg.sparseJacobian_(); @@ -120,8 +120,8 @@ TEST(GaussianFactorGraph, matrices) { GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Unit::Create(2); - gfg.add(0, A00, (Vector(2) << 4., 8.).finished(), model); - gfg.add(0, A10, 1, A11, (Vector(2) << 13.,16.).finished(), model); + gfg.add(0, A00, Vector2(4., 8.), model); + gfg.add(0, A10, 1, A11, Vector2(13.,16.), model); Matrix Ab(4,6); Ab << @@ -150,8 +150,8 @@ TEST(GaussianFactorGraph, matrices) { // hessianBlockDiagonal VectorValues expectLdiagonal; // Make explicit that diagonal is sum-squares of columns - expectLdiagonal.insert(0, (Vector(3) << 1+25+81, 4+36+100, 9+49).finished()); - expectLdiagonal.insert(1, (Vector(2) << 121+196, 144+225).finished()); + expectLdiagonal.insert(0, Vector3(1+25+81, 4+36+100, 9+49)); + expectLdiagonal.insert(1, Vector2(121+196, 144+225)); EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal())); // hessianBlockDiagonal @@ -168,11 +168,11 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(0, 10*eye(2), 2, -10*eye(2), (Vector(2) << 2.0, -1.0).finished(), unit2); + fg += JacobianFactor(0, 10*eye(2), 2, -10*eye(2), Vector2(2.0, -1.0), unit2); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(1, 5*eye(2), 2, -5*eye(2), (Vector(2) << 0.0, 1.0).finished(), unit2); + fg += JacobianFactor(1, 5*eye(2), 2, -5*eye(2), Vector2(0.0, 1.0), unit2); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5).finished(), unit2); + fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector2(-1.0, 1.5), unit2); return fg; } @@ -185,9 +185,9 @@ TEST( GaussianFactorGraph, gradient ) // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] VectorValues expected = map_list_of - (1, (Vector(2) << 5.0, -12.5).finished()) - (2, (Vector(2) << 30.0, 5.0).finished()) - (0, (Vector(2) << -25.0, 17.5).finished()); + (1, Vector2(5.0, -12.5)) + (2, Vector2(30.0, 5.0)) + (0, Vector2(-25.0, 17.5)); // Check the gradient at delta=0 VectorValues zero = VectorValues::Zero(expected); @@ -207,15 +207,15 @@ TEST( GaussianFactorGraph, transposeMultiplication ) GaussianFactorGraph A = createSimpleGaussianFactorGraph(); Errors e; e += - (Vector(2) << 0.0, 0.0).finished(), - (Vector(2) << 15.0, 0.0).finished(), - (Vector(2) << 0.0,-5.0).finished(), - (Vector(2) << -7.5,-5.0).finished(); + Vector2(0.0, 0.0), + Vector2(15.0, 0.0), + Vector2(0.0,-5.0), + Vector2(-7.5,-5.0); VectorValues expected; - expected.insert(1, (Vector(2) << -37.5,-50.0).finished()); - expected.insert(2, (Vector(2) << -150.0, 25.0).finished()); - expected.insert(0, (Vector(2) << 187.5, 25.0).finished()); + expected.insert(1, Vector2(-37.5,-50.0)); + expected.insert(2, Vector2(-150.0, 25.0)); + expected.insert(0, Vector2(187.5, 25.0)); VectorValues actual = A.transposeMultiply(e); EXPECT(assert_equal(expected, actual)); @@ -259,14 +259,14 @@ TEST( GaussianFactorGraph, multiplyHessianAdd ) GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); VectorValues x = map_list_of - (0, (Vector(2) << 1,2).finished()) - (1, (Vector(2) << 3,4).finished()) - (2, (Vector(2) << 5,6).finished()); + (0, Vector2(1,2)) + (1, Vector2(3,4)) + (2, Vector2(5,6)); VectorValues expected; - expected.insert(0, (Vector(2) << -450, -450).finished()); - expected.insert(1, (Vector(2) << 0, 0).finished()); - expected.insert(2, (Vector(2) << 950, 1050).finished()); + expected.insert(0, Vector2(-450, -450)); + expected.insert(1, Vector2(0, 0)); + expected.insert(2, Vector2(950, 1050)); VectorValues actual; gfg.multiplyHessianAdd(1.0, x, actual); @@ -280,8 +280,8 @@ TEST( GaussianFactorGraph, multiplyHessianAdd ) /* ************************************************************************* */ static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() { GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); - gfg += HessianFactor(1, 2, 100*eye(2,2), zeros(2,2), (Vector(2) << 0.0, 1.0).finished(), - 400*eye(2,2), (Vector(2) << 1.0, 1.0).finished(), 3.0); + gfg += HessianFactor(1, 2, 100*eye(2,2), zeros(2,2), Vector2(0.0, 1.0), + 400*eye(2,2), Vector2(1.0, 1.0), 3.0); return gfg; } @@ -297,14 +297,14 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 ) EXPECT(assert_equal(Y,AtA*X)); VectorValues x = map_list_of - (0, (Vector(2) << 1,2).finished()) - (1, (Vector(2) << 3,4).finished()) - (2, (Vector(2) << 5,6).finished()); + (0, Vector2(1,2)) + (1, Vector2(3,4)) + (2, Vector2(5,6)); VectorValues expected; - expected.insert(0, (Vector(2) << -450, -450).finished()); - expected.insert(1, (Vector(2) << 300, 400).finished()); - expected.insert(2, (Vector(2) << 2950, 3450).finished()); + expected.insert(0, Vector2(-450, -450)); + expected.insert(1, Vector2(300, 400)); + expected.insert(2, Vector2(2950, 3450)); VectorValues actual; gfg.multiplyHessianAdd(1.0, x, actual); @@ -358,9 +358,9 @@ TEST( GaussianFactorGraph, gradientAtZero ) GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); VectorValues expected; VectorValues actual = gfg.gradientAtZero(); - expected.insert(0, (Vector(2) << -25, 17.5).finished()); - expected.insert(1, (Vector(2) << 5, -13.5).finished()); - expected.insert(2, (Vector(2) << 29, 4).finished()); + expected.insert(0, Vector2(-25, 17.5)); + expected.insert(1, Vector2(5, -13.5)); + expected.insert(2, Vector2(29, 4)); EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 0180d91e7..cba65580e 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -76,7 +76,7 @@ TEST(HessianFactor, ConversionConstructor) HessianFactor actual(jacobian); VectorValues values = pair_list_of - (0, (Vector(2) << 1.0, 2.0).finished()) + (0, Vector2(1.0, 2.0)) (1, (Vector(4) << 3.0, 4.0, 5.0, 6.0).finished()); EXPECT_LONGS_EQUAL(2, (long)actual.size()); @@ -88,7 +88,7 @@ TEST(HessianFactor, ConversionConstructor) TEST(HessianFactor, Constructor1) { Matrix G = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished(); - Vector g = (Vector(2) << -8.0, -9.0).finished(); + Vector g = Vector2(-8.0, -9.0); double f = 10.0; HessianFactor factor(0, G, g, f); @@ -98,7 +98,7 @@ TEST(HessianFactor, Constructor1) EXPECT(assert_equal(g, Vector(factor.linearTerm()))); EXPECT_LONGS_EQUAL(1, (long)factor.size()); - VectorValues dx = pair_list_of(0, (Vector(2) << 1.5, 2.5).finished()); + VectorValues dx = pair_list_of(0, Vector2(1.5, 2.5)); // error 0.5*(f - 2*x'*g + x'*G*x) double expected = 80.375; @@ -112,7 +112,7 @@ TEST(HessianFactor, Constructor1) /* ************************************************************************* */ TEST(HessianFactor, Constructor1b) { - Vector mu = (Vector(2) << 1.0,2.0).finished(); + Vector mu = Vector2(1.0,2.0); Matrix Sigma = eye(2,2); HessianFactor factor(0, mu, Sigma); @@ -135,11 +135,11 @@ TEST(HessianFactor, Constructor2) Matrix G12 = (Matrix(1,2) << 2.0, 4.0).finished(); Matrix G22 = (Matrix(2,2) << 3.0, 5.0, 5.0, 6.0).finished(); Vector g1 = (Vector(1) << -7.0).finished(); - Vector g2 = (Vector(2) << -8.0, -9.0).finished(); + Vector g2 = Vector2(-8.0, -9.0); double f = 10.0; Vector dx0 = (Vector(1) << 0.5).finished(); - Vector dx1 = (Vector(2) << 1.5, 2.5).finished(); + Vector dx1 = Vector2(1.5, 2.5); VectorValues dx = pair_list_of (0, dx0) @@ -165,7 +165,7 @@ TEST(HessianFactor, Constructor2) VectorValues dxLarge = pair_list_of (0, dx0) (1, dx1) - (2, (Vector(2) << 0.1, 0.2).finished()); + (2, Vector2(0.1, 0.2)); EXPECT_DOUBLES_EQUAL(expected, factor.error(dxLarge), 1e-10); } @@ -182,14 +182,14 @@ TEST(HessianFactor, Constructor3) Matrix G33 = (Matrix(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0).finished(); Vector g1 = (Vector(1) << -7.0).finished(); - Vector g2 = (Vector(2) << -8.0, -9.0).finished(); - Vector g3 = (Vector(3) << 1.0, 2.0, 3.0).finished(); + Vector g2 = Vector2(-8.0, -9.0); + Vector g3 = Vector3(1.0, 2.0, 3.0); double f = 10.0; Vector dx0 = (Vector(1) << 0.5).finished(); - Vector dx1 = (Vector(2) << 1.5, 2.5).finished(); - Vector dx2 = (Vector(3) << 1.5, 2.5, 3.5).finished(); + Vector dx1 = Vector2(1.5, 2.5); + Vector dx2 = Vector3(1.5, 2.5, 3.5); VectorValues dx = pair_list_of (0, dx0) @@ -229,14 +229,14 @@ TEST(HessianFactor, ConstructorNWay) Matrix G33 = (Matrix(3,3) << 1.0, 2.0, 3.0, 2.0, 5.0, 6.0, 3.0, 6.0, 9.0).finished(); Vector g1 = (Vector(1) << -7.0).finished(); - Vector g2 = (Vector(2) << -8.0, -9.0).finished(); - Vector g3 = (Vector(3) << 1.0, 2.0, 3.0).finished(); + Vector g2 = Vector2(-8.0, -9.0); + Vector g3 = Vector3(1.0, 2.0, 3.0); double f = 10.0; Vector dx0 = (Vector(1) << 0.5).finished(); - Vector dx1 = (Vector(2) << 1.5, 2.5).finished(); - Vector dx2 = (Vector(3) << 1.5, 2.5, 3.5).finished(); + Vector dx1 = Vector2(1.5, 2.5); + Vector dx2 = Vector3(1.5, 2.5, 3.5); VectorValues dx = pair_list_of (0, dx0) @@ -379,7 +379,7 @@ TEST(HessianFactor, eliminate2 ) -0.20, 0.00,-0.80, 0.00, +0.00,-0.20,+0.00,-0.80 ).finished()/oldSigma; - Vector d = (Vector(2) << 0.2,-0.14).finished()/oldSigma; + Vector d = Vector2(0.2,-0.14)/oldSigma; GaussianConditional expectedCG(0, d, R11, 1, S12); EXPECT(assert_equal(expectedCG, *actual_Chol.first, 1e-4)); @@ -390,7 +390,7 @@ TEST(HessianFactor, eliminate2 ) 1.00, 0.00, -1.00, 0.00, 0.00, 1.00, +0.00, -1.00 ).finished()/sigma; - Vector b1 = (Vector(2) << 0.0,0.894427).finished(); + Vector b1 = Vector2(0.0,0.894427); JacobianFactor expectedLF(1, Bl1x1, b1, noiseModel::Isotropic::Sigma(2,1.0)); EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3)); } @@ -408,7 +408,7 @@ TEST(HessianFactor, combine) { Matrix A2 = (Matrix(2, 2) << -8.94427191, 0.0, 0.0, -8.94427191).finished(); - Vector b = (Vector(2) << 2.23606798,-1.56524758).finished(); + Vector b = Vector2(2.23606798,-1.56524758); SharedDiagonal model = noiseModel::Diagonal::Sigmas(ones(2)); GaussianFactor::shared_ptr f(new JacobianFactor(0, A0, 1, A1, 2, A2, b, model)); GaussianFactorGraph factors = list_of(f); @@ -435,7 +435,7 @@ TEST(HessianFactor, gradientAtZero) Matrix G12 = (Matrix(1, 2) << 0, 0).finished(); Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1).finished(); Vector g1 = (Vector(1) << -7).finished(); - Vector g2 = (Vector(2) << -8, -9).finished(); + Vector g2 = Vector2(-8, -9); double f = 194; HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); @@ -456,7 +456,7 @@ TEST(HessianFactor, hessianDiagonal) Matrix G12 = (Matrix(1, 2) << 0, 0).finished(); Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1).finished(); Vector g1 = (Vector(1) << -7).finished(); - Vector g2 = (Vector(2) << -8, -9).finished(); + Vector g2 = Vector2(-8, -9); double f = 194; HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); @@ -464,7 +464,7 @@ TEST(HessianFactor, hessianDiagonal) // hessianDiagonal VectorValues expected; expected.insert(0, (Vector(1) << 1).finished()); - expected.insert(1, (Vector(2) << 1,1).finished()); + expected.insert(1, Vector2(1,1)); EXPECT(assert_equal(expected, factor.hessianDiagonal())); // hessianBlockDiagonal diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 411392290..7b2d59171 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -42,8 +42,8 @@ namespace { (make_pair(15, 3*Matrix3::Identity())); // RHS and sigmas - const Vector b = (Vector(3) << 1., 2., 3.).finished(); - const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.5).finished()); + const Vector b = Vector3(1., 2., 3.); + const SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.5)); } } @@ -156,7 +156,7 @@ TEST(JabobianFactor, Hessian_conversion) { JacobianFactor expected(0, (Matrix(2,4) << 1.2530, 2.1508, -0.8779, -1.8755, 0, 2.5858, 0.4789, -2.3943).finished(), - (Vector(2) << -6.2929, -5.7941).finished()); + Vector2(-6.2929, -5.7941)); EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3)); } @@ -296,9 +296,9 @@ TEST(JacobianFactor, matrices) // hessianDiagonal VectorValues expectDiagonal; // below we divide by the variance 0.5^2 - expectDiagonal.insert(5, (Vector(3) << 1, 1, 1).finished()/0.25); - expectDiagonal.insert(10, (Vector(3) << 4, 4, 4).finished()/0.25); - expectDiagonal.insert(15, (Vector(3) << 9, 9, 9).finished()/0.25); + expectDiagonal.insert(5, Vector3(1, 1, 1)/0.25); + expectDiagonal.insert(10, Vector3(4, 4, 4)/0.25); + expectDiagonal.insert(15, Vector3(9, 9, 9)/0.25); EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal())); // hessianBlockDiagonal @@ -315,22 +315,22 @@ TEST(JacobianFactor, operators ) SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); Matrix I = eye(2); - Vector b = (Vector(2) << 0.2,-0.1).finished(); + Vector b = Vector2(0.2,-0.1); JacobianFactor lf(1, -I, 2, I, b, sigma0_1); VectorValues c; - c.insert(1, (Vector(2) << 10.,20.).finished()); - c.insert(2, (Vector(2) << 30.,60.).finished()); + c.insert(1, Vector2(10.,20.)); + c.insert(2, Vector2(30.,60.)); // test A*x - Vector expectedE = (Vector(2) << 200.,400.).finished(); + Vector expectedE = Vector2(200.,400.); Vector actualE = lf * c; EXPECT(assert_equal(expectedE, actualE)); // test A^e VectorValues expectedX; - expectedX.insert(1, (Vector(2) << -2000.,-4000.).finished()); - expectedX.insert(2, (Vector(2) << 2000., 4000.).finished()); + expectedX.insert(1, Vector2(-2000.,-4000.)); + expectedX.insert(2, Vector2(2000., 4000.)); VectorValues actualX = VectorValues::Zero(expectedX); lf.transposeMultiplyAdd(1.0, actualE, actualX); EXPECT(assert_equal(expectedX, actualX)); @@ -338,8 +338,8 @@ TEST(JacobianFactor, operators ) // test gradient at zero Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian(); VectorValues expectedG; - expectedG.insert(1, (Vector(2) << 20,-10).finished()); - expectedG.insert(2, (Vector(2) << -20, 10).finished()); + expectedG.insert(1, Vector2(20,-10)); + expectedG.insert(2, Vector2(-20, 10)); FastVector keys; keys += 1,2; EXPECT(assert_equal(-A.transpose()*b2, expectedG.vector(keys))); VectorValues actualG = lf.gradientAtZero(); @@ -465,7 +465,7 @@ TEST(JacobianFactor, eliminate2 ) -0.20, 0.00,-0.80, 0.00, +0.00,-0.20,+0.00,-0.80 ).finished()/oldSigma; - Vector d = (Vector(2) << 0.2,-0.14).finished()/oldSigma; + Vector d = Vector2(0.2,-0.14)/oldSigma; GaussianConditional expectedCG(2, d, R11, 11, S12); EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); @@ -477,7 +477,7 @@ TEST(JacobianFactor, eliminate2 ) 1.00, 0.00, -1.00, 0.00, 0.00, 1.00, +0.00, -1.00 ).finished()/sigma; - Vector b1 = (Vector(2) << 0.0, 0.894427).finished(); + Vector b1 = Vector2(0.0, 0.894427); JacobianFactor expectedLF(11, Bl1x1, b1); EXPECT(assert_equal(expectedLF, *actual.second,1e-3)); } @@ -561,7 +561,7 @@ TEST ( JacobianFactor, constraint_eliminate1 ) EXPECT(actual.second->size() == 0); // verify conditional Gaussian - Vector sigmas = (Vector(2) << 0.0, 0.0).finished(); + Vector sigmas = Vector2(0.0, 0.0); GaussianConditional expCG(1, v, eye(2), noiseModel::Diagonal::Sigmas(sigmas)); EXPECT(assert_equal(expCG, *actual.first)); } @@ -604,8 +604,8 @@ TEST ( JacobianFactor, constraint_eliminate2 ) Matrix S = (Matrix(2, 2) << 1.0, 2.0, 0.0, 0.0).finished(); - Vector d = (Vector(2) << 3.0, 0.6666).finished(); - Vector sigmas = (Vector(2) << 0.0, 0.0).finished(); + Vector d = Vector2(3.0, 0.6666); + Vector sigmas = Vector2(0.0, 0.0); GaussianConditional expectedCG(1, d, R, 2, S, noiseModel::Diagonal::Sigmas(sigmas)); EXPECT(assert_equal(expectedCG, *actual.first, 1e-4)); } diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index a4b017667..999541ff5 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -67,7 +67,7 @@ TEST( KalmanFilter, linear1 ) { // Create the controls and measurement properties for our example Matrix F = eye(2, 2); Matrix B = eye(2, 2); - Vector u = (Vector(2) << 1.0, 0.0).finished(); + Vector u = Vector2(1.0, 0.0); SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1); Matrix Q = 0.01*eye(2, 2); Matrix H = eye(2, 2); @@ -137,7 +137,7 @@ TEST( KalmanFilter, predict ) { // Create dynamics model Matrix F = (Matrix(2, 2) << 1.0, 0.1, 0.2, 1.1).finished(); Matrix B = (Matrix(2, 3) << 1.0, 0.1, 0.2, 1.1, 1.2, 0.8).finished(); - Vector u = (Vector(3) << 1.0, 0.0, 2.0).finished(); + Vector u = Vector3(1.0, 0.0, 2.0); Matrix R = (Matrix(2, 2) << 1.0, 0.5, 0.0, 3.0).finished(); Matrix M = trans(R)*R; Matrix Q = inverse(M); @@ -240,8 +240,8 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 9795.9, 83.6, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0, -9795.9, 0.0, -5.2, 0.0, 0.0, 0.0, 0.0, 1000.0, 0.0, -83.6, 5.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.).finished(); - Vector z = (Vector(3) << 0.2599 , 1.3327 , 0.2007).finished(); - Vector sigmas = (Vector(3) << 0.3323 , 0.2470 , 0.1904).finished(); + Vector z = Vector3(0.2599 , 1.3327 , 0.2007); + Vector sigmas = Vector3(0.3323 , 0.2470 , 0.1904); SharedDiagonal modelR = noiseModel::Diagonal::Sigmas(sigmas); // do update diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index eb8ecbe1d..5c6b2729d 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -48,8 +48,8 @@ static Matrix Sigma = (Matrix(3, 3) << /* ************************************************************************* */ TEST(NoiseModel, constructors) { - Vector whitened = (Vector(3) << 5.0,10.0,15.0).finished(); - Vector unwhitened = (Vector(3) << 10.0,20.0,30.0).finished(); + Vector whitened = Vector3(5.0,10.0,15.0); + Vector unwhitened = Vector3(10.0,20.0,30.0); // Construct noise models vector m; @@ -107,7 +107,7 @@ TEST(NoiseModel, constructors) /* ************************************************************************* */ TEST(NoiseModel, Unit) { - Vector v = (Vector(3) << 5.0,10.0,15.0).finished(); + Vector v = Vector3(5.0,10.0,15.0); Gaussian::shared_ptr u(Unit::Create(3)); EXPECT(assert_equal(v,u->whiten(v))); } @@ -118,7 +118,7 @@ TEST(NoiseModel, equals) Gaussian::shared_ptr g1 = Gaussian::SqrtInformation(R), g2 = Gaussian::SqrtInformation(eye(3,3)); Diagonal::shared_ptr d1 = Diagonal::Sigmas((Vector(3) << sigma, sigma, sigma).finished()), - d2 = Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3).finished()); + d2 = Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); Isotropic::shared_ptr i1 = Isotropic::Sigma(3, sigma), i2 = Isotropic::Sigma(3, 0.7); @@ -156,7 +156,7 @@ TEST(NoiseModel, ConstrainedConstructors ) size_t d = 3; double m = 100.0; Vector sigmas = (Vector(3) << sigma, 0.0, 0.0).finished(); - Vector mu = (Vector(3) << 200.0, 300.0, 400.0).finished(); + Vector mu = Vector3(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())); @@ -180,12 +180,12 @@ TEST(NoiseModel, ConstrainedConstructors ) /* ************************************************************************* */ TEST(NoiseModel, ConstrainedMixed ) { - Vector feasible = (Vector(3) << 1.0, 0.0, 1.0).finished(), - infeasible = (Vector(3) << 1.0, 1.0, 1.0).finished(); + Vector feasible = Vector3(1.0, 0.0, 1.0), + infeasible = Vector3(1.0, 1.0, 1.0); Diagonal::shared_ptr d = Constrained::MixedSigmas((Vector(3) << sigma, 0.0, sigma).finished()); // NOTE: we catch constrained variables elsewhere, so whitening does nothing - EXPECT(assert_equal((Vector(3) << 0.5, 1.0, 0.5).finished(),d->whiten(infeasible))); - EXPECT(assert_equal((Vector(3) << 0.5, 0.0, 0.5).finished(),d->whiten(feasible))); + EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible))); + EXPECT(assert_equal(Vector3(0.5, 0.0, 0.5),d->whiten(feasible))); DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9); DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9); @@ -194,13 +194,13 @@ TEST(NoiseModel, ConstrainedMixed ) /* ************************************************************************* */ TEST(NoiseModel, ConstrainedAll ) { - Vector feasible = (Vector(3) << 0.0, 0.0, 0.0).finished(), - infeasible = (Vector(3) << 1.0, 1.0, 1.0).finished(); + Vector feasible = Vector3(0.0, 0.0, 0.0), + infeasible = Vector3(1.0, 1.0, 1.0); Constrained::shared_ptr i = Constrained::All(3); // NOTE: we catch constrained variables elsewhere, so whitening does nothing - EXPECT(assert_equal((Vector(3) << 1.0, 1.0, 1.0).finished(),i->whiten(infeasible))); - EXPECT(assert_equal((Vector(3) << 0.0, 0.0, 0.0).finished(),i->whiten(feasible))); + EXPECT(assert_equal(Vector3(1.0, 1.0, 1.0),i->whiten(infeasible))); + EXPECT(assert_equal(Vector3(0.0, 0.0, 0.0),i->whiten(feasible))); DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9); DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9); @@ -317,7 +317,7 @@ TEST(NoiseModel, ScalarOrVector ) /* ************************************************************************* */ TEST(NoiseModel, WhitenInPlace) { - Vector sigmas = (Vector(3) << 0.1, 0.1, 0.1).finished(); + Vector sigmas = Vector3(0.1, 0.1, 0.1); SharedDiagonal model = Diagonal::Sigmas(sigmas); Matrix A = eye(3); model->WhitenInPlace(A); diff --git a/gtsam/linear/tests/testSampler.cpp b/gtsam/linear/tests/testSampler.cpp index dbcb96538..bd718500e 100644 --- a/gtsam/linear/tests/testSampler.cpp +++ b/gtsam/linear/tests/testSampler.cpp @@ -24,7 +24,7 @@ const double tol = 1e-5; /* ************************************************************************* */ TEST(testSampler, basic) { - Vector sigmas = (Vector(3) << 1.0, 0.1, 0.0).finished(); + Vector sigmas = Vector3(1.0, 0.1, 0.0); noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas); char seed = 'A'; Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1); diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index b1aad673d..f93788af9 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -49,10 +49,10 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* ************************************************************************* */ // example noise models -static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3).finished()); +static noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); static noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); static noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); -static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas((Vector(3) << 0.0, 0.0, 0.1).finished()); +static noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector3(0.0, 0.0, 0.1)); static noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); /* ************************************************************************* */ @@ -136,8 +136,8 @@ BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional TEST (Serialization, linear_factors) { VectorValues values; values.insert(0, (Vector(1) << 1.0).finished()); - values.insert(1, (Vector(2) << 2.0,3.0).finished()); - values.insert(2, (Vector(2) << 4.0,5.0).finished()); + values.insert(1, Vector2(2.0,3.0)); + values.insert(2, Vector2(4.0,5.0)); EXPECT(equalsObj(values)); EXPECT(equalsXML(values)); EXPECT(equalsBinary(values)); @@ -145,7 +145,7 @@ TEST (Serialization, linear_factors) { Key i1 = 4, i2 = 7; Matrix A1 = eye(3), A2 = -1.0 * eye(3); Vector b = ones(3); - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1.0, 2.0, 3.0).finished()); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0)); JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); EXPECT(equalsObj(jacobianfactor)); EXPECT(equalsXML(jacobianfactor)); @@ -186,7 +186,7 @@ TEST (Serialization, gaussian_factor_graph) { Key i1 = 4, i2 = 7; Matrix A1 = eye(3), A2 = -1.0 * eye(3); Vector b = ones(3); - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1.0, 2.0, 3.0).finished()); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1.0, 2.0, 3.0)); JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); HessianFactor hessianfactor(jacobianfactor); graph.push_back(jacobianfactor); diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index 26eeb5dab..949ec3a59 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -37,9 +37,9 @@ TEST(VectorValues, basics) // insert VectorValues actual; actual.insert(0, (Vector(1) << 1).finished()); - actual.insert(1, (Vector(2) << 2, 3).finished()); - actual.insert(5, (Vector(2) << 6, 7).finished()); - actual.insert(2, (Vector(2) << 4, 5).finished()); + actual.insert(1, Vector2(2, 3)); + actual.insert(5, Vector2(6, 7)); + actual.insert(2, Vector2(4, 5)); // Check dimensions LONGS_EQUAL(4, actual.size()); @@ -59,9 +59,9 @@ TEST(VectorValues, basics) // Check values EXPECT(assert_equal((Vector(1) << 1).finished(), actual[0])); - EXPECT(assert_equal((Vector(2) << 2, 3).finished(), actual[1])); - EXPECT(assert_equal((Vector(2) << 4, 5).finished(), actual[2])); - EXPECT(assert_equal((Vector(2) << 6, 7).finished(), actual[5])); + EXPECT(assert_equal(Vector2(2, 3), actual[1])); + EXPECT(assert_equal(Vector2(4, 5), actual[2])); + EXPECT(assert_equal(Vector2(6, 7), actual[5])); FastVector keys = list_of(0)(1)(2)(5); EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7).finished(), actual.vector(keys))); @@ -75,17 +75,17 @@ TEST(VectorValues, combine) { VectorValues expected; expected.insert(0, (Vector(1) << 1).finished()); - expected.insert(1, (Vector(2) << 2, 3).finished()); - expected.insert(5, (Vector(2) << 6, 7).finished()); - expected.insert(2, (Vector(2) << 4, 5).finished()); + expected.insert(1, Vector2(2, 3)); + expected.insert(5, Vector2(6, 7)); + expected.insert(2, Vector2(4, 5)); VectorValues first; first.insert(0, (Vector(1) << 1).finished()); - first.insert(1, (Vector(2) << 2, 3).finished()); + first.insert(1, Vector2(2, 3)); VectorValues second; - second.insert(5, (Vector(2) << 6, 7).finished()); - second.insert(2, (Vector(2) << 4, 5).finished()); + second.insert(5, Vector2(6, 7)); + second.insert(2, Vector2(4, 5)); VectorValues actual(first, second); @@ -97,9 +97,9 @@ TEST(VectorValues, subvector) { VectorValues init; init.insert(10, (Vector(1) << 1).finished()); - init.insert(11, (Vector(2) << 2, 3).finished()); - init.insert(12, (Vector(2) << 4, 5).finished()); - init.insert(13, (Vector(2) << 6, 7).finished()); + init.insert(11, Vector2(2, 3)); + init.insert(12, Vector2(4, 5)); + init.insert(13, Vector2(6, 7)); std::vector keys; keys += 10, 12, 13; @@ -112,15 +112,15 @@ TEST(VectorValues, LinearAlgebra) { VectorValues test1; test1.insert(0, (Vector(1) << 1).finished()); - test1.insert(1, (Vector(2) << 2, 3).finished()); - test1.insert(5, (Vector(2) << 6, 7).finished()); - test1.insert(2, (Vector(2) << 4, 5).finished()); + test1.insert(1, Vector2(2, 3)); + test1.insert(5, Vector2(6, 7)); + test1.insert(2, Vector2(4, 5)); VectorValues test2; test2.insert(0, (Vector(1) << 6).finished()); - test2.insert(1, (Vector(2) << 1, 6).finished()); - test2.insert(5, (Vector(2) << 4, 3).finished()); - test2.insert(2, (Vector(2) << 1, 8).finished()); + test2.insert(1, Vector2(1, 6)); + test2.insert(5, Vector2(4, 3)); + test2.insert(2, Vector2(1, 8)); // Dot product double dotExpected = test1.vector().dot(test2.vector()); @@ -176,9 +176,9 @@ TEST(VectorValues, convert) VectorValues expected; expected.insert(0, (Vector(1) << 1).finished()); - expected.insert(1, (Vector(2) << 2, 3).finished()); - expected.insert(2, (Vector(2) << 4, 5).finished()); - expected.insert(5, (Vector(2) << 6, 7).finished()); + expected.insert(1, Vector2(2, 3)); + expected.insert(2, Vector2(4, 5)); + expected.insert(5, Vector2(6, 7)); std::map dims; dims.insert(make_pair(0,1)); @@ -201,10 +201,10 @@ TEST(VectorValues, vector_sub) { VectorValues vv; vv.insert(0, (Vector(1) << 1).finished()); - vv.insert(1, (Vector(2) << 2, 3).finished()); - vv.insert(2, (Vector(2) << 4, 5).finished()); - vv.insert(5, (Vector(2) << 6, 7).finished()); - vv.insert(7, (Vector(2) << 8, 9).finished()); + vv.insert(1, Vector2(2, 3)); + vv.insert(2, Vector2(4, 5)); + vv.insert(5, Vector2(6, 7)); + vv.insert(7, Vector2(8, 9)); std::map dims; dims.insert(make_pair(0,1)); diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index cb7e40d25..bd321d51d 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -25,8 +25,8 @@ using namespace gtsam; /* ************************************************************************* */ TEST( ImuBias, Constructor) { - Vector bias_acc((Vector(3) << 0.1,0.2,0.4).finished()); - Vector bias_gyro((Vector(3) << -0.2, 0.5, 0.03).finished()); + Vector bias_acc(Vector3(0.1,0.2,0.4)); + Vector bias_gyro(Vector3(-0.2, 0.5, 0.03)); // Default Constructor gtsam::imuBias::ConstantBias bias1; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index d9444f490..d176c7895 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -167,9 +167,9 @@ TEST( ImuFactor, Error ) // Linearization point imuBias::ConstantBias bias; // Bias Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); - Vector3 v1((Vector(3) << 0.5, 0.0, 0.0).finished()); + Vector3 v1(Vector3(0.5, 0.0, 0.0)); Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); - Vector3 v2((Vector(3) << 0.5, 0.0, 0.0).finished()); + Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -239,16 +239,16 @@ TEST( ImuFactor, ErrorWithBiases ) // Linearization point // Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0).finished()); +// Vector3 v1(Vector3(0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0).finished()); +// Vector3 v2(Vector3(0.5, 0.0, 0.0)); imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - Vector3 v1((Vector(3) << 0.5, 0.0, 0.0).finished()); + Vector3 v1(Vector3(0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - Vector3 v2((Vector(3) << 0.5, 0.0, 0.0).finished()); + Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -446,9 +446,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) //{ // // Linearization point // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0).finished()); +// Vector3 v1(Vector3(0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0).finished()); +// Vector3 v2(Vector3(0.5, 0.0, 0.0)); // imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); // // // Pre-integrator @@ -502,9 +502,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - Vector3 v1((Vector(3) << 0.5, 0.0, 0.0).finished()); + Vector3 v1(Vector3(0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - Vector3 v2((Vector(3) << 0.5, 0.0, 0.0).finished()); + Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 6a7a47e4c..c2e728a2b 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -123,7 +123,7 @@ struct GTSAM_EXPORT ISAM2Params { * \code FastMap thresholds; thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished(); // 0.1 rad rotation threshold, 0.5 m translation threshold - thresholds['l'] = (Vector(3) << 1.0, 1.0, 1.0).finished(); // 1.0 m landmark position threshold + thresholds['l'] = Vector3(1.0, 1.0, 1.0); // 1.0 m landmark position threshold params.relinearizeThreshold = thresholds; \endcode */ diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index 92f4f4d07..bd862ef94 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -19,7 +19,7 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 1.0).finished()); +const gtsam::noiseModel::Diagonal::shared_ptr diag_model2 = noiseModel::Diagonal::Sigmas(Vector2(1.0, 1.0)); const double tol = 1e-5; gtsam::Key l1 = 101, l2 = 102, x1 = 1, x2 = 2; @@ -36,8 +36,8 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) { Matrix A2 = (Matrix(2, 2) << -0.0455167, -0.0443573, -0.0222154, -0.102489).finished(); - Vector b = (Vector(2) << 0.0277052, - -0.0533393).finished(); + Vector b = Vector2(0.0277052, + -0.0533393); JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2); @@ -70,8 +70,8 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { Matrix A2 = (Matrix(2, 2) << -0.0455167, -0.0443573, -0.0222154, -0.102489).finished(); - Vector b = (Vector(2) << 0.0277052, - -0.0533393).finished(); + Vector b = Vector2(0.0277052, + -0.0533393); JacobianFactor expLinFactor(l1, A1, l2, A2, b, diag_model2); @@ -97,8 +97,8 @@ TEST( testLinearContainerFactor, jacobian_factor_withlinpoints ) { EXPECT(assert_equal(expLinPoint, *actFactor.linearizationPoint())); // Check error evaluation - Vector delta_l1 = (Vector(2) << 1.0, 2.0).finished(); - Vector delta_l2 = (Vector(2) << 3.0, 4.0).finished(); + Vector delta_l1 = Vector2(1.0, 2.0); + Vector delta_l2 = Vector2(3.0, 4.0); VectorValues delta = values.zeroVectors(); delta.at(l1) = delta_l1; @@ -131,8 +131,8 @@ TEST( testLinearContainerFactor, generic_hessian_factor ) { 0.0, 0.0, 9.0).finished(); Vector g1 = (Vector(1) << -7.0).finished(); - Vector g2 = (Vector(2) << -8.0, -9.0).finished(); - Vector g3 = (Vector(3) << 1.0, 2.0, 3.0).finished(); + Vector g2 = Vector2(-8.0, -9.0); + Vector g3 = Vector3(1.0, 2.0, 3.0); double f = 10.0; @@ -166,13 +166,13 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { 1.0, 2.0, 3.0, 5.0, 4.0, 6.0).finished(); - Vector g1 = (Vector(3) << 1.0, 2.0, 3.0).finished(); + Vector g1 = Vector3(1.0, 2.0, 3.0); Matrix G22 = (Matrix(2, 2) << 0.5, 0.2, 0.0, 0.6).finished(); - Vector g2 = (Vector(2) << -8.0, -9.0).finished(); + Vector g2 = Vector2(-8.0, -9.0); double f = 10.0; @@ -197,9 +197,9 @@ TEST( testLinearContainerFactor, hessian_factor_withlinpoints ) { EXPECT(assert_equal(expLinPoints, actLinPoint)); // Create delta - Vector delta_l1 = (Vector(2) << 1.0, 2.0).finished(); - Vector delta_x1 = (Vector(3) << 3.0, 4.0, 0.5).finished(); - Vector delta_x2 = (Vector(3) << 6.0, 7.0, 0.3).finished(); + Vector delta_l1 = Vector2(1.0, 2.0); + Vector delta_x1 = Vector3(3.0, 4.0, 0.5); + Vector delta_x2 = Vector3(6.0, 7.0, 0.3); // Check error calculation VectorValues delta = linearizationPoint.zeroVectors(); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index c2c736a87..09fe0f253 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -194,7 +194,7 @@ TEST(Values, basic_functions) //TEST(Values, dim_zero) //{ // Values config0; -// config0.insert(key1, Vector2((Vector(2) << 2.0, 3.0).finished()); +// config0.insert(key1, Vector2(Vector2(2.0, 3.0)); // config0.insert(key2, Vector3(5.0, 6.0, 7.0)); // LONGS_EQUAL(5, config0.dim()); // @@ -212,8 +212,8 @@ TEST(Values, expmap_a) config0.insert(key2, Vector3(5.0, 6.0, 7.0)); VectorValues increment = pair_list_of - (key1, (Vector(3) << 1.0, 1.1, 1.2).finished()) - (key2, (Vector(3) << 1.3, 1.4, 1.5).finished()); + (key1, Vector3(1.0, 1.1, 1.2)) + (key2, Vector3(1.3, 1.4, 1.5)); Values expected; expected.insert(key1, Vector3(2.0, 3.1, 4.2)); @@ -230,7 +230,7 @@ TEST(Values, expmap_b) config0.insert(key2, Vector3(5.0, 6.0, 7.0)); VectorValues increment = pair_list_of - (key2, (Vector(3) << 1.3, 1.4, 1.5).finished()); + (key2, Vector3(1.3, 1.4, 1.5)); Values expected; expected.insert(key1, Vector3(1.0, 2.0, 3.0)); @@ -283,8 +283,8 @@ TEST(Values, localCoordinates) valuesA.insert(key2, Vector3(5.0, 6.0, 7.0)); VectorValues expDelta = pair_list_of - (key1, (Vector(3) << 0.1, 0.2, 0.3).finished()) - (key2, (Vector(3) << 0.4, 0.5, 0.6).finished()); + (key1, Vector3(0.1, 0.2, 0.3)) + (key2, Vector3(0.4, 0.5, 0.6)); Values valuesB = valuesA.retract(expDelta); diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index dbdf54fd3..26580f41e 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -291,7 +291,7 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl double th = logRot.norm(); if(th != th){ // the second case means that th = nan (logRot does not work well for +/-pi) - Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01).finished()) ); // some perturbation + Rot3 R1pert = R1.compose( Rot3::Expmap(Vector3(0.01, 0.01, 0.01)) ); // some perturbation logRot = Rot3::Logmap(R1pert.between(R2)); th = logRot.norm(); } diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index 203645b5b..3f50a8240 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -338,7 +338,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, } } // add prior - linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0).finished(), + linearPose2graph.add(keyAnchor, I3, Vector3(0.0, 0.0, 0.0), priorPose2Noise); // optimize diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 32a422f6e..d6a5ffa8c 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -110,7 +110,7 @@ TEST( dataSet, readG2o) expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934)); EXPECT(assert_equal(expectedValues,*actualValues,1e-5)); - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699).finished()); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); NonlinearFactorGraph expectedGraph; expectedGraph.add(BetweenFactor(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); @@ -238,7 +238,7 @@ TEST( dataSet, readG2oHuber) bool is3D = false; boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeHUBER); - noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699).finished()); + noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel); NonlinearFactorGraph expectedGraph; @@ -266,7 +266,7 @@ TEST( dataSet, readG2oTukey) bool is3D = false; boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY); - noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699).finished()); + noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); NonlinearFactorGraph expectedGraph; diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index a5f0d3bf2..e8acb0db0 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -71,7 +71,7 @@ TEST (EssentialMatrixFactor, testData) { EXPECT(assert_equal(Point2(-1, 0.2), pB(4), 1e-8)); // Check homogeneous version - EXPECT(assert_equal((Vector(3) << -1, 0.2, 1).finished(), vB(4), 1e-8)); + EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4), 1e-8)); // Check epipolar constraint for (size_t i = 0; i < 5; i++) diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 87beb44c2..8adbaa62d 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); TEST( GeneralSFMFactor, equals ) { // Create two identical factors and make sure they're equal - Vector z = (Vector(2) << 323.,240.).finished(); + Vector z = Vector2(323.,240.); const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr @@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) { Pose3 x1(R,t1); values.insert(X(1), GeneralCamera(x1)); Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal(((Vector) (Vector(2) << -3.0, 0.0).finished()), factor->unwhitenedError(values))); + EXPECT(assert_equal(((Vector) Vector2(-3.0, 0.0)), factor->unwhitenedError(values))); } static const double baseline = 5.0 ; diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 432ff3fec..9cc634b37 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); TEST( GeneralSFMFactor_Cal3Bundler, equals ) { // Create two identical factors and make sure they're equal - Vector z = (Vector(2) << 323.,240.).finished(); + Vector z = Vector2(323.,240.); const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr @@ -111,7 +111,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, error ) { Pose3 x1(R,t1); values.insert(X(1), GeneralCamera(x1)); Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal((Vector(2) << -3.0, 0.0).finished(), factor->unwhitenedError(values))); + EXPECT(assert_equal(Vector2(-3.0, 0.0), factor->unwhitenedError(values))); } diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 272fa14ee..61c4566bf 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -145,7 +145,7 @@ TEST( InitializePose3, iterationGradient ) { NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations - Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01).finished()); + Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); Values givenPoses; givenPoses.insert(x0,simple::pose0); givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) )); @@ -186,7 +186,7 @@ TEST( InitializePose3, orientationsGradient ) { NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations - Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01).finished()); + Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); Values givenPoses; givenPoses.insert(x0,simple::pose0); givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) )); diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index 361a6ec6b..9165ff17a 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -19,7 +19,7 @@ using namespace gtsam; const SharedNoiseModel model1 = noiseModel::Diagonal::Sigmas((Vector(1) << 0.1).finished()); -const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3).finished()); +const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); typedef PoseRotationPrior Pose2RotationPrior; typedef PoseRotationPrior Pose3RotationPrior; @@ -30,7 +30,7 @@ const gtsam::Key poseKey = 1; // Pose3 examples const Point3 point3A(1.0, 2.0, 3.0), point3B(4.0, 6.0, 8.0); -const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap((Vector(3) << 0.1, 0.2, 0.3).finished()); +const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap(Vector3(0.1, 0.2, 0.3)); // Pose2 examples const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); @@ -63,9 +63,9 @@ TEST( testPoseRotationFactor, level3_error ) { Pose3RotationPrior factor(poseKey, rot3C, model3); Matrix actH1; #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - EXPECT(assert_equal((Vector(3) << -0.1, -0.2,-0.3).finished(), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Vector3(-0.1, -0.2,-0.3), factor.evaluateError(pose1, actH1))); #else - EXPECT(assert_equal((Vector(3) << -0.1, -0.2, -0.3).finished(), factor.evaluateError(pose1, actH1),1e-2)); + EXPECT(assert_equal(Vector3(-0.1, -0.2, -0.3), factor.evaluateError(pose1, actH1),1e-2)); #endif Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); // the derivative is more complex, but is close to the identity for Rot3 around the origin diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 5bdad605c..f7bd412fe 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -15,8 +15,8 @@ using namespace gtsam; -const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2).finished()); -const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.2, 0.3).finished()); +const SharedNoiseModel model2 = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); +const SharedNoiseModel model3 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.2, 0.3)); typedef PoseTranslationPrior Pose2TranslationPrior; typedef PoseTranslationPrior Pose3TranslationPrior; @@ -58,7 +58,7 @@ TEST( testPoseTranslationFactor, level3_error ) { Pose3 pose1(rot3A, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; - EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0).finished(), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -78,7 +78,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) { Pose3 pose1(rot3B, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; - EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0).finished(), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -98,7 +98,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) { Pose3 pose1(rot3C, point3A); Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; - EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0).finished(), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -118,7 +118,7 @@ TEST( testPoseTranslationFactor, level2_error ) { Pose2 pose1(rot2A, point2A); Pose2TranslationPrior factor(poseKey, point2B, model2); Matrix actH1; - EXPECT(assert_equal((Vector(2) << -3.0,-4.0).finished(), factor.evaluateError(pose1, actH1))); + EXPECT(assert_equal(Vector2(-3.0,-4.0), factor.evaluateError(pose1, actH1))); Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 59577456b..1c8f3b8ed 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -108,7 +108,7 @@ TEST( ProjectionFactor, Error ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = (Vector(2) << -3.0, 0.0).finished(); + Vector expectedError = Vector2(-3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -131,7 +131,7 @@ TEST( ProjectionFactor, ErrorWithTransform ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = (Vector(2) << -3.0, 0.0).finished(); + Vector expectedError = Vector2(-3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 6ec84a460..2a2e3b40f 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -58,11 +58,11 @@ TEST (RotateFactor, test) { RotateFactor f(1, i1Ri2, c1Zc2, model); EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8)); - Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1).finished()); + Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1)); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - Vector expectedE = (Vector(3) << -0.0248752, 0.202981, -0.0890529).finished(); + Vector expectedE = Vector3(-0.0248752, 0.202981, -0.0890529); #else - Vector expectedE = (Vector(3) << -0.0246305, 0.20197, -0.08867).finished(); + Vector expectedE = Vector3(-0.0246305, 0.20197, -0.08867); #endif EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); @@ -99,7 +99,7 @@ TEST (RotateFactor, minimization) { // Check error at initial estimate Values initial; double degree = M_PI / 180; - Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20).finished()); + Rot3 initialE = iRc.retract(degree * Vector3(20, -20, 20)); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) @@ -128,12 +128,12 @@ TEST (RotateDirectionsFactor, test) { RotateDirectionsFactor f(1, p1, z1, model); EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8)); - Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1).finished()); + Rot3 R = iRc.retract(Vector3(0.1, 0.2, 0.1)); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) - Vector expectedE = (Vector(2) << -0.0890529, -0.202981).finished(); + Vector expectedE = Vector2(-0.0890529, -0.202981); #else - Vector expectedE = (Vector(2) << -0.08867, -0.20197).finished(); + Vector expectedE = Vector2(-0.08867, -0.20197); #endif EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); @@ -173,7 +173,7 @@ TEST (RotateDirectionsFactor, minimization) { // Check error at initial estimate Values initial; double degree = M_PI / 180; - Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20).finished()); + Rot3 initialE = iRc.retract(degree * Vector3(20, -20, 20)); initial.insert(1, initialE); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 63d9a26af..b6d2e2ef8 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -102,7 +102,7 @@ TEST( StereoFactor, Error ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance - Vector expectedError = (Vector(3) << -3.0, +2.0, -1.0).finished(); + Vector expectedError = Vector3(-3.0, +2.0, -1.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -123,7 +123,7 @@ TEST( StereoFactor, ErrorWithTransform ) { Vector actualError(factor.evaluateError(pose, point)); // The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance - Vector expectedError = (Vector(3) << -3.0, +2.0, -1.0).finished(); + Vector expectedError = Vector3(-3.0, +2.0, -1.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); diff --git a/gtsam_unstable/base/tests/testFixedVector.cpp b/gtsam_unstable/base/tests/testFixedVector.cpp index d1db45448..1bae52a80 100644 --- a/gtsam_unstable/base/tests/testFixedVector.cpp +++ b/gtsam_unstable/base/tests/testFixedVector.cpp @@ -37,7 +37,7 @@ TEST( testFixedVector, conversions ) { /* ************************************************************************* */ TEST( testFixedVector, variable_constructor ) { - TestVector3 act((Vector(3) << 1.0, 2.0, 3.0).finished()); + TestVector3 act(Vector3(1.0, 2.0, 3.0)); EXPECT_DOUBLES_EQUAL(1.0, act(0), tol); EXPECT_DOUBLES_EQUAL(2.0, act(1), tol); EXPECT_DOUBLES_EQUAL(3.0, act(2), tol); @@ -45,8 +45,8 @@ TEST( testFixedVector, variable_constructor ) { /* ************************************************************************* */ TEST( testFixedVector, equals ) { - TestVector3 vec1((Vector(3) << 1.0, 2.0, 3.0).finished()), vec2((Vector(3) << 1.0, 2.0, 3.0).finished()), - vec3((Vector(3) << 2.0, 3.0, 4.0).finished()); + TestVector3 vec1(Vector3(1.0, 2.0, 3.0)), vec2(Vector3(1.0, 2.0, 3.0)), + vec3(Vector3(2.0, 3.0, 4.0)); TestVector5 vec4((Vector(5) << 1.0, 2.0, 3.0, 4.0, 5.0).finished()); EXPECT(assert_equal(vec1, vec1, tol)); @@ -61,23 +61,23 @@ TEST( testFixedVector, equals ) { /* ************************************************************************* */ TEST( testFixedVector, static_constructors ) { TestVector3 actZero = TestVector3::zero(); - TestVector3 expZero((Vector(3) << 0.0, 0.0, 0.0).finished()); + TestVector3 expZero(Vector3(0.0, 0.0, 0.0)); EXPECT(assert_equal(expZero, actZero, tol)); TestVector3 actOnes = TestVector3::ones(); - TestVector3 expOnes((Vector(3) << 1.0, 1.0, 1.0).finished()); + TestVector3 expOnes(Vector3(1.0, 1.0, 1.0)); EXPECT(assert_equal(expOnes, actOnes, tol)); TestVector3 actRepeat = TestVector3::repeat(2.3); - TestVector3 expRepeat((Vector(3) << 2.3, 2.3, 2.3).finished()); + TestVector3 expRepeat(Vector3(2.3, 2.3, 2.3)); EXPECT(assert_equal(expRepeat, actRepeat, tol)); TestVector3 actBasis = TestVector3::basis(1); - TestVector3 expBasis((Vector(3) << 0.0, 1.0, 0.0).finished()); + TestVector3 expBasis(Vector3(0.0, 1.0, 0.0)); EXPECT(assert_equal(expBasis, actBasis, tol)); TestVector3 actDelta = TestVector3::delta(1, 2.3); - TestVector3 expDelta((Vector(3) << 0.0, 2.3, 0.0).finished()); + TestVector3 expDelta(Vector3(0.0, 2.3, 0.0)); EXPECT(assert_equal(expDelta, actDelta, tol)); } diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 03edc48de..56850a0fb 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), (Vector)(Vector(3) << 2.0, 2.0, 0.0).finished()), - pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), (Vector)(Vector(3) << 0.0, 0.0, 0.0).finished()), - pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), (Vector)(Vector(3) << 2.0, 2.0, 0.0).finished()); + pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), (Vector)Vector3(2.0, 2.0, 0.0)), + pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), (Vector)Vector3(0.0, 0.0, 0.0)), + pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), (Vector)Vector3(2.0, 2.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Unit::Create(6); @@ -102,9 +102,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vector(3) << 1.0, 0.0, 0.0).finished()), - pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vector(3) << 1.0, 0.0, 0.0).finished()), - pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)(Vector(3) << 1.0, 0.0, 0.0).finished()); + pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)Vector3(1.0, 0.0, 0.0)), + pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)Vector3(1.0, 0.0, 0.0)), + pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)Vector3(1.0, 0.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); diff --git a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp index 418802ea5..0261257be 100644 --- a/gtsam_unstable/dynamics/tests/testPoseRTV.cpp +++ b/gtsam_unstable/dynamics/tests/testPoseRTV.cpp @@ -99,7 +99,7 @@ TEST( testPoseRTV, dynamics_identities ) { PoseRTV x0, x1, x2, x3, x4; const double dt = 0.1; - Vector accel = (Vector(3) << 0.2, 0.0, 0.0).finished(), gyro = (Vector(3) << 0.0, 0.0, 0.2).finished(); + Vector accel = Vector3(0.2, 0.0, 0.0), gyro = Vector3(0.0, 0.0, 0.2); Vector imu01 = zero(6), imu12 = zero(6), imu23 = zero(6), imu34 = zero(6); x1 = x0.generalDynamics(accel, gyro, dt); @@ -181,14 +181,14 @@ TEST( testPoseRTV, transformed_from_2 ) { TEST(testPoseRTV, RRTMbn) { EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(zero(3)), tol)); EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()), tol)); - EXPECT(assert_equal(PoseRTV::RRTMbn((Vector(3) << 0.3, 0.2, 0.1).finished()), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol)); + EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol)); } /* ************************************************************************* */ TEST(testPoseRTV, RRTMnb) { EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(zero(3)), tol)); EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()), tol)); - EXPECT(assert_equal(PoseRTV::RRTMnb((Vector(3) << 0.3, 0.2, 0.1).finished()), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol)); + EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 99f17c002..bdc71fea2 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -25,8 +25,8 @@ Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP)); //Vector6 v2 = Pose3::Logmap(g1.between(g2)); double mass = 100.0; -Vector gamma2 = (Vector(2) << 0.0, 0.0).finished(); // no shape -Vector u2 = (Vector(2) << 0.0, 0.0).finished(); // no control at time 2 +Vector gamma2 = Vector2(0.0, 0.0); // no shape +Vector u2 = Vector2(0.0, 0.0); // no control at time 2 double distT = 1.0; // distance from the body-centered x axis to the big top motor double distR = 5.0; // distance from the body-centered z axis to the small motor Matrix Mass = diag((Vector(3) << mass, mass, mass).finished()); diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index 1d5d1eab5..7e4802393 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp @@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2; const double dt = 1.0; PoseRTV origin, - pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vector(3) << 1.0, 0.0, 0.0).finished()), + pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), (Vector)Vector3(1.0, 0.0, 0.0)), pose1a(Point3(0.5, 0.0, 0.0)), - pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), (Vector)(Vector(3) << 1.0, 0.0, 0.0).finished()); + pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), (Vector)Vector3(1.0, 0.0, 0.0)); /* ************************************************************************* */ TEST( testVelocityConstraint, trapezoidal ) { diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp index 24ddf0ebb..f2fa1b31b 100644 --- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp +++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp @@ -84,7 +84,7 @@ int main(int argc, char** argv) { // Create a prior on the first pose, placing it at the origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1).finished()); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); Key priorKey = 0; newFactors.push_back(PriorFactor(priorKey, priorMean, priorNoise)); newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior @@ -110,11 +110,11 @@ int main(int argc, char** argv) { // Add odometry factors from two different sources with different error stats Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05).finished()); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05).finished()); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Unlike the fixed-lag versions, the concurrent filter implementation @@ -159,7 +159,7 @@ int main(int argc, char** argv) { Key loopKey1(1000 * (0.0)); Key loopKey2(1000 * (5.0)); Pose2 loopMeasurement = Pose2(9.5, 1.00, 0.00); - noiseModel::Diagonal::shared_ptr loopNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.25).finished()); + noiseModel::Diagonal::shared_ptr loopNoise = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.25)); NonlinearFactor::shared_ptr loopFactor(new BetweenFactor(loopKey1, loopKey2, loopMeasurement, loopNoise)); // This measurement cannot be added directly to the concurrent filter because it connects a filter state to a smoother state @@ -189,11 +189,11 @@ int main(int argc, char** argv) { // Add odometry factors from two different sources with different error stats Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05).finished()); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05).finished()); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Unlike the fixed-lag versions, the concurrent filter implementation @@ -262,11 +262,11 @@ int main(int argc, char** argv) { // Add odometry factors from two different sources with different error stats Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05).finished()); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05).finished()); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Unlike the fixed-lag versions, the concurrent filter implementation diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index 8bd23994b..ea1381bab 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -78,7 +78,7 @@ int main(int argc, char** argv) { // Create a prior on the first pose, placing it at the origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1).finished()); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); Key priorKey = 0; newFactors.push_back(PriorFactor(priorKey, priorMean, priorNoise)); newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior @@ -104,11 +104,11 @@ int main(int argc, char** argv) { // Add odometry factors from two different sources with different error stats Pose2 odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05).finished()); + noiseModel::Diagonal::shared_ptr odometryNoise1 = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); Pose2 odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 0.05).finished()); + noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Update the smoothers with the new factors diff --git a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp b/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp index f257205eb..3a3b97b77 100644 --- a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp +++ b/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp @@ -41,11 +41,11 @@ int main(int argc, char** argv) { Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5); // 2a. Add a prior on the first pose, setting it to the origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1).finished()); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.push_back(ExpressionFactor(priorNoise, Pose2(0, 0, 0), x1)); // For simplicity, we will use the same noise model for odometry and loop closures - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished()); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors graph.push_back(ExpressionFactor(model, Pose2(2, 0, 0 ), between(x1,x2))); diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index 5795d6f5e..25dd5fe98 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -60,8 +60,8 @@ TEST( BatchFixedLagSmoother, Example ) // SETDEBUG("BatchFixedLagSmoother calculateMarginalFactors", true); // Set up parameters - SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1).finished()); - SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1).finished()); + SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); + SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // Create a Fixed-Lag Smoother typedef BatchFixedLagSmoother::KeyTimestampMap Timestamps; diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index 7f27225d2..d359d0eff 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -39,8 +39,8 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75).finished()), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1).finished()), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx(Vector3(0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx(Vector3(0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index 4febb8576..51c2a55a2 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -39,8 +39,8 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75).finished()), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1).finished()), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx(Vector3(0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx(Vector3(0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index 7ce4bfe20..08dbc311c 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -38,9 +38,9 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75).finished()), Point3(1.0, -0.25, 0.10) ); -//const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1).finished()), Point3(0.05, -0.05, 0.02) ); -const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.1, 0.02, -0.1).finished()), Point3(0.5, -0.05, 0.2) ); +const Pose3 poseOdometry( Rot3::RzRyRx(Vector3(0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +//const Pose3 poseError( Rot3::RzRyRx(Vector3(0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseError( Rot3::RzRyRx(Vector3(0.1, 0.02, -0.1)), Point3(0.5, -0.05, 0.2) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index 97036d241..5f91527e6 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -39,8 +39,8 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75).finished()), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1).finished()), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx(Vector3(0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx(Vector3(0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 0ef569c64..9708eedb5 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -38,8 +38,8 @@ namespace { // Set up initial pose, odometry difference, loop closure difference, and initialization errors const Pose3 poseInitial; -const Pose3 poseOdometry( Rot3::RzRyRx((Vector(3) << 0.05, 0.10, -0.75).finished()), Point3(1.0, -0.25, 0.10) ); -const Pose3 poseError( Rot3::RzRyRx((Vector(3) << 0.01, 0.02, -0.1).finished()), Point3(0.05, -0.05, 0.02) ); +const Pose3 poseOdometry( Rot3::RzRyRx(Vector3(0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx(Vector3(0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); // Set up noise models for the factors const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index b40777eb0..e609af953 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -111,7 +111,7 @@ TEST(ExpressionFactor, Unary) { JacobianFactor expected( // 2, (Matrix(2, 3) << 1, 0, 0, 0, 1, 0).finished(), // - (Vector(2) << -17, 30).finished()); + Vector2(-17, 30)); // Create leaves Point3_ p(2); diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 1aa2c78b2..832d37d14 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -59,8 +59,8 @@ TEST( IncrementalFixedLagSmoother, Example ) SETDEBUG("IncrementalFixedLagSmoother update", true); // Set up parameters - SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1).finished()); - SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1).finished()); + SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); + SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // Create a Fixed-Lag Smoother typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps; diff --git a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp index 8fb04b5d0..bfd3c9168 100644 --- a/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testParticleFactor.cpp @@ -128,7 +128,7 @@ TEST( ParticleFilter, linear1 ) { // Create the controls and measurement properties for our example Matrix F = eye(2, 2); Matrix B = eye(2, 2); - Vector u = (Vector(2) << 1.0, 0.0).finished(); + Vector u = Vector2(1.0, 0.0); SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1); Matrix Q = 0.01 * eye(2, 2); Matrix H = eye(2, 2); diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 60eaa3594..2a756b5af 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -221,8 +221,8 @@ std::pair AHRS::aidGeneral( Matrix b_g = skewSymmetric(increment* f_previous); Matrix H = collect(3, &b_g, &I3, &Z3); // Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_)); -// Matrix R = diag((Vector(3) << 1.0, 0.2, 1.0).finished()); // good for L_twice - Matrix R = diag((Vector(3) << 0.01, 0.0001, 0.01).finished()); +// Matrix R = diag(Vector3(1.0, 0.2, 1.0)); // good for L_twice + Matrix R = diag(Vector3(0.01, 0.0001, 0.01)); // update the Kalman filter KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R); diff --git a/gtsam_unstable/slam/tests/testAHRS.cpp b/gtsam_unstable/slam/tests/testAHRS.cpp index c0bddbbd9..44f516ae9 100644 --- a/gtsam_unstable/slam/tests/testAHRS.cpp +++ b/gtsam_unstable/slam/tests/testAHRS.cpp @@ -75,7 +75,7 @@ TEST (AHRS, Mechanization_integrate) { Mechanization_bRn2 mech; KalmanFilter::State state; // boost::tie(mech,state) = ahrs.initialize(g_e); -// Vector u = (Vector(3) << 0.05,0.0,0.0).finished(); +// Vector u = Vector3(0.05,0.0,0.0); // double dt = 2; // Rot3 expected; // Mechanization_bRn2 mech2 = mech.integrate(u,dt); diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index e860da9ec..5bb4dfd2e 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -56,8 +56,8 @@ TEST( BetweenFactorEM, ConstructorAndEquals) gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.5, 0.5, 0.05).finished())); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 5, 5, 1.0).finished())); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(5, 5, 1.0))); double prior_outlier = 0.5; double prior_inlier = 0.5; @@ -85,8 +85,8 @@ TEST( BetweenFactorEM, EvaluateError) gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.5, 0.5, 0.05).finished())); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0).finished())); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0))); gtsam::Values values; values.insert(key1, p1); @@ -157,8 +157,8 @@ TEST (BetweenFactorEM, jacobian ) { gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.5, 0.5, 0.05).finished())); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0).finished())); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0))); gtsam::Values values; values.insert(key1, p1); @@ -220,8 +220,8 @@ TEST( BetweenFactorEM, CaseStudy) gtsam::Pose2 p2(-0.0491752554, -0.289649075, -0.328993962); gtsam::Pose2 rel_pose_msr(0.0316191379, 0.0247539161, 0.004102182); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.4021, 0.286, 0.428).finished())); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 4.9821, 4.614, 1.8387).finished())); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.4021, 0.286, 0.428))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(4.9821, 4.614, 1.8387))); gtsam::Values values; values.insert(key1, p1); diff --git a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp index 70495edb3..d8a3ba0c1 100644 --- a/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp +++ b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp @@ -29,7 +29,7 @@ TEST(BiasedGPSFactor, errorNoiseless) { Point3 measured = t + noise; BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); - Vector expectedError = (Vector(3) << 0.0, 0.0, 0.0 ).finished(); + Vector expectedError = Vector3(0.0, 0.0, 0.0 ); Vector actualError = factor.evaluateError(pose,bias); EXPECT(assert_equal(expectedError,actualError, 1E-5)); } @@ -44,7 +44,7 @@ TEST(BiasedGPSFactor, errorNoisy) { Point3 measured = t - noise; BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); - Vector expectedError = (Vector(3) << 1.0, 2.0, 3.0 ).finished(); + Vector expectedError = Vector3(1.0, 2.0, 3.0 ); Vector actualError = factor.evaluateError(pose,bias); EXPECT(assert_equal(expectedError,actualError, 1E-5)); } diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 7759b0cf6..476447f8b 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -37,9 +37,9 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor) Key biasKey1(31); // IMU accumulation variables - Vector delta_pos_in_t0 = (Vector(3) << 0.0, 0.0, 0.0).finished(); - Vector delta_vel_in_t0 = (Vector(3) << 0.0, 0.0, 0.0).finished(); - Vector delta_angles = (Vector(3) << 0.0, 0.0, 0.0).finished(); + Vector delta_pos_in_t0 = Vector3(0.0, 0.0, 0.0); + Vector delta_vel_in_t0 = Vector3(0.0, 0.0, 0.0); + Vector delta_angles = Vector3(0.0, 0.0, 0.0); double delta_t = 0.0; Matrix EquivCov_Overall = zeros(15,15); Matrix Jacobian_wrt_t0_Overall = eye(15); diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index 1de95f4cc..ff7307840 100644 --- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -42,7 +42,7 @@ TEST( GaussMarkovFactor, equals ) Key x1(1); Key x2(2); double delta_t = 0.10; - Vector tau = (Vector(3) << 100.0, 150.0, 10.0).finished(); + Vector tau = Vector3(100.0, 150.0, 10.0); SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0); GaussMarkovFactor factor1(x1, x2, delta_t, tau, model); @@ -58,11 +58,11 @@ TEST( GaussMarkovFactor, error ) Key x1(1); Key x2(2); double delta_t = 0.10; - Vector tau = (Vector(3) << 100.0, 150.0, 10.0).finished(); + Vector tau = Vector3(100.0, 150.0, 10.0); SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0); - LieVector v1 = LieVector((Vector(3) << 10.0, 12.0, 13.0).finished()); - LieVector v2 = LieVector((Vector(3) << 10.0, 15.0, 14.0).finished()); + LieVector v1 = LieVector(Vector3(10.0, 12.0, 13.0)); + LieVector v2 = LieVector(Vector3(10.0, 15.0, 14.0)); // Create two nodes linPoint.insert(x1, v1); @@ -90,14 +90,14 @@ TEST (GaussMarkovFactor, jacobian ) { Key x1(1); Key x2(2); double delta_t = 0.10; - Vector tau = (Vector(3) << 100.0, 150.0, 10.0).finished(); + Vector tau = Vector3(100.0, 150.0, 10.0); SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0); GaussMarkovFactor factor(x1, x2, delta_t, tau, model); // Update the linearization point - LieVector v1_upd = LieVector((Vector(3) << 0.5, -0.7, 0.3).finished()); - LieVector v2_upd = LieVector((Vector(3) << -0.7, 0.4, 0.9).finished()); + LieVector v1_upd = LieVector(Vector3(0.5, -0.7, 0.3)); + LieVector v2_upd = LieVector(Vector3(-0.7, 0.4, 0.9)); // Calculate the Jacobian matrix using the factor Matrix computed_H1, computed_H2; diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 1f2dc164c..6cfcd93e6 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -104,8 +104,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // First test: zero angular motion, some acceleration - Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.3 - 9.81).finished()); - Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0).finished()); + Vector measurement_acc(Vector3(0.1, 0.2, 0.3 - 9.81)); + Vector measurement_gyro(Vector3(0.0, 0.0, 0.0)); InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -113,10 +113,10 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, model); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40).finished()); + Vector3 Vel1(Vector3(0.50, -0.50, 0.40)); imuBias::ConstantBias Bias1; Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); - Vector3 expectedVel2((Vector(3) << 0.51, -0.48, 0.43).finished()); + Vector3 expectedVel2(Vector3(0.51, -0.48, 0.43)); Pose3 actualPose2; Vector3 actualVel2; f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); @@ -137,8 +137,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // First test: zero angular motion, some acceleration - Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.3 - 9.81).finished()); - Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0).finished()); + Vector measurement_acc(Vector3(0.1, 0.2, 0.3 - 9.81)); + Vector measurement_gyro(Vector3(0.0, 0.0, 0.0)); InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -147,8 +147,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); - Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40).finished()); - Vector3 Vel2((Vector(3) << 0.51, -0.48, 0.43).finished()); + Vector3 Vel1(Vector3(0.50, -0.50, 0.40)); + Vector3 Vel2(Vector3(0.51, -0.48, 0.43)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -169,8 +169,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // Second test: zero angular motion, some acceleration - Vector measurement_acc((Vector(3) << 0.0, 0.0, 0.0 - 9.81).finished()); - Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3).finished()); + Vector measurement_acc(Vector3(0.0, 0.0, 0.0 - 9.81)); + Vector measurement_gyro(Vector3(0.1, 0.2, 0.3)); InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -180,8 +180,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Pose3 Pose1(Rot3(), Point3(2.0, 1.0, 3.0)); Pose3 Pose2(Rot3::Expmap(measurement_gyro * measurement_dt), Point3(2.0, 1.0, 3.0)); - Vector3 Vel1((Vector(3) << 0.0, 0.0, 0.0).finished()); - Vector3 Vel2((Vector(3) << 0.0, 0.0, 0.0).finished()); + Vector3 Vel1(Vector3(0.0, 0.0, 0.0)); + Vector3 Vel2(Vector3(0.0, 0.0, 0.0)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -203,8 +203,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, // Second test: zero angular motion, some acceleration - generated in matlab Vector measurement_acc( - (Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343).finished()); - Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3).finished()); + Vector3(6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro(Vector3(0.1, 0.2, 0.3)); InertialNavFactor_GlobalVelocity f( PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, @@ -215,7 +215,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, -0.427669306, -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0, 1.0, 3.0); Pose3 Pose1(R1, t1); - Vector3 Vel1((Vector(3) << 0.5, -0.5, 0.4).finished()); + Vector3 Vel1(Vector3(0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt)); @@ -237,9 +237,9 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, //} // //TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) { -// Vector3 angles((Vector(3) << 3.001, -1.0004, 2.0005).finished()); +// Vector3 angles(Vector3(3.001, -1.0004, 2.0005)); // Rot3 R1(Rot3().RzRyRx(angles)); -// Vector3 q((Vector(3) << 5.8, -2.2, 4.105).finished()); +// Vector3 q(Vector3(5.8, -2.2, 4.105)); // Rot3 qx(0.0, -q[2], q[1], // q[2], 0.0, -q[0], // -q[1], q[0],0.0); @@ -271,7 +271,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); Vector measurement_acc( - (Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343).finished()); + Vector3(6.501390843381716, -6.763926150509185, -2.300389940090343)); Vector measurement_gyro((Vector(3) << 3.14, 3.14 / 2, -3.14).finished()); InertialNavFactor_GlobalVelocity factor( @@ -283,13 +283,13 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, -0.427669306, -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0, 1.0, 3.0); Pose3 Pose1(R1, t1); - Vector3 Vel1((Vector(3) << 0.5, -0.5, 0.4).finished()); + Vector3 Vel1(Vector3(0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); Pose3 Pose2(R2, t2); Vector3 Vel2( - (Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000).finished()); + Vector3(0.510000000000000, -0.480000000000000, 0.430000000000000)); imuBias::ConstantBias Bias1; Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; @@ -374,8 +374,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Key Vel2(22); Key Bias1(31); - Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4).finished()); - Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03).finished()); + Vector measurement_acc(Vector3(0.1, 0.2, 0.4)); + Vector measurement_gyro(Vector3(-0.2, 0.5, 0.03)); double measurement_dt(0.1); @@ -396,8 +396,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Key Vel2(22); Key Bias1(31); - Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4).finished()); - Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03).finished()); + Vector measurement_acc(Vector3(0.1, 0.2, 0.4)); + Vector measurement_gyro(Vector3(-0.2, 0.5, 0.03)); double measurement_dt(0.1); @@ -430,9 +430,9 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation // First test: zero angular motion, some acceleration - Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0).finished()); // Measured in ENU orientation + Vector measurement_gyro(Vector3(0.0, 0.0, 0.0)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3 + 9.81).finished() + Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() * body_P_sensor.translation().vector(); // Measured in ENU orientation @@ -443,10 +443,10 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, model, body_P_sensor); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); - Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40).finished()); + Vector3 Vel1(Vector3(0.50, -0.50, 0.40)); imuBias::ConstantBias Bias1; Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); - Vector3 expectedVel2((Vector(3) << 0.51, -0.48, 0.43).finished()); + Vector3 expectedVel2(Vector3(0.51, -0.48, 0.43)); Pose3 actualPose2; Vector3 actualVel2; f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); @@ -469,9 +469,9 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation // First test: zero angular motion, some acceleration - Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0).finished()); // Measured in ENU orientation + Vector measurement_gyro(Vector3(0.0, 0.0, 0.0)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3 + 9.81).finished() + Vector measurement_acc = Vector3(0.2, 0.1, -0.3 + 9.81) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() * body_P_sensor.translation().vector(); // Measured in ENU orientation @@ -483,8 +483,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); - Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40).finished()); - Vector3 Vel2((Vector(3) << 0.51, -0.48, 0.43).finished()); + Vector3 Vel1(Vector3(0.50, -0.50, 0.40)); + Vector3 Vel2(Vector3(0.51, -0.48, 0.43)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -507,9 +507,9 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation // Second test: zero angular motion, some acceleration - Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3).finished()); // Measured in ENU orientation + Vector measurement_gyro(Vector3(0.2, 0.1, -0.3)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); - Vector measurement_acc = (Vector(3) << 0.0, 0.0, 0.0 + 9.81).finished() + Vector measurement_acc = Vector3(0.0, 0.0, 0.0 + 9.81) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() * body_P_sensor.translation().vector(); // Measured in ENU orientation @@ -524,8 +524,8 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Rot3::Expmap( body_P_sensor.rotation().matrix() * measurement_gyro * measurement_dt), Point3(2.0, 1.0, 3.0)); - Vector3 Vel1((Vector(3) << 0.0, 0.0, 0.0).finished()); - Vector3 Vel2((Vector(3) << 0.0, 0.0, 0.0).finished()); + Vector3 Vel1(Vector3(0.0, 0.0, 0.0)); + Vector3 Vel2(Vector3(0.0, 0.0, 0.0)); imuBias::ConstantBias Bias1; Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); @@ -548,10 +548,10 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation // Second test: zero angular motion, some acceleration - generated in matlab - Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3).finished()); // Measured in ENU orientation + Vector measurement_gyro(Vector3(0.2, 0.1, -0.3)); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); Vector measurement_acc = - (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343).finished() + Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() * body_P_sensor.translation().vector(); // Measured in ENU orientation @@ -565,7 +565,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, -0.427669306, -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0, 1.0, 3.0); Pose3 Pose1(R1, t1); - Vector3 Vel1((Vector(3) << 0.5, -0.5, 0.4).finished()); + Vector3 Vel1(Vector3(0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt)); @@ -573,7 +573,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector dv = measurement_dt * (R1.matrix() * body_P_sensor.rotation().matrix() - * (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343).finished() + * Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343) + world_g); Vector3 Vel2 = Vel1 + dv; imuBias::ConstantBias Bias1; @@ -602,7 +602,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector measurement_gyro((Vector(3) << 3.14 / 2, 3.14, +3.14).finished()); // Measured in ENU orientation Matrix omega__cross = skewSymmetric(measurement_gyro); Vector measurement_acc = - (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343).finished() + Vector3(-6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross * omega__cross * body_P_sensor.rotation().inverse().matrix() * body_P_sensor.translation().vector(); // Measured in ENU orientation diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index 48b800eda..2c85b3dad 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -66,7 +66,7 @@ TEST( InvDepthFactorVariant2, optimize) { Values values; values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30).finished())); values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30).finished())); - values.insert(landmarkKey, Vector3(expected + (Vector(3) << +0.02, -0.04, +0.05).finished())); + values.insert(landmarkKey, Vector3(expected + Vector3(+0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index 78f6dbade..ec9aa90c3 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -66,7 +66,7 @@ TEST( InvDepthFactorVariant3, optimize) { Values values; values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30).finished())); values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30).finished())); - values.insert(landmarkKey, Vector3(expected + (Vector(3) << +0.02, -0.04, +0.05).finished())); + values.insert(landmarkKey, Vector3(expected + Vector3(+0.02, -0.04, +0.05))); // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp index 7cd10ef8c..2794e5707 100644 --- a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp @@ -147,7 +147,7 @@ TEST( MultiProjectionFactor, create ){ // Vector actualError(factor.evaluateError(pose, point)); // // // The expected error is (-3.0, 0.0) pixels / UnitCovariance -// Vector expectedError = (Vector(2) << -3.0, 0.0).finished(); +// Vector expectedError = Vector2(-3.0, 0.0); // // // Verify we get the expected error // CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -170,7 +170,7 @@ TEST( MultiProjectionFactor, create ){ // Vector actualError(factor.evaluateError(pose, point)); // // // The expected error is (-3.0, 0.0) pixels / UnitCovariance -// Vector expectedError = (Vector(2) << -3.0, 0.0).finished(); +// Vector expectedError = Vector2(-3.0, 0.0); // // // Verify we get the expected error // CHECK(assert_equal(expectedError, actualError, 1e-9)); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 94f42da22..0e5f6421f 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -114,7 +114,7 @@ TEST( ProjectionFactorPPP, Error ) { Vector actualError(factor.evaluateError(pose, Pose3(), point)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = (Vector(2) << -3.0, 0.0).finished(); + Vector expectedError = Vector2(-3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -138,7 +138,7 @@ TEST( ProjectionFactorPPP, ErrorWithTransform ) { Vector actualError(factor.evaluateError(pose, transform, point)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = (Vector(2) << -3.0, 0.0).finished(); + Vector expectedError = Vector2(-3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp index c5196c959..e878ea5d9 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp @@ -86,7 +86,7 @@ TEST( ProjectionFactorPPPC, Error ) { Vector actualError(factor.evaluateError(pose, Pose3(), point, *K1)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = (Vector(2) << -3.0, 0.0).finished(); + Vector expectedError = Vector2(-3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); @@ -107,7 +107,7 @@ TEST( ProjectionFactorPPPC, ErrorWithTransform ) { Vector actualError(factor.evaluateError(pose, transform, point, *K1)); // The expected error is (-3.0, 0.0) pixels / UnitCovariance - Vector expectedError = (Vector(2) << -3.0, 0.0).finished(); + Vector expectedError = Vector2(-3.0, 0.0); // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp index f56709b23..d8d720e83 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -52,7 +52,7 @@ TEST( TransformBtwRobotsUnaryFactor, ConstructorAndEquals) gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05).finished())); + SharedGaussian model(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); gtsam::Values valA, valB; valA.insert(keyA, p1); @@ -83,7 +83,7 @@ TEST( TransformBtwRobotsUnaryFactor, unwhitenedError) gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal; - SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05).finished())); + SharedGaussian model(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); gtsam::Values valA, valB; valA.insert(keyA, orgA_T_1); @@ -117,7 +117,7 @@ TEST( TransformBtwRobotsUnaryFactor, unwhitenedError2) gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(orgA_T_currB); gtsam::Pose2 rel_pose_msr = rel_pose_ideal; - SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05).finished())); + SharedGaussian model(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); gtsam::Values valA, valB; valA.insert(keyA, orgA_T_currA); @@ -160,7 +160,7 @@ TEST( TransformBtwRobotsUnaryFactor, Optimize) gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1; gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1; - SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05).finished())); + SharedGaussian model(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); gtsam::Values valA, valB; valA.insert(keyA, orgA_T_currA); @@ -213,7 +213,7 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian) gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05).finished())); + SharedGaussian model(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); gtsam::Values valA, valB; valA.insert(keyA, orgA_T_1); @@ -251,8 +251,8 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian) // gtsam::Pose2 rel_pose_ideal = p1.between(p2); // gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); // -// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.5, 0.5, 0.05).finished())); -// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0).finished())); +// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05))); +// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0))); // // gtsam::Values values; // values.insert(keyA, p1); diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index 6e5c41c4a..dbd90f3a3 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -52,8 +52,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, ConstructorAndEquals) gtsam::Pose2 rel_pose_ideal = p1.between(p2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05).finished())); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0).finished())); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(5, 5, 1.0))); double prior_outlier = 0.5; double prior_inlier = 0.5; @@ -89,8 +89,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError) gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal; - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05).finished())); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0).finished())); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(5, 5, 1.0))); double prior_outlier = 0.01; double prior_inlier = 0.99; @@ -128,8 +128,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, unwhitenedError2) gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(orgA_T_currB); gtsam::Pose2 rel_pose_msr = rel_pose_ideal; - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05).finished())); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0).finished())); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(5, 5, 1.0))); double prior_outlier = 0.01; double prior_inlier = 0.99; @@ -176,8 +176,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, Optimize) gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1; gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1; - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05).finished())); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0).finished())); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(5, 5, 1.0))); double prior_outlier = 0.01; double prior_inlier = 0.99; @@ -237,8 +237,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2); gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05).finished())); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas((Vector(3) << 5, 5, 1.0).finished())); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(Vector3(5, 5, 1.0))); double prior_outlier = 0.5; double prior_inlier = 0.5; @@ -276,8 +276,8 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) // gtsam::Pose2 rel_pose_ideal = p1.between(p2); // gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); // -// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.5, 0.5, 0.05).finished())); -// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0).finished())); +// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.5, 0.5, 0.05))); +// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::Vector3(50.0, 50.0, 10.0))); // // gtsam::Values values; // values.insert(keyA, p1); diff --git a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp index d11dc4c08..5227f8786 100644 --- a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp @@ -53,7 +53,7 @@ int main() { gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vector(3) << 0.0, 0.0, 9.81).finished()); + Vector world_g(Vector3(0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0).finished()); // NED system gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5).finished()); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); @@ -61,8 +61,8 @@ int main() { SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // Second test: zero angular motion, some acceleration - generated in matlab - Vector measurement_acc((Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343).finished()); - Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3).finished()); + Vector measurement_acc(Vector3(6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro(Vector3(0.1, 0.2, 0.3)); InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); @@ -71,7 +71,7 @@ int main() { -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - Vector3 Vel1 = Vector((Vector(3) << 0.5,-0.5,0.4).finished()); + Vector3 Vel1 = Vector(Vector3(0.5,-0.5,0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); diff --git a/tests/smallExample.h b/tests/smallExample.h index d0bddf0f9..b7cb7aefe 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -222,9 +222,9 @@ inline Values createValues() { inline VectorValues createVectorValues() { using namespace impl; VectorValues c = boost::assign::pair_list_of - (_l1_, (Vector(2) << 0.0, -1.0).finished()) - (_x1_, (Vector(2) << 0.0, 0.0).finished()) - (_x2_, (Vector(2) << 1.5, 0.0).finished()); + (_l1_, Vector2(0.0, -1.0)) + (_x1_, Vector2(0.0, 0.0)) + (_x2_, Vector2(1.5, 0.0)); return c; } @@ -249,9 +249,9 @@ inline VectorValues createCorrectDelta() { using symbol_shorthand::X; using symbol_shorthand::L; VectorValues c; - c.insert(L(1), (Vector(2) << -0.1, 0.1).finished()); - c.insert(X(1), (Vector(2) << -0.1, -0.1).finished()); - c.insert(X(2), (Vector(2) << 0.1, -0.2).finished()); + c.insert(L(1), Vector2(-0.1, 0.1)); + c.insert(X(1), Vector2(-0.1, -0.1)); + c.insert(X(2), Vector2(0.1, -0.2)); return c; } @@ -277,13 +277,13 @@ inline GaussianFactorGraph createGaussianFactorGraph() { fg += JacobianFactor(X(1), 10*eye(2), -1.0*ones(2)); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(X(1), -10*eye(2), X(2), 10*eye(2), (Vector(2) << 2.0, -1.0).finished()); + fg += JacobianFactor(X(1), -10*eye(2), X(2), 10*eye(2), Vector2(2.0, -1.0)); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(X(1), -5*eye(2), L(1), 5*eye(2), (Vector(2) << 0.0, 1.0).finished()); + fg += JacobianFactor(X(1), -5*eye(2), L(1), 5*eye(2), Vector2(0.0, 1.0)); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(X(2), -5*eye(2), L(1), 5*eye(2), (Vector(2) << -1.0, 1.5).finished()); + fg += JacobianFactor(X(2), -5*eye(2), L(1), 5*eye(2), Vector2(-1.0, 1.5)); return fg; } @@ -349,7 +349,7 @@ inline boost::shared_ptr sharedReallyNonlinearFactor using symbol_shorthand::X; using symbol_shorthand::L; boost::shared_ptr fg(new NonlinearFactorGraph); - Vector z = (Vector(2) << 1.0, 0.0).finished(); + Vector z = Vector2(1.0, 0.0); double sigma = 0.1; boost::shared_ptr factor( new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); @@ -421,7 +421,7 @@ inline GaussianFactorGraph createSimpleConstraintGraph() { // |0 1||x_2| | 0 -1||y_2| |0| Matrix Ax1 = eye(2); Matrix Ay1 = eye(2) * -1; - Vector b2 = (Vector(2) << 0.0, 0.0).finished(); + Vector b2 = Vector2(0.0, 0.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); @@ -439,7 +439,7 @@ inline VectorValues createSimpleConstraintValues() { using symbol_shorthand::X; using symbol_shorthand::L; VectorValues config; - Vector v = (Vector(2) << 1.0, -1.0).finished(); + Vector v = Vector2(1.0, -1.0); config.insert(_x_, v); config.insert(_y_, v); return config; @@ -467,7 +467,7 @@ inline GaussianFactorGraph createSingleConstraintGraph() { Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0; Matrix Ay1 = eye(2) * 10; - Vector b2 = (Vector(2) << 1.0, 2.0).finished(); + Vector b2 = Vector2(1.0, 2.0); JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, constraintModel)); @@ -483,8 +483,8 @@ inline GaussianFactorGraph createSingleConstraintGraph() { inline VectorValues createSingleConstraintValues() { using namespace impl; VectorValues config = boost::assign::pair_list_of - (_x_, (Vector(2) << 1.0, -1.0).finished()) - (_y_, (Vector(2) << 0.2, 0.1).finished()); + (_x_, Vector2(1.0, -1.0)) + (_y_, Vector2(0.2, 0.1)); return config; } @@ -493,7 +493,7 @@ inline GaussianFactorGraph createMultiConstraintGraph() { using namespace impl; // unary factor 1 Matrix A = eye(2); - Vector b = (Vector(2) << -2.0, 2.0).finished(); + Vector b = Vector2(-2.0, 2.0); JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1)); // constraint 1 @@ -547,9 +547,9 @@ inline GaussianFactorGraph createMultiConstraintGraph() { inline VectorValues createMultiConstraintValues() { using namespace impl; VectorValues config = boost::assign::pair_list_of - (_x_, (Vector(2) << -2.0, 2.0).finished()) - (_y_, (Vector(2) << -0.1, 0.4).finished()) - (_z_, (Vector(2) <<-4.0, 5.0).finished()); + (_x_, Vector2(-2.0, 2.0)) + (_y_, Vector2(-0.1, 0.4)) + (_z_, Vector2(-4.0, 5.0)); return config; } diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 86a337cc5..a2bb57e1c 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -48,21 +48,21 @@ TEST(DoglegOptimizer, ComputeBlend) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 0, (Vector(2) << 1.0,2.0).finished(), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(), + 0, Vector2(1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(), 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(), 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 1, (Vector(2) << 15.0,16.0).finished(), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(), + 1, Vector2(15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(), 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(), 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 2, (Vector(2) << 29.0,30.0).finished(), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(), + 2, Vector2(29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(), 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 3, (Vector(2) << 39.0,40.0).finished(), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(), + 3, Vector2(39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(), 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, (Vector(2) << 49.0,50.0).finished(), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished())); + 4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished())); // Compute steepest descent point VectorValues xu = gbn.optimizeGradientSearch(); @@ -84,21 +84,21 @@ TEST(DoglegOptimizer, ComputeDoglegPoint) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 0, (Vector(2) << 1.0,2.0).finished(), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(), + 0, Vector2(1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(), 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(), 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 1, (Vector(2) << 15.0,16.0).finished(), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(), + 1, Vector2(15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(), 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(), 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 2, (Vector(2) << 29.0,30.0).finished(), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(), + 2, Vector2(29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(), 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 3, (Vector(2) << 39.0,40.0).finished(), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(), + 3, Vector2(39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(), 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished())); gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, (Vector(2) << 49.0,50.0).finished(), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished())); + 4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished())); // Compute dogleg point for different deltas diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index d2864d028..8748e7464 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -37,20 +37,20 @@ TEST( ExtendedKalmanFilter, linear ) { // Create the Kalman Filter initialization point Point2 x_initial(0.0, 0.0); - SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1).finished()); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // Create an ExtendedKalmanFilter object ExtendedKalmanFilter ekf(x0, x_initial, P_initial); // Create the controls and measurement properties for our example double dt = 1.0; - Vector u = (Vector(2) << 1.0, 0.0).finished(); + Vector u = Vector2(1.0, 0.0); Point2 difference(u*dt); - SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1).finished(), true); + SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1), true); Point2 z1(1.0, 0.0); Point2 z2(2.0, 0.0); Point2 z3(3.0, 0.0); - SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vector(2) << 0.25, 0.25).finished(), true); + SharedDiagonal R = noiseModel::Diagonal::Sigmas(Vector2(0.25, 0.25), true); // Create the set of expected output TestValues Point2 expected1(1.0, 0.0); @@ -107,7 +107,7 @@ public: NonlinearMotionModel(){} NonlinearMotionModel(const Symbol& TestKey1, const Symbol& TestKey2) : - Base(noiseModel::Diagonal::Sigmas((Vector(2) << 1.0, 1.0).finished()), TestKey1, TestKey2), Q_(2,2) { + Base(noiseModel::Diagonal::Sigmas(Vector2(1.0, 1.0)), TestKey1, TestKey2), Q_(2,2) { // Initialize motion model parameters: // w is vector of motion noise sigmas. The final covariance is calculated as G*w*w'*G' @@ -403,7 +403,7 @@ TEST( ExtendedKalmanFilter, nonlinear ) { // Create the Kalman Filter initialization point Point2 x_initial(0.90, 1.10); - SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1).finished()); + SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // Create an ExtendedKalmanFilter object ExtendedKalmanFilter ekf(X(0), x_initial, P_initial); diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 24e1baed5..3a0081bdb 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -79,7 +79,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1 ) // create expected Conditional Gaussian Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; - Vector d = (Vector(2) << -0.133333, -0.0222222).finished(); + Vector d = Vector2(-0.133333, -0.0222222); GaussianConditional expected(X(1),15*d,R11,L(1),S12,X(2),S13); EXPECT(assert_equal(expected,*conditional,tol)); @@ -97,7 +97,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2 ) // create expected Conditional Gaussian double sig = 0.0894427; Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; - Vector d = (Vector(2) << 0.2, -0.14).finished()/sig, sigma = ones(2); + Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2); GaussianConditional expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -113,7 +113,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1 ) // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; - Vector d = (Vector(2) << -0.1, 0.25).finished()/sig, sigma = ones(2); + Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -130,7 +130,7 @@ TEST( GaussianFactorGraph, eliminateOne_x1_fast ) // create expected Conditional Gaussian Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; - Vector d = (Vector(2) << -0.133333, -0.0222222).finished(), sigma = ones(2); + Vector d = Vector2(-0.133333, -0.0222222), sigma = ones(2); GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma); // Create expected remaining new factor @@ -160,7 +160,7 @@ TEST( GaussianFactorGraph, eliminateOne_x2_fast ) // create expected Conditional Gaussian double sig = 0.0894427; Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; - Vector d = (Vector(2) << 0.2, -0.14).finished()/sig, sigma = ones(2); + Vector d = Vector2(0.2, -0.14)/sig, sigma = ones(2); GaussianConditional expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -176,7 +176,7 @@ TEST( GaussianFactorGraph, eliminateOne_l1_fast ) // create expected Conditional Gaussian double sig = sqrt(2.0)/10.; Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; - Vector d = (Vector(2) << -0.1, 0.25).finished()/sig, sigma = ones(2); + Vector d = Vector2(-0.1, 0.25)/sig, sigma = ones(2); GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma); EXPECT(assert_equal(expected,*actual,tol)); @@ -191,15 +191,15 @@ TEST( GaussianFactorGraph, eliminateAll ) Ordering ordering; ordering += X(2),L(1),X(1); - Vector d1 = (Vector(2) << -0.1,-0.1).finished(); + Vector d1 = Vector2(-0.1,-0.1); GaussianBayesNet expected = simpleGaussian(ordering[X(1)],d1,0.1); double sig1 = 0.149071; - Vector d2 = (Vector(2) << 0.0, 0.2).finished()/sig1, sigma2 = ones(2); + Vector d2 = Vector2(0.0, 0.2)/sig1, sigma2 = ones(2); push_front(expected,ordering[L(1)],d2, I/sig1,ordering[X(1)], (-1)*I/sig1,sigma2); double sig2 = 0.0894427; - Vector d3 = (Vector(2) << 0.2, -0.14).finished()/sig2, sigma3 = ones(2); + Vector d3 = Vector2(0.2, -0.14)/sig2, sigma3 = ones(2); push_front(expected,ordering[X(2)],d3, I/sig2,ordering[L(1)], (-0.2)*I/sig2, ordering[X(1)], (-0.8)*I/sig2, sigma3); // Check one ordering @@ -374,10 +374,10 @@ TEST( GaussianFactorGraph, multiplication ) VectorValues x = createCorrectDelta(ord); Errors actual = A * x; Errors expected; - expected += (Vector(2) << -1.0,-1.0).finished(); - expected += (Vector(2) << 2.0,-1.0).finished(); - expected += (Vector(2) << 0.0, 1.0).finished(); - expected += (Vector(2) << -1.0, 1.5).finished(); + expected += Vector2(-1.0,-1.0); + expected += Vector2(2.0,-1.0); + expected += Vector2(0.0, 1.0); + expected += Vector2(-1.0, 1.5); EXPECT(assert_equal(expected,actual)); } diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index b2c6d6b4c..b43f0d3ef 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -172,10 +172,10 @@ done: // // Create values where indices 1 and 3 are above the threshold of 0.1 // VectorValues values; // values.reserve(4, 10); -// values.push_back_preallocated((Vector(2) << 0.09, 0.09).finished()); -// values.push_back_preallocated((Vector(3) << 0.11, 0.11, 0.09).finished()); -// values.push_back_preallocated((Vector(3) << 0.09, 0.09, 0.09).finished()); -// values.push_back_preallocated((Vector(2) << 0.11, 0.11).finished()); +// values.push_back_preallocated(Vector2(0.09, 0.09)); +// values.push_back_preallocated(Vector3(0.11, 0.11, 0.09)); +// values.push_back_preallocated(Vector3(0.09, 0.09, 0.09)); +// values.push_back_preallocated(Vector2(0.11, 0.11)); // // // Create a permutation // Permutation permutation(4); diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 0b9cb57d2..cdb1509b6 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -105,7 +105,7 @@ TEST( GaussianJunctionTreeB, optimizeMultiFrontal ) // verify VectorValues expected(vector(7,2)); // expected solution - Vector v = (Vector(2) << 0., 0.).finished(); + Vector v = Vector2(0., 0.); for (int i=1; i<=7; i++) expected[ordering[X(i)]] = v; EXPECT(assert_equal(expected,actual)); @@ -199,7 +199,7 @@ TEST(GaussianJunctionTreeB, simpleMarginal) { // Create a simple graph NonlinearFactorGraph fg; fg.add(PriorFactor(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0))); - fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas((Vector(3) << 10.0, 1.0, 1.0).finished()))); + fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector3(10.0, 1.0, 1.0)))); Values init; init.insert(X(0), Pose2()); diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 48054c8c0..81bcac344 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -30,18 +30,18 @@ boost::tuple generateProblem() { // 2a. Add Gaussian prior Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1).finished()); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph += PriorFactor(1, priorMean, priorNoise); // 2b. Add odometry factors - SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished()); + SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise); graph += BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph += BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); graph += BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add pose constraint - SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished()); + SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph += BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 85c756e50..6c1fd7dd5 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -114,7 +114,7 @@ TEST( GaussianFactorGraph, findMinimumSpanningTree ) GaussianFactorGraph g; Matrix I = eye(2); Vector2 b(0, 0); - const SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(2) << 0.5, 0.5).finished()); + const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); using namespace symbol_shorthand; g += JacobianFactor(X(1), I, X(2), I, b, model); g += JacobianFactor(X(1), I, X(3), I, b, model); diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index e43393124..071b9d12d 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -105,7 +105,7 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint ) VectorValues expected; expected.insert(X(1), zero(3)); - expected.insert(X(2), (Vector(3) << -0.5,0.,0.).finished()); + expected.insert(X(2), Vector3(-0.5,0.,0.)); CHECK(assert_equal(expected, actual)); } @@ -132,7 +132,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) VectorValues expected; expected.insert(X(1), zero(3)); - expected.insert(X(2), (Vector(3) << -0.5,0.,0.).finished()); + expected.insert(X(2), Vector3(-0.5,0.,0.)); CHECK(assert_equal(expected, actual)); } diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index 8f52f2fc3..dd22ae738 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -51,13 +51,13 @@ TEST(Marginals, planarSLAMmarginals) { /* add prior */ // gaussian for prior - SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1).finished()); + SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin graph += PriorFactor(x1, priorMean, priorNoise); // add the factor to the graph /* add odometry */ // general noisemodel for odometry - SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1).finished()); + SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) // create between factors to represent odometry graph += BetweenFactor(x1, x2, odometry, odometryNoise); @@ -65,7 +65,7 @@ TEST(Marginals, planarSLAMmarginals) { /* add measurements */ // general noisemodel for measurements - SharedDiagonal measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2).finished()); + SharedDiagonal measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // create the measurement values - indices are (pose id, landmark id) Rot2 bearing11 = Rot2::fromDegrees(45), diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index b6628b56d..6366d9fa5 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -166,7 +166,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { // the unwhitened error should provide logmap to the feasible state Pose2 badPoint1(0.0, 2.0, 3.0); Vector actVec = nle.evaluateError(badPoint1); - Vector expVec = (Vector(3) << -0.989992, -0.14112, 0.0).finished(); + Vector expVec = Vector3(-0.989992, -0.14112, 0.0); EXPECT(assert_equal(expVec, actVec, 1e-5)); // the actual error should have a gain on it @@ -267,8 +267,8 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) { Point2 ptBad1(2.0, 2.0); config2.insert(key, ptBad1); EXPECT(constraint.active(config2)); - EXPECT(assert_equal((Vector(2) << 1.0, 0.0).finished(), constraint.evaluateError(ptBad1), tol)); - EXPECT(assert_equal((Vector(2) << 1.0, 0.0).finished(), constraint.unwhitenedError(config2), tol)); + EXPECT(assert_equal(Vector2(1.0, 0.0), constraint.evaluateError(ptBad1), tol)); + EXPECT(assert_equal(Vector2(1.0, 0.0), constraint.unwhitenedError(config2), tol)); EXPECT_DOUBLES_EQUAL(500.0, constraint.error(config2), tol); } @@ -289,7 +289,7 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) { Point2 ptBad(2.0, 2.0); config2.insert(key, ptBad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); - GaussianFactor::shared_ptr expected2(new JacobianFactor(key, eye(2,2), (Vector(2) <<-1.0,0.0).finished(), hard_model)); + GaussianFactor::shared_ptr expected2(new JacobianFactor(key, eye(2,2), Vector2(-1.0,0.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } @@ -347,8 +347,8 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { config2.insert(key1, x1bad); config2.insert(key2, x2bad); EXPECT(constraint.active(config2)); - EXPECT(assert_equal((Vector(2) << -1.0, -1.0).finished(), constraint.evaluateError(x1bad, x2bad), tol)); - EXPECT(assert_equal((Vector(2) << -1.0, -1.0).finished(), constraint.unwhitenedError(config2), tol)); + EXPECT(assert_equal(Vector2(-1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol)); + EXPECT(assert_equal(Vector2(-1.0, -1.0), constraint.unwhitenedError(config2), tol)); EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol); } @@ -376,7 +376,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr expected2( new JacobianFactor(key1, -eye(2,2), key2, - eye(2,2), (Vector(2) << 1.0, 1.0).finished(), hard_model)); + eye(2,2), Vector2(1.0, 1.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 93f468910..e9e266bb3 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -27,7 +27,7 @@ TEST(testNonlinearISAM, markov_chain ) { NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 3.0, 3.0, 0.5).finished()); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(3.0, 3.0, 0.5)); Sampler sampler(model, 42u); // create initial graph @@ -74,8 +74,8 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) { NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object - SharedDiagonal model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 3.0, 3.0, 0.5).finished()); - SharedDiagonal model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 2.0, 2.0).finished()); + SharedDiagonal model3 = noiseModel::Diagonal::Sigmas(Vector3(3.0, 3.0, 0.5)); + SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(Vector2(2.0, 2.0)); Sampler sampler(model3, 42u); // create initial graph @@ -151,8 +151,8 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object - SharedDiagonal model3 = noiseModel::Diagonal::Sigmas((Vector(3) << 3.0, 3.0, 0.5).finished()); - SharedDiagonal model2 = noiseModel::Diagonal::Sigmas((Vector(2) << 2.0, 2.0).finished()); + SharedDiagonal model3 = noiseModel::Diagonal::Sigmas(Vector3(3.0, 3.0, 0.5)); + SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(Vector2(2.0, 2.0)); Sampler sampler(model3, 42u); // create initial graph diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index 972c842f0..da3462b57 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -67,9 +67,9 @@ TEST( PCGSolver, llt ) { // 2., -1., // 1.).finished(); - Vector b = (Vector(3) << 1., 2., 3.).finished(); + Vector b = Vector3(1., 2., 3.); - Vector x = (Vector(3) << 6.5, 2.5, 3.).finished() ; + Vector x = Vector3(6.5, 2.5, 3.) ; /* test cholesky */ Matrix Rhat = AtA.llt().matrixL().transpose(); diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index aecdcd626..ad2c0767a 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -34,11 +34,11 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), (Vector(2) << 2.0, -1.0).finished(), unit2); + fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), Vector2(2.0, -1.0), unit2); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), (Vector(2) << 0.0, 1.0).finished(), unit2); + fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), Vector2(0.0, 1.0), unit2); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5).finished(), unit2); + fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector2(-1.0, 1.5), unit2); return fg; } diff --git a/tests/testSimulated2DOriented.cpp b/tests/testSimulated2DOriented.cpp index 348393d69..d411e7d5e 100644 --- a/tests/testSimulated2DOriented.cpp +++ b/tests/testSimulated2DOriented.cpp @@ -58,7 +58,7 @@ TEST( simulated2DOriented, Dprior ) TEST( simulated2DOriented, constructor ) { Pose2 measurement(0.2, 0.3, 0.1); - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1., 1., 1.).finished()); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector3(1., 1., 1.)); simulated2DOriented::Odometry factor(measurement, model, X(1), X(2)); simulated2DOriented::Values config; diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index 34629ecbc..6a49f66d3 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -131,12 +131,12 @@ TEST( SubgraphPreconditioner, system ) // y1 = perturbed y0 VectorValues y1 = zeros; - y1[1] = (Vector(2) << 1.0, -1.0).finished(); + y1[1] = Vector2(1.0, -1.0); // Check corresponding x values VectorValues expected_x1 = xtrue, x1 = system.x(y1); - expected_x1[1] = (Vector(2) << 2.01, 2.99).finished(); - expected_x1[0] = (Vector(2) << 3.01, 2.99).finished(); + expected_x1[1] = Vector2(2.01, 2.99); + expected_x1[0] = Vector2(3.01, 2.99); CHECK(assert_equal(xtrue, system.x(y0))); CHECK(assert_equal(expected_x1,system.x(y1))); @@ -150,21 +150,21 @@ TEST( SubgraphPreconditioner, system ) VectorValues expected_gx0 = zeros; VectorValues expected_gx1 = zeros; CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); - expected_gx1[2] = (Vector(2) << -100., 100.).finished(); - expected_gx1[4] = (Vector(2) << -100., 100.).finished(); - expected_gx1[1] = (Vector(2) << 200., -200.).finished(); - expected_gx1[3] = (Vector(2) << -100., 100.).finished(); - expected_gx1[0] = (Vector(2) << 100., -100.).finished(); + expected_gx1[2] = Vector2(-100., 100.); + expected_gx1[4] = Vector2(-100., 100.); + expected_gx1[1] = Vector2(200., -200.); + expected_gx1[3] = Vector2(-100., 100.); + expected_gx1[0] = Vector2(100., -100.); CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); // Test gradient in y VectorValues expected_gy0 = zeros; VectorValues expected_gy1 = zeros; - expected_gy1[2] = (Vector(2) << 2., -2.).finished(); - expected_gy1[4] = (Vector(2) << -2., 2.).finished(); - expected_gy1[1] = (Vector(2) << 3., -3.).finished(); - expected_gy1[3] = (Vector(2) << -1., 1.).finished(); - expected_gy1[0] = (Vector(2) << 1., -1.).finished(); + expected_gy1[2] = Vector2(2., -2.); + expected_gy1[4] = Vector2(-2., 2.); + expected_gy1[1] = Vector2(3., -3.); + expected_gy1[3] = Vector2(-1., 1.); + expected_gy1[0] = Vector2(1., -1.); CHECK(assert_equal(expected_gy0,gradient(system,y0))); CHECK(assert_equal(expected_gy1,gradient(system,y1))); @@ -204,7 +204,7 @@ TEST( SubgraphPreconditioner, conjugateGradients ) VectorValues y0 = VectorValues::Zero(xbar); VectorValues y1 = y0; - y1[1] = (Vector(2) << 1.0, -1.0).finished(); + y1[1] = Vector2(1.0, -1.0); VectorValues x1 = system.x(y1); // Solve for the remaining constraints using PCG