Modified Vector_() to Vec() in examples

examples/CameraResectioning.cpp

@@ -70,7 +70,7 @@ int main(int argc, char* argv[]) {
 
   /* 2. add factors to the graph */
   // add measurement factors
-  SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector_(2, 0.5, 0.5));
+  SharedDiagonal measurementNoise = Diagonal::Sigmas((Vec(2) << 0.5, 0.5));
   boost::shared_ptr<ResectioningFactor> factor;
   graph.push_back(
       boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,

examples/LocalizationExample.cpp

@@ -94,7 +94,7 @@ public:
     // [ derror_x/dx  derror_x/dy  derror_x/dtheta ] = [1 0 0]
     // [ derror_y/dx  derror_y/dy  derror_y/dtheta ] = [0 1 0]
     if (H) (*H) = Matrix_(2,3, 1.0,0.0,0.0, 0.0,1.0,0.0);
-    return Vector_(2, q.x() - mx_, q.y() - my_);
+    return (Vec(2) << q.x() - mx_, q.y() - my_);
   }
 
   // The second is a 'clone' function that allows the factor to be copied. Under most
@@ -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));
+  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 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, 0.0), odometryNoise));
   graph.push_back(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)); // 10cm std on x,y
+  noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1)); // 10cm std on x,y
   graph.push_back(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
   graph.push_back(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
   graph.push_back(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));

examples/OdometryExample.cpp

@@ -64,13 +64,13 @@ int main(int argc, char** argv) {
   // Add a prior on the first pose, setting it to the origin
   // A prior factor consists of a mean and a noise model (covariance matrix)
   Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
+  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1));
   graph.push_back(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));
+  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1));
   // Create odometry (Between) factors between consecutive poses
   graph.push_back(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
   graph.push_back(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));

examples/PlanarSLAMExample.cpp

@@ -80,18 +80,18 @@ int main(int argc, char** argv) {
 
   // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
   Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
+  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
   graph.push_back(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)); // 20cm std on x,y, 0.1 rad on theta
+  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
   graph.push_back(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
   graph.push_back(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)); // 0.1 rad std on bearing, 20cm on range
+  noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vec(2) << 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),

examples/Pose2SLAMExample.cpp

@@ -71,11 +71,11 @@ int main(int argc, char** argv) {
 
   // 2a. Add a prior on the first pose, setting it to the origin
   // A prior factor consists of a mean and a noise model (covariance matrix)
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
+  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 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));
+  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
   // Create odometry (Between) factors between consecutive poses

examples/Pose2SLAMExample_graph.cpp

@@ -31,13 +31,13 @@ int main (int argc, char** argv) {
   // we are in build/examples, data is in examples/Data
   NonlinearFactorGraph::shared_ptr graph;
   Values::shared_ptr initial;
-  SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 5.0 * M_PI / 180.0));
+  SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vec(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
   boost::tie(graph, initial) = load2D("../../examples/Data/w100.graph", model);
   initial->print("Initial estimate:\n");
 
   // 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));
+  SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.01, 0.01, 0.01));
   graph->push_back(PriorFactor<Pose2>(0, priorMean, priorNoise));
 
   // Single Step Optimization using Levenberg-Marquardt

examples/Pose2SLAMExample_graphviz.cpp

@@ -32,11 +32,11 @@ int main (int argc, char** argv) {
   NonlinearFactorGraph graph;
 
   // 2a. Add a prior on the first pose, setting it to the origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
+  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 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));
+  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
   graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));

examples/Pose2SLAMwSPCG.cpp

@@ -68,12 +68,12 @@ int main(int argc, char** argv) {
   // 2a. Add a prior on the first pose, setting it to the origin
   // A prior factor consists of a mean and a noise model (covariance matrix)
   Pose2 prior(0.0, 0.0, 0.0); // prior at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
+  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 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));
+  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 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));
+  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vec(3) << 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
 

examples/SelfCalibrationExample.cpp

@@ -53,7 +53,7 @@ int main(int argc, char* argv[]) {
   NonlinearFactorGraph graph;
 
   // Add a prior on pose x1.
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vec(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
   graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
 
   // Simulated measurements from each camera pose, adding them to the factor graph
@@ -74,7 +74,7 @@ int main(int argc, char* argv[]) {
   graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
 
   // Add a prior on the calibration.
-  noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas(Vector_(5, 500, 500, 0.1, 100, 100));
+  noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vec(5) << 500, 500, 0.1, 100, 100));
   graph.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), K, calNoise));
 
   // Create the initial estimate to the solution

examples/VisualISAM2Example.cpp

@@ -103,7 +103,7 @@ int main(int argc, char* argv[]) {
     // adding it to iSAM.
     if( i == 0) {
       // Add a prior on pose x0
-      noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+      noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vec(6) << Vector3::Constant(0.3),Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
       graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
 
       // Add a prior on landmark l0

examples/VisualISAMExample.cpp

@@ -97,7 +97,7 @@ int main(int argc, char* argv[]) {
     // adding it to iSAM.
     if( i == 0) {
       // Add a prior on pose x0
-      noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+      noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vec(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
       graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
 
       // Add a prior on landmark l0

examples/VisualSLAMExample.cpp

@@ -80,7 +80,7 @@ int main(int argc, char* argv[]) {
   NonlinearFactorGraph graph;
 
   // Add a prior on pose x1. This indirectly specifies where the origin is.
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vec(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
   graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
 
   // Simulated measurements from each camera pose, adding them to the factor graph

examples/easyPoint2KalmanFilter.cpp

@@ -37,7 +37,7 @@ int main() {
 
   // Create the Kalman Filter initialization point
   Point2 x_initial(0.0, 0.0);
-  SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
+  SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vec(2) << 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);
-  SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1), true);
+  Vector u = (Vec(2) << 1.0, 0.0);
+  SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vec(2) << 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), true);
+  SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vec(2) << 0.25, 0.25), true);
 
   // This simple measurement can be modeled with a PriorFactor
   Point2 z1(1.0, 0.0);

examples/elaboratePoint2KalmanFilter.cpp

@@ -58,7 +58,7 @@ int main() {
   // Initialize state x0 (2D point) at origin by adding a prior factor, i.e., Bayes net P(x0)
   // This is equivalent to x_0 and P_0
   Point2 x_initial(0,0);
-  SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
+  SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1));
   PriorFactor<Point2> factor1(x0, x_initial, P_initial);
   // Linearize the factor and add it to the linear factor graph
   linearizationPoints.insert(x0, x_initial);
@@ -89,7 +89,7 @@ int main() {
   ordering->insert(x1, 1);
 
   Point2 difference(1,0);
-  SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
+  SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1));
   BetweenFactor<Point2> factor2(x0, x1, difference, Q);
   // Linearize the factor and add it to the linear factor graph
   linearizationPoints.insert(x1, x_initial);
@@ -167,7 +167,7 @@ int main() {
   //    = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
   // This can be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R.
   Point2 z1(1.0, 0.0);
-  SharedDiagonal R1 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
+  SharedDiagonal R1 = noiseModel::Diagonal::Sigmas((Vec(2) << 0.25, 0.25));
   PriorFactor<Point2> factor4(x1, z1, R1);
   // Linearize the factor and add it to the linear factor graph
   linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering));
@@ -219,7 +219,7 @@ int main() {
 
   // Create a nonlinear factor describing the motion model
   difference = Point2(1,0);
-  Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
+  Q = noiseModel::Diagonal::Sigmas((Vec(2) <, 0.1, 0.1));
   BetweenFactor<Point2> factor6(x1, x2, difference, Q);
 
   // Linearize the factor and add it to the linear factor graph
@@ -257,7 +257,7 @@ int main() {
 
   // And update using z2 ...
   Point2 z2(2.0, 0.0);
-  SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
+  SharedDiagonal R2 = noiseModel::Diagonal::Sigmas((Vec(2) << 0.25, 0.25));
   PriorFactor<Point2> factor8(x2, z2, R2);
 
   // Linearize the factor and add it to the linear factor graph
@@ -308,7 +308,7 @@ int main() {
 
   // Create a nonlinear factor describing the motion model
   difference = Point2(1,0);
-  Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
+  Q = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1));
   BetweenFactor<Point2> factor10(x2, x3, difference, Q);
 
   // Linearize the factor and add it to the linear factor graph
@@ -346,7 +346,7 @@ int main() {
 
   // And update using z3 ...
   Point2 z3(3.0, 0.0);
-  SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
+  SharedDiagonal R3 = noiseModel::Diagonal::Sigmas((Vec(2) << 0.25, 0.25));
   PriorFactor<Point2> factor12(x3, z3, R3);
 
   // Linearize the factor and add it to the linear factor graph