Modified Vector_() to Vec() in examples
							parent
							
								
									56e1fb5af0
								
							
						
					
					
						commit
						4478bc6b87
					
				|  | @ -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, | ||||
|  |  | |||
|  | @ -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)); | ||||
|  |  | |||
|  | @ -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)); | ||||
|  |  | |||
|  | @ -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), | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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)); | ||||
|  |  | |||
|  | @ -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
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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); | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue