diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 891269f16..63377f8aa 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -70,7 +70,7 @@ int main(int argc, char* argv[]) { /* 2. add factors to the graph */ // add measurement factors - SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector_(2, 0.5, 0.5)); + SharedDiagonal measurementNoise = Diagonal::Sigmas((Vec(2) << 0.5, 0.5)); boost::shared_ptr factor; graph.push_back( boost::make_shared(measurementNoise, X(1), calib, diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 4df997dcd..6cf98607e 100644 --- a/examples/LocalizationExample.cpp +++ b/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(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); // 2b. Add "GPS-like" measurements // We will use our custom UnaryFactor for this. - noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // 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(1, 0.0, 0.0, unaryNoise)); graph.push_back(boost::make_shared(2, 2.0, 0.0, unaryNoise)); graph.push_back(boost::make_shared(3, 4.0, 0.0, unaryNoise)); diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index 2ef322a9b..08b2d48f2 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -64,13 +64,13 @@ int main(int argc, char** argv) { // Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, priorMean, priorNoise)); // Add odometry factors Pose2 odometry(2.0, 0.0, 0.0); // For simplicity, we will use the same noise model for each odometry factor - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + 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(1, 2, odometry, odometryNoise)); graph.push_back(BetweenFactor(2, 3, odometry, odometryNoise)); diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index 5a8624db0..94cf99c0e 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -80,18 +80,18 @@ int main(int argc, char** argv) { // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); // 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(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(x1, x2, odometry, odometryNoise)); graph.push_back(BetweenFactor(x2, x3, odometry, odometryNoise)); // Add Range-Bearing measurements to two different landmarks // create a noise model for the landmark measurements - noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2)); // 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), diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 8d7ec7bc3..23ab3cf72 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -71,11 +71,11 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); // For simplicity, we will use the same noise model for odometry and loop closures - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + 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 diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index ad8f9dde6..c7d9c2c7e 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/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(0, priorMean, priorNoise)); // Single Step Optimization using Levenberg-Marquardt diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index a7f36c3c1..912413e1e 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -32,11 +32,11 @@ int main (int argc, char** argv) { NonlinearFactorGraph graph; // 2a. Add a prior on the first pose, setting it to the origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); // For simplicity, we will use the same noise model for odometry and loop closures - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1)); // 2b. Add odometry factors graph.push_back(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 6fe5ca68d..ca2ea00b8 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -68,12 +68,12 @@ int main(int argc, char** argv) { // 2a. Add a prior on the first pose, setting it to the origin // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 prior(0.0, 0.0, 0.0); // prior at origin - noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vec(3) << 0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, prior, priorNoise)); // 2b. Add odometry factors // For simplicity, we will use the same noise model for each odometry factor - noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + 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(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); @@ -85,7 +85,7 @@ int main(int argc, char** argv) { // these constraints may be identified in many ways, such as appearance-based techniques // with camera images. // We will use another Between Factor to enforce this constraint, with the distance set to zero, - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vec(3) << 0.2, 0.2, 0.1)); graph.push_back(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); graph.print("\nFactor Graph:\n"); // print diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index d83bfa9e1..446f5b4e2 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/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(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(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(Symbol('K', 0), K, calNoise)); // Create the initial estimate to the solution diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index d0ea9f752..47f4bff5c 100644 --- a/examples/VisualISAM2Example.cpp +++ b/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(Symbol('x', 0), poses[0], poseNoise)); // Add a prior on landmark l0 diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 0c10fe307..c9537a7e5 100644 --- a/examples/VisualISAMExample.cpp +++ b/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(Symbol('x', 0), poses[0], poseNoise)); // Add a prior on landmark l0 diff --git a/examples/VisualSLAMExample.cpp b/examples/VisualSLAMExample.cpp index 0919cbb9b..e55e2971f 100644 --- a/examples/VisualSLAMExample.cpp +++ b/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(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph // Simulated measurements from each camera pose, adding them to the factor graph diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index 8d8c36d4b..ca60669cc 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -37,7 +37,7 @@ int main() { // Create the Kalman Filter initialization point Point2 x_initial(0.0, 0.0); - SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + 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); diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 049d1a015..a0300adf9 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/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 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 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 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 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 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 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 factor12(x3, z3, R3); // Linearize the factor and add it to the linear factor graph