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

Replaced graph.add with graph.emplace_shared if needed.

release/4.3a0
Yao Chen 2016-10-01 11:41:37 -04:00
parent 249d6b0b1b
commit 3c1a0a8801
13 changed files with 93 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
examples/LocalizationExample.cpp
View File

@ -120,15 +120,15 @@ int main(int argc, char** argv) {
// For simplicity, we will use the same noise model for each odometry factor // For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses // Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); graph.emplace_shared<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)); graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise);
// 2b. Add "GPS-like" measurements // 2b. Add "GPS-like" measurements
// We will use our custom UnaryFactor for this. // We will use our custom UnaryFactor for this.
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 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.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise)); graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise)); graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);
graph.print("\nFactor Graph:\n"); // print graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution // 3. Create the data structure to hold the initialEstimate estimate to the solution

6
examples/METISOrderingExample.cpp
View File

@ -37,12 +37,12 @@ int main(int argc, char** argv) {
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise)); graph.emplace_shared<PriorFactor<Pose2> >(1, priorMean, priorNoise);
Pose2 odometry(2.0, 0.0, 0.0); Pose2 odometry(2.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise)); graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise)); graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
graph.print("\nFactor Graph:\n"); // print graph.print("\nFactor Graph:\n"); // print
Values initial; Values initial;

6
examples/OdometryExample.cpp
View File

@ -65,15 +65,15 @@ int main(int argc, char** argv) {
// A prior factor consists of a mean and a noise model (covariance matrix) // A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise)); graph.emplace_shared<PriorFactor<Pose2> >(1, priorMean, priorNoise);
// Add odometry factors // Add odometry factors
Pose2 odometry(2.0, 0.0, 0.0); Pose2 odometry(2.0, 0.0, 0.0);
// For simplicity, we will use the same noise model for each odometry factor // For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses // Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise)); graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise)); graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
graph.print("\nFactor Graph:\n"); // print graph.print("\nFactor Graph:\n"); // print
// Create the data structure to hold the initialEstimate estimate to the solution // Create the data structure to hold the initialEstimate estimate to the solution

12
examples/PlanarSLAMExample.cpp
View File

@ -81,13 +81,13 @@ 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) // 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 Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
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 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 graph.emplace_shared<PriorFactor<Pose2> >(x1, prior, priorNoise); // add directly to graph
// Add two odometry factors // Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) 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(Vector3(0.2, 0.2, 0.1)); // 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.emplace_shared<BetweenFactor<Pose2> >(x1, x2, odometry, odometryNoise);
graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise)); graph.emplace_shared<BetweenFactor<Pose2> >(x2, x3, odometry, odometryNoise);
// Add Range-Bearing measurements to two different landmarks // Add Range-Bearing measurements to two different landmarks
// create a noise model for the landmark measurements // create a noise model for the landmark measurements
@ -101,9 +101,9 @@ int main(int argc, char** argv) {
range32 = 2.0; range32 = 2.0;
// Add Bearing-Range factors // Add Bearing-Range factors
graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise)); graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x1, l1, bearing11, range11, measurementNoise);
graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise)); graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x2, l1, bearing21, range21, measurementNoise);
graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise)); graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x3, l2, bearing32, range32, measurementNoise);
// Print // Print
graph.print("Factor Graph:\n"); graph.print("Factor Graph:\n");

12
examples/Pose2SLAMExample.cpp
View File

@ -72,23 +72,23 @@ int main(int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin // 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) // A prior factor consists of a mean and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); 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)); graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures // For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors // 2b. Add odometry factors
// Create odometry (Between) factors between consecutive poses // Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model)); graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0 ), model);
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model)); graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model)); graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model)); graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
// 2c. Add the loop closure constraint // 2c. Add the loop closure constraint
// This factor encodes the fact that we have returned to the same pose. In real systems, // This factor encodes the fact that we have returned to the same pose. In real systems,
// these constraints may be identified in many ways, such as appearance-based techniques // 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 camera images. We will use another Between Factor to enforce this constraint:
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model)); graph.emplace_shared<BetweenFactor<Pose2> >(5, 2, Pose2(2, 0, M_PI_2), model);
graph.print("\nFactor Graph:\n"); // print graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution // 3. Create the data structure to hold the initialEstimate estimate to the solution

9
examples/VisualISAMExample.cpp
View File

@ -89,9 +89,8 @@ int main(int argc, char* argv[]) {
SimpleCamera camera(poses[i], *K); SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]); Point2 measurement = camera.project(points[j]);
// Add measurement // Add measurement
graph.add( graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, noise,
GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, noise, Symbol('x', i), Symbol('l', j), K);
Symbol('x', i), Symbol('l', j), K));
} }
// Intentionally initialize the variables off from the ground truth // Intentionally initialize the variables off from the ground truth
@ -109,12 +108,12 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
// Add a prior on landmark l0 // Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::shared_ptr pointNoise =
noiseModel::Isotropic::Sigma(3, 0.1); noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise);
// Add initial guesses to all observed landmarks // Add initial guesses to all observed landmarks
Point3 noise(-0.25, 0.20, 0.15); Point3 noise(-0.25, 0.20, 0.15);

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

@ -426,7 +426,7 @@ TEST(Unit3, ErrorBetweenFactor) {
// Add prior factors. // Add prior factors.
SharedNoiseModel R_prior = noiseModel::Unit::Create(2); SharedNoiseModel R_prior = noiseModel::Unit::Create(2);
for (size_t i = 0; i < data.size(); i++) { for (size_t i = 0; i < data.size(); i++) {
graph.add(PriorFactor<Unit3>(U(i), data[i], R_prior)); graph.emplace_shared<PriorFactor<Unit3> >(U(i), data[i], R_prior);
} }
// Add process factors using the dot product error function. // Add process factors using the dot product error function.

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

@ -781,8 +781,8 @@ TEST(ImuFactor, bodyPSensorWithBias) {
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factors // Create factors
graph.add(ImuFactor(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim)); graph.emplace_shared<ImuFactor>(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim);
graph.add(BetweenFactor<Bias>(B(i - 1), B(i), zeroBias, biasNoiseModel)); graph.emplace_shared<BetweenFactor<Bias> >(B(i - 1), B(i), zeroBias, biasNoiseModel);
values.insert(X(i), Pose3()); values.insert(X(i), Pose3());
values.insert(V(i), zeroVel); values.insert(V(i), zeroVel);

7
gtsam/slam/InitializePose3.cpp
View File

@ -120,9 +120,8 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
boost::shared_ptr<PriorFactor<Pose3> > pose3Prior = boost::shared_ptr<PriorFactor<Pose3> > pose3Prior =
boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor); boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
if (pose3Prior) if (pose3Prior)
pose3Graph.add( pose3Graph.emplace_shared<BetweenFactor<Pose3> >(keyAnchor, pose3Prior->keys()[0],
BetweenFactor<Pose3>(keyAnchor, pose3Prior->keys()[0], pose3Prior->prior(), pose3Prior->noiseModel());
pose3Prior->prior(), pose3Prior->noiseModel()));
} }
return pose3Graph; return pose3Graph;
} }
@ -330,7 +329,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
// add prior // add prior
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
initialPose.insert(keyAnchor, Pose3()); initialPose.insert(keyAnchor, Pose3());
pose3graph.add(PriorFactor<Pose3>(keyAnchor, Pose3(), priorModel)); pose3graph.emplace_shared<PriorFactor<Pose3> >(keyAnchor, Pose3(), priorModel);
// Create optimizer // Create optimizer
GaussNewtonParams params; GaussNewtonParams params;

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

@ -112,18 +112,18 @@ TEST( dataSet, readG2o)
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699));
NonlinearFactorGraph expectedGraph; NonlinearFactorGraph expectedGraph;
expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); expectedGraph.emplace_shared<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)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
expectedGraph.add(BetweenFactor<Pose2>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
expectedGraph.add(BetweenFactor<Pose2>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
expectedGraph.add(BetweenFactor<Pose2>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
expectedGraph.add(BetweenFactor<Pose2>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
expectedGraph.add(BetweenFactor<Pose2>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
expectedGraph.add(BetweenFactor<Pose2>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
expectedGraph.add(BetweenFactor<Pose2>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(9,10, Pose2(1.003350, 0.022250, -0.195918), model);
expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3,10, Pose2(0.044020, 0.988477, -1.553511), model);
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
} }
@ -164,27 +164,27 @@ TEST( dataSet, readG2o3D)
Point3 p01 = Point3(1.001367, 0.015390, 0.004948); Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
expectedGraph.add(BetweenFactor<Pose3>(0, 1, Pose3(R01,p01), model)); expectedGraph.emplace_shared<BetweenFactor<Pose3> >(0, 1, Pose3(R01,p01), model);
Point3 p12 = Point3(0.523923, 0.776654, 0.326659); Point3 p12 = Point3(0.523923, 0.776654, 0.326659);
Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 );
expectedGraph.add(BetweenFactor<Pose3>(1, 2, Pose3(R12,p12), model)); expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, Pose3(R12,p12), model);
Point3 p23 = Point3(0.910927, 0.055169, -0.411761); Point3 p23 = Point3(0.910927, 0.055169, -0.411761);
Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 );
expectedGraph.add(BetweenFactor<Pose3>(2, 3, Pose3(R23,p23), model)); expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, Pose3(R23,p23), model);
Point3 p34 = Point3(0.775288, 0.228798, -0.596923); Point3 p34 = Point3(0.775288, 0.228798, -0.596923);
Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 );
expectedGraph.add(BetweenFactor<Pose3>(3, 4, Pose3(R34,p34), model)); expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, Pose3(R34,p34), model);
Point3 p14 = Point3(-0.577841, 0.628016, -0.543592); Point3 p14 = Point3(-0.577841, 0.628016, -0.543592);
Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 );
expectedGraph.add(BetweenFactor<Pose3>(1, 4, Pose3(R14,p14), model)); expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 4, Pose3(R14,p14), model);
Point3 p30 = Point3(-0.623267, 0.086928, 0.773222); Point3 p30 = Point3(-0.623267, 0.086928, 0.773222);
Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 );
expectedGraph.add(BetweenFactor<Pose3>(3, 0, Pose3(R30,p30), model)); expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 0, Pose3(R30,p30), model);
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
} }
@ -224,7 +224,7 @@ TEST( dataSet, readG2o3DNonDiagonalNoise)
NonlinearFactorGraph expectedGraph; NonlinearFactorGraph expectedGraph;
Point3 p01 = Point3(1.001367, 0.015390, 0.004948); Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
expectedGraph.add(BetweenFactor<Pose3>(0, 1, Pose3(R01,p01), model)); expectedGraph.emplace_shared<BetweenFactor<Pose3> >(0, 1, Pose3(R01,p01), model);
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2)); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2));
} }
@ -242,18 +242,18 @@ TEST( dataSet, readG2oHuber)
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel);
NonlinearFactorGraph expectedGraph; NonlinearFactorGraph expectedGraph;
expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); expectedGraph.emplace_shared<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)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
expectedGraph.add(BetweenFactor<Pose2>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
expectedGraph.add(BetweenFactor<Pose2>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
expectedGraph.add(BetweenFactor<Pose2>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
expectedGraph.add(BetweenFactor<Pose2>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
expectedGraph.add(BetweenFactor<Pose2>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
expectedGraph.add(BetweenFactor<Pose2>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
expectedGraph.add(BetweenFactor<Pose2>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(9,10, Pose2(1.003350, 0.022250, -0.195918), model);
expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3,10, Pose2(0.044020, 0.988477, -1.553511), model);
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
} }
@ -270,18 +270,18 @@ TEST( dataSet, readG2oTukey)
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
NonlinearFactorGraph expectedGraph; NonlinearFactorGraph expectedGraph;
expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); expectedGraph.emplace_shared<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)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
expectedGraph.add(BetweenFactor<Pose2>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
expectedGraph.add(BetweenFactor<Pose2>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
expectedGraph.add(BetweenFactor<Pose2>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
expectedGraph.add(BetweenFactor<Pose2>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
expectedGraph.add(BetweenFactor<Pose2>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
expectedGraph.add(BetweenFactor<Pose2>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
expectedGraph.add(BetweenFactor<Pose2>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(9,10, Pose2(1.003350, 0.022250, -0.195918), model);
expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3,10, Pose2(0.044020, 0.988477, -1.553511), model);
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
} }

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

@ -178,7 +178,7 @@ TEST (EssentialMatrixFactor, minimization) {
// Noise sigma is 1cm, assuming metric measurements // Noise sigma is 1cm, assuming metric measurements
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++) for (size_t i = 0; i < 5; i++)
graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1)); graph.emplace_shared<EssentialMatrixFactor>(1, pA(i), pB(i), model1);
// Check error at ground truth // Check error at ground truth
Values truth; Values truth;
@ -251,7 +251,7 @@ TEST (EssentialMatrixFactor2, minimization) {
// Noise sigma is 1cm, assuming metric measurements // Noise sigma is 1cm, assuming metric measurements
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++) for (size_t i = 0; i < 5; i++)
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2)); graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i), model2);
// Check error at ground truth // Check error at ground truth
Values truth; Values truth;
@ -323,7 +323,7 @@ TEST (EssentialMatrixFactor3, minimization) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++) for (size_t i = 0; i < 5; i++)
// but now we specify the rotation bRc // but now we specify the rotation bRc
graph.add(EssentialMatrixFactor3(100, i, pA(i), pB(i), cRb, model2)); graph.emplace_shared<EssentialMatrixFactor3>(100, i, pA(i), pB(i), cRb, model2);
// Check error at ground truth // Check error at ground truth
Values truth; Values truth;
@ -391,7 +391,7 @@ TEST (EssentialMatrixFactor, extraMinimization) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++) for (size_t i = 0; i < 5; i++)
graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1, K)); graph.emplace_shared<EssentialMatrixFactor>(1, pA(i), pB(i), model1, K);
// Check error at ground truth // Check error at ground truth
Values truth; Values truth;
@ -465,7 +465,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) {
// Noise sigma is 1, assuming pixel measurements // Noise sigma is 1, assuming pixel measurements
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t i = 0; i < data.number_tracks(); i++) for (size_t i = 0; i < data.number_tracks(); i++)
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2, K)); graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i), model2, K);
// Check error at ground truth // Check error at ground truth
Values truth; Values truth;

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

@ -89,9 +89,9 @@ TEST (RotateFactor, minimization) {
// Let's try to recover the correct iRc by minimizing // Let's try to recover the correct iRc by minimizing
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
Model model = noiseModel::Isotropic::Sigma(3, 0.01); Model model = noiseModel::Isotropic::Sigma(3, 0.01);
graph.add(RotateFactor(1, i1Ri2, c1Zc2, model)); graph.emplace_shared<RotateFactor>(1, i1Ri2, c1Zc2, model);
graph.add(RotateFactor(1, i2Ri3, c2Zc3, model)); graph.emplace_shared<RotateFactor>(1, i2Ri3, c2Zc3, model);
graph.add(RotateFactor(1, i3Ri4, c3Zc4, model)); graph.emplace_shared<RotateFactor>(1, i3Ri4, c3Zc4, model);
// Check error at ground truth // Check error at ground truth
Values truth; Values truth;
@ -162,9 +162,9 @@ TEST (RotateDirectionsFactor, minimization) {
// Let's try to recover the correct iRc by minimizing // Let's try to recover the correct iRc by minimizing
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
Model model = noiseModel::Isotropic::Sigma(2, 0.01); Model model = noiseModel::Isotropic::Sigma(2, 0.01);
graph.add(RotateDirectionsFactor(1, p1, z1, model)); graph.emplace_shared<RotateDirectionsFactor>(1, p1, z1, model);
graph.add(RotateDirectionsFactor(1, p2, z2, model)); graph.emplace_shared<RotateDirectionsFactor>(1, p2, z2, model);
graph.add(RotateDirectionsFactor(1, p3, z3, model)); graph.emplace_shared<RotateDirectionsFactor>(1, p3, z3, model);
// Check error at ground truth // Check error at ground truth
Values truth; Values truth;

12
gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp
View File

@ -27,22 +27,22 @@ NonlinearFactorGraph planarSLAMGraph() {
// Prior on pose x1 at the origin. // Prior on pose x1 at the origin.
Pose2 prior(0.0, 0.0, 0.0); Pose2 prior(0.0, 0.0, 0.0);
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(x1, prior, priorNoise)); graph.emplace_shared<PriorFactor<Pose2> >(x1, prior, priorNoise);
// Two odometry factors // Two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); Pose2 odometry(2.0, 0.0, 0.0);
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise)); graph.emplace_shared<BetweenFactor<Pose2> >(x1, x2, odometry, odometryNoise);
graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise)); graph.emplace_shared<BetweenFactor<Pose2> >(x2, x3, odometry, odometryNoise);
// Add Range-Bearing measurements to two different landmarks // Add Range-Bearing measurements to two different landmarks
auto measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); auto measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2));
Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90), Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90); bearing32 = Rot2::fromDegrees(90);
double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0; double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0;
graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise)); graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x1, l1, bearing11, range11, measurementNoise);
graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise)); graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x2, l1, bearing21, range21, measurementNoise);
graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise)); graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x3, l2, bearing32, range32, measurementNoise);
return graph; return graph;
} }