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

Converted Vector(2|3) << ... to Vector2(...) or Vector3(...) syntax

release/4.3a0
Richard Roberts 2014-11-23 10:22:25 -08:00
parent 77254900f2
commit f9ca07e610
102 changed files with 583 additions and 583 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/CameraResectioning.cpp
View File

@ -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<ResectioningFactor> factor;
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,

4
examples/LocalizationExample.cpp
View File

@ -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<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.add(BetweenFactor<Pose2>(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<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));

4
examples/OdometryExample.cpp
View File

@ -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<Pose2>(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<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));

6
examples/PlanarSLAMExample.cpp
View File

@ -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<Pose2>(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<Pose2>(x1, x2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(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),

4
examples/Pose2SLAMExample.cpp
View File

@ -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<Pose2>(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

2
examples/Pose2SLAMExample_graph.cpp
View File

@ -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<Pose2>(0, priorMean, priorNoise));
// Single Step Optimization using Levenberg-Marquardt

4
examples/Pose2SLAMExample_graphviz.cpp
View File

@ -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<Pose2>(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<Pose2>(1, 2, Pose2(2, 0, 0 ), model));

6
examples/Pose2SLAMwSPCG.cpp
View File

@ -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<Pose2>(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<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(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<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
graph.print("\nFactor Graph:\n"); // print

8
examples/easyPoint2KalmanFilter.cpp
View File

@ -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);

4
gtsam/base/tests/testLieVector.cpp
View File

@ -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);

26
gtsam/base/tests/testMatrix.cpp
View File

@ -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,

6
gtsam/base/tests/testSerializationBase.cpp
View File

@ -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) {

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

@ -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));
}

4
gtsam/discrete/tests/testDiscreteMarginals.cpp
View File

@ -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));
}
/* ************************************************************************* */

2
gtsam/geometry/Unit3.cpp
View File

@ -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 {

6
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -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);

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

@ -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)));
}
/* ************************************************************************* */

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

@ -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)));
}
/* ************************************************************************* */

16
gtsam/geometry/tests/testPose2.cpp
View File

@ -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<Pose2>(pose, (Vector(3) << 0.01, -0.015, 0.99).finished());
Pose2 actual = expmap_default<Pose2>(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<Pose2>(pose, (Vector(3) << 0.01, -0.015, 0.99).finished());
Pose2 actual = expmap_default<Pose2>(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<Pose2>(pose0, pose);
EXPECT(assert_equal(expected, actual, 1e-5));
}

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

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

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

@ -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)

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

@ -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)));
}
/* ************************************************************************* */

24
gtsam/geometry/tests/testUnit3.cpp
View File

@ -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++) {

6
gtsam/linear/VectorValues.h
View File

@ -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
*

4
gtsam/linear/tests/testErrors.cpp
View File

@ -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));
}

18
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -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<Vector10>(

24
gtsam/linear/tests/testGaussianBayesTree.cpp
View File

@ -88,7 +88,7 @@ TEST( GaussianBayesTree, eliminate )
GaussianBayesTree bayesTree_expected;
bayesTree_expected.insertRoot(
MakeClique(GaussianConditional(pair_list_of<Key, Matrix>(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<Key, Matrix>(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<Key, Matrix>(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<Key, Matrix> (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<Key, Matrix> (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<Key, Matrix> (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<Key, Matrix> (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<Key, Matrix> (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<Key, Matrix> (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<Key, Vector>
(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();

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

@ -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<pair<Key, Matrix> > 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<Key, Vector>
(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

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

@ -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);

78
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -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<Key, Vector>
(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<Key, Vector>
(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<Key, Vector>
(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));
}

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

@ -76,7 +76,7 @@ TEST(HessianFactor, ConversionConstructor)
HessianFactor actual(jacobian);
VectorValues values = pair_list_of<Key, Vector>
(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<Key, Vector>(0, (Vector(2) << 1.5, 2.5).finished());
VectorValues dx = pair_list_of<Key, Vector>(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<Key, Vector>
(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

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

@ -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<Key> 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));
}

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

@ -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

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

@ -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<Gaussian::shared_ptr> 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);

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

@ -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);

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

@ -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<VectorValues>(values));
EXPECT(equalsXML<VectorValues>(values));
EXPECT(equalsBinary<VectorValues>(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);

56
gtsam/linear/tests/testVectorValues.cpp
View File

@ -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<Key> 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<Key> 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<Key,size_t> 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<Key,size_t> dims;
dims.insert(make_pair(0,1));

4
gtsam/navigation/tests/testImuBias.cpp
View File

@ -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;

20
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -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;

2
gtsam/nonlinear/ISAM2.h
View File

@ -123,7 +123,7 @@ struct GTSAM_EXPORT ISAM2Params {
* \code
FastMap<char,Vector> 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
*/

28
gtsam/nonlinear/tests/testLinearContainerFactor.cpp
View File

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

12
gtsam/nonlinear/tests/testValues.cpp
View File

@ -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<Key, Vector>
(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<Key, Vector>
(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<Key, Vector>
(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);

2
gtsam/slam/InitializePose3.cpp
View File

@ -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();
}

2
gtsam/slam/lago.cpp
View File

@ -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

6
gtsam/slam/tests/testDataset.cpp
View File

@ -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<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model));
expectedGraph.add(BetweenFactor<Pose2>(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;

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

@ -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++)

4
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -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<Projection>
@ -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 ;

4
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -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<Projection>
@ -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)));
}

4
gtsam/slam/tests/testInitializePose3.cpp
View File

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

8
gtsam/slam/tests/testPoseRotationPrior.cpp
View File

@ -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<Pose2> Pose2RotationPrior;
typedef PoseRotationPrior<Pose3> 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

12
gtsam/slam/tests/testPoseTranslationPrior.cpp
View File

@ -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<Pose2> Pose2TranslationPrior;
typedef PoseTranslationPrior<Pose3> 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));
}

4
gtsam/slam/tests/testProjectionFactor.cpp
View File

@ -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));

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

@ -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)

4
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -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));

16
gtsam_unstable/base/tests/testFixedVector.cpp
View File

@ -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));
}

12
gtsam_unstable/dynamics/tests/testIMUSystem.cpp
View File

@ -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);

6
gtsam_unstable/dynamics/tests/testPoseRTV.cpp
View File

@ -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));
}
/* ************************************************************************* */

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

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

4
gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
View File

@ -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 ) {

16
gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp
View File

@ -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<Pose2>(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<Pose2>(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<Pose2>(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<Pose2>(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<Pose2>(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<Pose2>(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<Pose2>(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<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Unlike the fixed-lag versions, the concurrent filter implementation

6
gtsam_unstable/examples/FixedLagSmootherExample.cpp
View File

@ -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<Pose2>(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<Pose2>(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<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
// Update the smoothers with the new factors

4
gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp
View File

@ -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<Pose2>(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<Pose2>(model, Pose2(2, 0, 0 ), between(x1,x2)));

4
gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp
View File

@ -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;

4
gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
View File

@ -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);

4
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
View File

@ -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);

6
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
View File

@ -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);

4
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
View File

@ -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);

4
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
View File

@ -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);

2
gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
View File

@ -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);

4
gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
View File

@ -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;

2
gtsam_unstable/nonlinear/tests/testParticleFactor.cpp
View File

@ -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);

4
gtsam_unstable/slam/AHRS.cpp
View File

@ -221,8 +221,8 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> 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);

2
gtsam_unstable/slam/tests/testAHRS.cpp
View File

@ -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);

16
gtsam_unstable/slam/tests/testBetweenFactorEM.cpp
View File

@ -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);

4
gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp
View File

@ -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));
}

6
gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp
View File

@ -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);

14
gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
View File

@ -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;

82
gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
View File

@ -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<Pose3, Vector3, imuBias::ConstantBias> 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<Pose3, Vector3, imuBias::ConstantBias> 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<Pose3, Vector3, imuBias::ConstantBias> 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<Pose3, Vector3, imuBias::ConstantBias> 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<Pose3, Vector3, imuBias::ConstantBias> 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

2
gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp
View File

@ -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;

2
gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp
View File

@ -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;

4
gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp
View File

@ -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));

4
gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp
View File

@ -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));

4
gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp
View File

@ -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));

14
gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp
View File

@ -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);

24
gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp
View File

@ -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);

8
gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp
View File

@ -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<Pose3, Vector3, imuBias::ConstantBias> 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);

38
tests/smallExample.h
View File

@ -222,9 +222,9 @@ inline Values createValues() {
inline VectorValues createVectorValues() {
using namespace impl;
VectorValues c = boost::assign::pair_list_of<Key, Vector>
(_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<const NonlinearFactorGraph> sharedReallyNonlinearFactor
using symbol_shorthand::X;
using symbol_shorthand::L;
boost::shared_ptr<NonlinearFactorGraph> 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<smallOptimize::UnaryFactor> 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<Key, Vector>
(_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<Key, Vector>
(_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;
}

20
tests/testDoglegOptimizer.cpp
View File

@ -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

12
tests/testExtendedKalmanFilter.cpp
View File

@ -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<Point2> 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<Point2> ekf(X(0), x_initial, P_initial);

26
tests/testGaussianFactorGraphB.cpp
View File

@ -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));
}

8
tests/testGaussianISAM2.cpp
View File

@ -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);

4
tests/testGaussianJunctionTreeB.cpp
View File

@ -105,7 +105,7 @@ TEST( GaussianJunctionTreeB, optimizeMultiFrontal )
// verify
VectorValues expected(vector<size_t>(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<Pose2>(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0)));
fg.add(BetweenFactor<Pose2>(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<Pose2>(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());

6
tests/testGradientDescentOptimizer.cpp
View File

@ -30,18 +30,18 @@ boost::tuple<NonlinearFactorGraph, Values> 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<Pose2>(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<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise);
graph += BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph += BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph += BetweenFactor<Pose2>(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<Pose2>(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty);
// 3. Create the data structure to hold the initialEstimate estimate to the solution

2
tests/testGraph.cpp
View File

@ -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);

4
tests/testIterative.cpp
View File

@ -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));
}

6
tests/testMarginals.cpp
View File

@ -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<Pose2>(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<Pose2>(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),

14
tests/testNonlinearEquality.cpp
View File

@ -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));
}

10
tests/testNonlinearISAM.cpp
View File

@ -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

4
tests/testPCGSolver.cpp
View File

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

6
tests/testPreconditioner.cpp
View File

@ -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;
}

Some files were not shown because too many files have changed in this diff Show More