diff --git a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp index 318053ffb..d11dc4c08 100644 --- a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp @@ -31,7 +31,7 @@ gtsam::Rot3 world_R_ECEF( 0.85173, -0.52399, 0, 0.41733, 0.67835, -0.60471); -gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); +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); /* ************************************************************************* */ @@ -54,8 +54,8 @@ int main() { double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81).finished()); - Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + 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); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); diff --git a/timing/timeGaussianFactor.cpp b/timing/timeGaussianFactor.cpp index 261fbac48..e6ae51aa4 100644 --- a/timing/timeGaussianFactor.cpp +++ b/timing/timeGaussianFactor.cpp @@ -63,7 +63,7 @@ int main() +0.,-5., 10., 0., +0.,10. - ); + ).finished(); Matrix Al1 = (Matrix(8, 10) << // l1 @@ -75,7 +75,7 @@ int main() 0., 5.,1.,2.,3.,4.,5.,6.,7.,8., 0., 0.,1.,2.,3.,4.,5.,6.,7.,8., 0., 0.,1.,2.,3.,4.,5.,6.,7.,8. - ); + ).finished(); Matrix Ax1 = (Matrix(8, 2) << // x1 @@ -87,7 +87,7 @@ int main() 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8., -10., 0.,1.,2.,3.,4.,5.,6.,7.,8., 0.00,-10.,1.,2.,3.,4.,5.,6.,7.,8. - ); + ).finished(); // and a RHS Vector b2(8); diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp index ff60c4a27..931e2498f 100644 --- a/timing/timeLago.cpp +++ b/timing/timeLago.cpp @@ -36,14 +36,14 @@ int main(int argc, char *argv[]) { Values::shared_ptr solution; NonlinearFactorGraph::shared_ptr g; string inputFile = findExampleDataFile("w10000"); - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0)); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished()); boost::tie(g, solution) = load2D(inputFile, model); // add noise to create initial estimate Values initial; Sampler sampler(42u); Values::ConstFiltered poses = solution->filter(); - SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0)); + SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished()); BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& it, poses) initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise))); diff --git a/timing/timePose2.cpp b/timing/timePose2.cpp index 94bd78ef9..06ab633c6 100644 --- a/timing/timePose2.cpp +++ b/timing/timePose2.cpp @@ -61,7 +61,7 @@ Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2, *H1 = (Matrix(3,3) << -c, -s, dt1, s, -c, dt2, - 0.0, 0.0,-1.0); + 0.0, 0.0,-1.0).finished(); } if (H2) *H2 = Matrix::Identity(3,3); @@ -138,7 +138,7 @@ int main() // create a random pose: double x = 4.0, y = 2.0, r = 0.3; - Vector v = (Vector(3) << x, y, r); + Vector v = (Vector(3) << x, y, r).finished(); Pose2 X = Pose2(3,2,0.4), X2 = X.retract(v), X3(5,6,0.3); TEST(Expmap, Pose2::Expmap(v)); diff --git a/timing/timePose3.cpp b/timing/timePose3.cpp index 3539468d5..713d99d09 100644 --- a/timing/timePose3.cpp +++ b/timing/timePose3.cpp @@ -37,7 +37,7 @@ int main() double norm=sqrt(1.0+16.0+4.0); double x=1.0/norm, y=4.0/norm, z=2.0/norm; - Vector v = (Vector(6) << x, y, z, 0.1, 0.2, -0.1); + Vector v = (Vector(6) << x, y, z, 0.1, 0.2, -0.1).finished(); Pose3 T = Pose3::Expmap((Vector(6) << 0.1, 0.1, 0.2, 0.1, 0.4, 0.2).finished()), T2 = T.retract(v); Matrix H1,H2; diff --git a/timing/timeRot2.cpp b/timing/timeRot2.cpp index 86dda2b8c..6860914ef 100644 --- a/timing/timeRot2.cpp +++ b/timing/timeRot2.cpp @@ -95,7 +95,7 @@ int main() // create a random direction: double norm=sqrt(16.0+4.0); double x=4.0/norm, y=2.0/norm; - Vector v = (Vector(2) << x, y); + Vector v = (Vector(2) << x, y).finished(); Rot2 R = Rot2(0.4), R2 = R.retract(v), R3(0.6); TEST(Rot2_Expmap, Rot2::Expmap(v)); diff --git a/timing/timeRot3.cpp b/timing/timeRot3.cpp index dd98465ed..4977fba86 100644 --- a/timing/timeRot3.cpp +++ b/timing/timeRot3.cpp @@ -39,7 +39,7 @@ int main() // create a random direction: double norm=sqrt(1.0+16.0+4.0); double x=1.0/norm, y=4.0/norm, z=2.0/norm; - Vector v = (Vector(3) << x, y, z); + Vector v = (Vector(3) << x, y, z).finished(); Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2), R2 = R.retract(v); TEST("Rodriguez formula given axis angle", Rot3::rodriguez(v,0.001))