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

Merged in feature/variadic-emplace_back (pull request #274) 

Feature/variadic emplace_back

Approved-by: Jing Dong
release/4.3a0
Yao Chen 2017-03-12 05:19:15 +00:00 committed by Chris Beall
parent 2a7c64d4a8 80562c8ae2
commit bd67779f74
48 changed files with 373 additions and 385 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
examples/CameraResectioning.cpp
View File

@ -71,18 +71,14 @@ int main(int argc, char* argv[]) {
// add measurement factors
SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5));
boost::shared_ptr<ResectioningFactor> factor;
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(55, 45), Point3(10, 10, 0)));
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(45, 45), Point3(-10, 10, 0)));
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(45, 55), Point3(-10, -10, 0)));
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(55, 55), Point3(10, -10, 0)));
graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(55, 45), Point3(10, 10, 0));
graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(45, 45), Point3(-10, 10, 0));
graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(45, 55), Point3(-10, -10, 0));
graph.emplace_shared<ResectioningFactor>(measurementNoise, X(1), calib,
Point2(55, 55), Point3(10, -10, 0));
/* 3. Create an initial estimate for the camera pose */
Values initial;

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
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(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> >(1, 2, 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
// 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
graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
graph.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);
graph.print("\nFactor Graph:\n"); // print
// 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
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);
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
graph.print("\nFactor Graph:\n"); // print
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)
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));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
graph.emplace_shared<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(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
graph.print("\nFactor Graph:\n"); // print
// 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)
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
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
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
graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
graph.emplace_shared<BetweenFactor<Pose2> >(x1, x2, odometry, odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(x2, x3, odometry, odometryNoise);
// Add Range-Bearing measurements to two different landmarks
// create a noise model for the landmark measurements
@ -101,9 +101,9 @@ int main(int argc, char** argv) {
range32 = 2.0;
// Add Bearing-Range factors
graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x1, l1, bearing11, range11, measurementNoise);
graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x2, l1, bearing21, range21, measurementNoise);
graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x3, l2, bearing32, range32, measurementNoise);
// Print
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
// 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));
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
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.add(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.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0 ), model);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, 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
// 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
// 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
// 3. Create the data structure to hold the initialEstimate estimate to the solution

12
examples/Pose2SLAMExample_graphviz.cpp
View File

@ -33,19 +33,19 @@ int main (int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.push_back(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
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0 ), model);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, 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
graph.push_back(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);
// 3. Create the data structure to hold the initial estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values

12
examples/Pose2SLAMwSPCG.cpp
View File

@ -36,18 +36,18 @@ int main(int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, prior, priorNoise));
graph.emplace_shared<PriorFactor<Pose2> >(1, prior, priorNoise);
// 2b. Add odometry factors
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
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));
graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 2c. Add the loop closure constraint
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.push_back(BetweenFactor<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
graph.emplace_shared<BetweenFactor<Pose2> >(5, 1, Pose2(0.0, 0.0, 0.0), model);
graph.print("\nFactor Graph:\n"); // print

6
examples/SFMExample.cpp
View File

@ -75,14 +75,14 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x1. This indirectly specifies where the origin is.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 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
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise); // add directly to graph
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) {
SimpleCamera camera(poses[i], *K);
for (size_t j = 0; j < points.size(); ++j) {
Point2 measurement = camera.project(points[j]);
graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
}
}
@ -90,7 +90,7 @@ int main(int argc, char* argv[]) {
// Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
// between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
graph.print("Factor Graph:\n");
// Create the data structure to hold the initial estimate to the solution

4
examples/SFMExample_SmartFactor.cpp
View File

@ -82,13 +82,13 @@ int main(int argc, char* argv[]) {
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Pose3>(0, poses[0], noise));
graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
// Because the structure-from-motion problem has a scale ambiguity, the problem is
// still under-constrained. Here we add a prior on the second pose x1, so this will
// fix the scale by indicating the distance between x0 and x1.
// Because these two are fixed, the rest of the poses will be also be fixed.
graph.push_back(PriorFactor<Pose3>(1, poses[1], noise)); // add directly to graph
graph.emplace_shared<PriorFactor<Pose3> >(1, poses[1], noise); // add directly to graph
graph.print("Factor Graph:\n");

4
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -74,10 +74,10 @@ int main(int argc, char* argv[]) {
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Pose3>(0, poses[0], noise));
graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
// Fix the scale ambiguity by adding a prior
graph.push_back(PriorFactor<Pose3>(1, poses[0], noise));
graph.emplace_shared<PriorFactor<Pose3> >(1, poses[0], noise);
// Create the initial estimate to the solution
Values initialEstimate;

6
examples/SFMExample_bal.cpp
View File

@ -59,15 +59,15 @@ int main (int argc, char* argv[]) {
for(const SfM_Measurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
}
j += 1;
}
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
graph.emplace_shared<PriorFactor<SfM_Camera> >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.emplace_shared<PriorFactor<Point3> > (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
// Create initial estimate
Values initial;

6
examples/SFMExample_bal_COLAMD_METIS.cpp
View File

@ -64,15 +64,15 @@ int main (int argc, char* argv[]) {
for(const SfM_Measurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
}
j += 1;
}
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
graph.emplace_shared<PriorFactor<SfM_Camera> >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.emplace_shared<PriorFactor<Point3> >(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
// Create initial estimate
Values initial;

8
examples/SelfCalibrationExample.cpp
View File

@ -54,7 +54,7 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x1.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
// Simulated measurements from each camera pose, adding them to the factor graph
Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
@ -65,17 +65,17 @@ int main(int argc, char* argv[]) {
Point2 measurement = camera.project(points[j]);
// The only real difference with the Visual SLAM example is that here we use a
// different factor type, that also calculates the Jacobian with respect to calibration
graph.push_back(GeneralSFMFactor2<Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0)));
graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0));
}
}
// Add a prior on the position of the first landmark.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.emplace_shared<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).finished());
graph.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), K, calNoise));
graph.emplace_shared<PriorFactor<Cal3_S2> >(Symbol('K', 0), K, calNoise);
// Create the initial estimate to the solution
// now including an estimate on the camera calibration parameters

14
examples/StereoVOExample.cpp
View File

@ -39,7 +39,7 @@ int main(int argc, char** argv){
//create graph object, add first pose at origin with key '1'
NonlinearFactorGraph graph;
Pose3 first_pose;
graph.push_back(NonlinearEquality<Pose3>(1, Pose3()));
graph.emplace_shared<NonlinearEquality<Pose3> >(1, Pose3());
//create factor noise model with 3 sigmas of value 1
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
@ -47,14 +47,14 @@ int main(int argc, char** argv){
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
//create and add stereo factors between first pose (key value 1) and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(520, 480, 440), model, 1, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(120, 80, 440), model, 1, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 280, 140), model, 1, 5, K));
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(520, 480, 440), model, 1, 3, K);
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(120, 80, 440), model, 1, 4, K);
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(320, 280, 140), model, 1, 5, K);
//create and add stereo factors between second pose and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(570, 520, 490), model, 2, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(70, 20, 490), model, 2, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 270, 115), model, 2, 5, K));
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(570, 520, 490), model, 2, 3, K);
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(70, 20, 490), model, 2, 4, K);
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(320, 270, 115), model, 2, 5, K);
//create Values object to contain initial estimates of camera poses and landmark locations
Values initial_estimate;

8
examples/StereoVOExample_large.cpp
View File

@ -83,9 +83,9 @@ int main(int argc, char** argv){
cout << "Reading stereo factors" << endl;
//read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
graph.push_back(
GenericStereoFactor<Pose3, Point3>(StereoPoint2(uL, uR, v), model,
Symbol('x', x), Symbol('l', l), K));
graph.emplace_shared<
GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model,
Symbol('x', x), Symbol('l', l), K);
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
if (!initial_estimate.exists(Symbol('l', l))) {
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
@ -99,7 +99,7 @@ int main(int argc, char** argv){
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x',1),first_pose);
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to optimize the factor graph

6
examples/VisualISAM2Example.cpp
View File

@ -90,7 +90,7 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
}
// Add an initial guess for the current pose
@ -104,11 +104,11 @@ int main(int argc, char* argv[]) {
if( i == 0) {
// Add a prior on pose x0
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(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
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth

9
examples/VisualISAMExample.cpp
View File

@ -89,9 +89,8 @@ int main(int argc, char* argv[]) {
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
// Add measurement
graph.add(
GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, noise,
Symbol('x', i), Symbol('l', j), K));
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, noise,
Symbol('x', i), Symbol('l', j), K);
}
// 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
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
(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
noiseModel::Isotropic::shared_ptr pointNoise =
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
Point3 noise(-0.25, 0.20, 0.15);

12
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -378,12 +378,12 @@ TEST( triangulation, StereotriangulateNonlinear ) {
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3));
graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK)));
graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK)));
graph.emplace_shared<StereoFactor>(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK);
graph.emplace_shared<StereoFactor>(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK);
const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9);
graph.push_back(PriorFactor<Pose3>(Symbol('x',1), Pose3(m1), posePrior));
graph.push_back(PriorFactor<Pose3>(Symbol('x',2), Pose3(m2), posePrior));
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x',1), Pose3(m1), posePrior);
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x',2), Pose3(m2), posePrior);
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
@ -399,8 +399,8 @@ TEST( triangulation, StereotriangulateNonlinear ) {
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3));
graph.push_back(TriangulationFactor<StereoCamera>(cameras[0], measurements[0], unit, Symbol('l',1)));
graph.push_back(TriangulationFactor<StereoCamera>(cameras[1], measurements[1], unit, Symbol('l',1)));
graph.emplace_shared<TriangulationFactor<StereoCamera> >(cameras[0], measurements[0], unit, Symbol('l',1));
graph.emplace_shared<TriangulationFactor<StereoCamera> >(cameras[1], measurements[1], unit, Symbol('l',1));
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();

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

@ -426,7 +426,7 @@ TEST(Unit3, ErrorBetweenFactor) {
// Add prior factors.
SharedNoiseModel R_prior = noiseModel::Unit::Create(2);
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.

8
gtsam/geometry/triangulation.h
View File

@ -89,8 +89,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const Pose3& pose_i = poses[i];
typedef PinholePose<CALIBRATION> Camera;
Camera camera_i(pose_i, sharedCal);
graph.push_back(TriangulationFactor<Camera> //
(camera_i, measurements[i], unit2, landmarkKey));
graph.emplace_shared<TriangulationFactor<Camera> > //
(camera_i, measurements[i], unit2, landmarkKey);
}
return std::make_pair(graph, values);
}
@ -116,8 +116,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
traits<typename CAMERA::Measurement>::dimension));
for (size_t i = 0; i < measurements.size(); i++) {
const CAMERA& camera_i = cameras[i];
graph.push_back(TriangulationFactor<CAMERA> //
(camera_i, measurements[i], unit, landmarkKey));
graph.emplace_shared<TriangulationFactor<CAMERA> > //
(camera_i, measurements[i], unit, landmarkKey);
}
return std::make_pair(graph, values);
}

40
gtsam/inference/FactorGraph.h
View File

@ -22,16 +22,17 @@
#pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Key.h>
#include <boost/serialization/nvp.hpp>
#include <boost/assign/list_inserter.hpp>
#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <gtsam/base/Testable.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Key.h>
#include <type_traits>
#include <utility>
namespace gtsam {
@ -151,7 +152,7 @@ namespace gtsam {
/** Add a factor directly using a shared_ptr */
template<class DERIVEDFACTOR>
typename boost::enable_if<boost::is_base_of<FactorType, DERIVEDFACTOR> >::type
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
push_back(boost::shared_ptr<DERIVEDFACTOR> factor) {
factors_.push_back(boost::shared_ptr<FACTOR>(factor)); }
@ -159,15 +160,22 @@ namespace gtsam {
void push_back(const sharedFactor& factor) {
factors_.push_back(factor); }
/** Emplace a factor */
template<class DERIVEDFACTOR, class... Args>
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
emplace_shared(Args&&... args) {
factors_.push_back(boost::make_shared<DERIVEDFACTOR>(std::forward<Args>(args)...));
}
/** push back many factors with an iterator over shared_ptr (factors are not copied) */
template<typename ITERATOR>
typename boost::enable_if<boost::is_base_of<FactorType, typename ITERATOR::value_type::element_type> >::type
typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type::element_type>::value>::type
push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
factors_.insert(end(), firstFactor, lastFactor); }
/** push back many factors as shared_ptr's in a container (factors are not copied) */
template<typename CONTAINER>
typename boost::enable_if<boost::is_base_of<FactorType, typename CONTAINER::value_type::element_type> >::type
typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type::element_type>::value>::type
push_back(const CONTAINER& container) {
push_back(container.begin(), container.end());
}
@ -175,22 +183,24 @@ namespace gtsam {
/** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived
* classes in favor of a type-specialized version that calls this templated function. */
template<class CLIQUE>
typename boost::enable_if<boost::is_base_of<This, typename CLIQUE::FactorGraphType> >::type
typename std::enable_if<std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
push_back(const BayesTree<CLIQUE>& bayesTree) {
bayesTree.addFactorsToGraph(*this);
}
//#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid
* the copy). */
template<class DERIVEDFACTOR>
typename boost::enable_if<boost::is_base_of<FactorType, DERIVEDFACTOR> >::type
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
push_back(const DERIVEDFACTOR& factor) {
factors_.push_back(boost::make_shared<DERIVEDFACTOR>(factor));
}
//#endif
/** push back many factors with an iterator over plain factors (factors are copied) */
template<typename ITERATOR>
typename boost::enable_if<boost::is_base_of<FactorType, typename ITERATOR::value_type> >::type
typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type>::value>::type
push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
for (ITERATOR f = firstFactor; f != lastFactor; ++f)
push_back(*f);
@ -198,14 +208,14 @@ namespace gtsam {
/** push back many factors as non-pointer objects in a container (factors are copied) */
template<typename CONTAINER>
typename boost::enable_if<boost::is_base_of<FactorType, typename CONTAINER::value_type> >::type
typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type>::value>::type
push_back(const CONTAINER& container) {
push_back(container.begin(), container.end());
}
/** Add a factor directly using a shared_ptr */
template<class DERIVEDFACTOR>
typename boost::enable_if<boost::is_base_of<FactorType, DERIVEDFACTOR>,
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value,
boost::assign::list_inserter<RefCallPushBack<This> > >::type
operator+=(boost::shared_ptr<DERIVEDFACTOR> factor) {
return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(factor);
@ -226,7 +236,7 @@ namespace gtsam {
/** Add a factor directly using a shared_ptr */
template<class DERIVEDFACTOR>
typename boost::enable_if<boost::is_base_of<FactorType, DERIVEDFACTOR> >::type
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
add(boost::shared_ptr<DERIVEDFACTOR> factor) {
push_back(factor);
}
@ -355,4 +365,4 @@ namespace gtsam {
} // namespace gtsam
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/inference/FactorGraph-inst.h>

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

@ -781,8 +781,8 @@ TEST(ImuFactor, bodyPSensorWithBias) {
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factors
graph.add(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<ImuFactor>(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim);
graph.emplace_shared<BetweenFactor<Bias> >(B(i - 1), B(i), zeroBias, biasNoiseModel);
values.insert(X(i), Pose3());
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::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
if (pose3Prior)
pose3Graph.add(
BetweenFactor<Pose3>(keyAnchor, pose3Prior->keys()[0],
pose3Prior->prior(), pose3Prior->noiseModel()));
pose3Graph.emplace_shared<BetweenFactor<Pose3> >(keyAnchor, pose3Prior->keys()[0],
pose3Prior->prior(), pose3Prior->noiseModel());
}
return pose3Graph;
}
@ -330,7 +329,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
// add prior
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
initialPose.insert(keyAnchor, Pose3());
pose3graph.add(PriorFactor<Pose3>(keyAnchor, Pose3(), priorModel));
pose3graph.emplace_shared<PriorFactor<Pose3> >(keyAnchor, Pose3(), priorModel);
// Create optimizer
GaussNewtonParams params;

4
gtsam/slam/tests/testAntiFactor.cpp
View File

@ -90,8 +90,8 @@ TEST( AntiFactor, EquivalentBayesNet)
SharedNoiseModel sigma(noiseModel::Unit::Create(6));
NonlinearFactorGraph graph;
graph.push_back(PriorFactor<Pose3>(1, pose1, sigma));
graph.push_back(BetweenFactor<Pose3>(1, 2, pose1.between(pose2), sigma));
graph.emplace_shared<PriorFactor<Pose3> >(1, pose1, sigma);
graph.emplace_shared<BetweenFactor<Pose3> >(1, 2, pose1.between(pose2), sigma);
// Create a configuration corresponding to the ground truth
Values values;

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));
NonlinearFactorGraph expectedGraph;
expectedGraph.add(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.add(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.add(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.add(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.add(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.add(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> >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(9,10, Pose2(1.003350, 0.022250, -0.195918), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3,10, Pose2(0.044020, 0.988477, -1.553511), model);
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
}
@ -164,27 +164,27 @@ TEST( dataSet, readG2o3D)
Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
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);
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);
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);
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);
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);
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));
}
@ -224,7 +224,7 @@ TEST( dataSet, readG2o3DNonDiagonalNoise)
NonlinearFactorGraph expectedGraph;
Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
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));
}
@ -242,18 +242,18 @@ TEST( dataSet, readG2oHuber)
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel);
NonlinearFactorGraph expectedGraph;
expectedGraph.add(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.add(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.add(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.add(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.add(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.add(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> >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(9,10, Pose2(1.003350, 0.022250, -0.195918), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3,10, Pose2(0.044020, 0.988477, -1.553511), model);
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);
NonlinearFactorGraph expectedGraph;
expectedGraph.add(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.add(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.add(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.add(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.add(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.add(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> >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(9,10, Pose2(1.003350, 0.022250, -0.195918), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3,10, Pose2(0.044020, 0.988477, -1.553511), model);
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
NonlinearFactorGraph graph;
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
Values truth;
@ -251,7 +251,7 @@ TEST (EssentialMatrixFactor2, minimization) {
// Noise sigma is 1cm, assuming metric measurements
NonlinearFactorGraph graph;
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
Values truth;
@ -323,7 +323,7 @@ TEST (EssentialMatrixFactor3, minimization) {
NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++)
// 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
Values truth;
@ -391,7 +391,7 @@ TEST (EssentialMatrixFactor, extraMinimization) {
NonlinearFactorGraph graph;
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
Values truth;
@ -465,7 +465,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) {
// Noise sigma is 1, assuming pixel measurements
NonlinearFactorGraph graph;
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
Values truth;

36
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -368,9 +368,9 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
graph.addCameraConstraint(0, cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.push_back(
RangeFactor<GeneralCamera, GeneralCamera>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 10.)));
graph.emplace_shared<
RangeFactor<GeneralCamera, GeneralCamera> >(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 10.));
const double reproj_error = 1e-5;
@ -385,12 +385,12 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
// Tests range factor between a GeneralCamera and a Pose3
Graph graph;
graph.addCameraConstraint(0, GeneralCamera());
graph.push_back(
RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 1.)));
graph.push_back(
PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
noiseModel::Isotropic::Sigma(6, 1.)));
graph.emplace_shared<
RangeFactor<GeneralCamera, Pose3> >(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 1.));
graph.emplace_shared<
PriorFactor<Pose3> >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
noiseModel::Isotropic::Sigma(6, 1.));
Values init;
init.insert(X(0), GeneralCamera());
@ -413,15 +413,15 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
// Tests range factor between a CalibratedCamera and a Pose3
NonlinearFactorGraph graph;
graph.push_back(
PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(),
noiseModel::Isotropic::Sigma(6, 1.)));
graph.push_back(
RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 1.)));
graph.push_back(
PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
noiseModel::Isotropic::Sigma(6, 1.)));
graph.emplace_shared<
PriorFactor<CalibratedCamera> >(X(0), CalibratedCamera(),
noiseModel::Isotropic::Sigma(6, 1.));
graph.emplace_shared<
RangeFactor<CalibratedCamera, Pose3> >(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 1.));
graph.emplace_shared<
PriorFactor<Pose3> >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
noiseModel::Isotropic::Sigma(6, 1.));
Values init;
init.insert(X(0), CalibratedCamera());

6
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -369,9 +369,9 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) {
graph.addCameraConstraint(0, cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.push_back(
RangeFactor<GeneralCamera, GeneralCamera>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 10.)));
graph.emplace_shared<
RangeFactor<GeneralCamera, GeneralCamera> >(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 10.));
const double reproj_error = 1e-5;

24
gtsam/slam/tests/testReferenceFrameFactor.cpp
View File

@ -136,15 +136,15 @@ TEST( ReferenceFrameFactor, converge_trans ) {
// Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 2.0); // beyond pi/2 - fails
NonlinearFactorGraph graph;
graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.push_back(PointReferenceFrameFactor(lB2, tA1, lA2));
graph.emplace_shared<PointReferenceFrameFactor>(lB1, tA1, lA1);
graph.emplace_shared<PointReferenceFrameFactor>(lB2, tA1, lA2);
// hard constraints on points
double error_gain = 1000.0;
graph.push_back(NonlinearEquality<gtsam::Point2>(lA1, local1, error_gain));
graph.push_back(NonlinearEquality<gtsam::Point2>(lA2, local2, error_gain));
graph.push_back(NonlinearEquality<gtsam::Point2>(lB1, global1, error_gain));
graph.push_back(NonlinearEquality<gtsam::Point2>(lB2, global2, error_gain));
graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lA1, local1, error_gain);
graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lA2, local2, error_gain);
graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lB1, global1, error_gain);
graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lB2, global2, error_gain);
// create initial estimate
Values init;
@ -186,9 +186,9 @@ TEST( ReferenceFrameFactor, converge_local ) {
NonlinearFactorGraph graph;
double error_gain = 1000.0;
graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.push_back(NonlinearEquality<gtsam::Point2>(lB1, global, error_gain));
graph.push_back(NonlinearEquality<gtsam::Pose2>(tA1, trans, error_gain));
graph.emplace_shared<PointReferenceFrameFactor>(lB1, tA1, lA1);
graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lB1, global, error_gain);
graph.emplace_shared<NonlinearEquality<gtsam::Pose2> >(tA1, trans, error_gain);
// create initial estimate
Values init;
@ -222,9 +222,9 @@ TEST( ReferenceFrameFactor, converge_global ) {
NonlinearFactorGraph graph;
double error_gain = 1000.0;
graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.push_back(NonlinearEquality<gtsam::Point2>(lA1, local, error_gain));
graph.push_back(NonlinearEquality<gtsam::Pose2>(tA1, trans, error_gain));
graph.emplace_shared<PointReferenceFrameFactor>(lB1, tA1, lA1);
graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lA1, local, error_gain);
graph.emplace_shared<NonlinearEquality<gtsam::Pose2> >(tA1, trans, error_gain);
// create initial estimate
Values init;

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

20
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -209,8 +209,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
// Create initial estimate
Values initial;
@ -321,8 +321,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
Values values;
values.insert(c1, cam1);
@ -398,8 +398,8 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
graph.push_back(smartFactor3);
graph.push_back(smartFactor4);
graph.push_back(smartFactor5);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
Values values;
values.insert(c1, cam1);
@ -476,8 +476,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
Values values;
values.insert(c1, cam1);
@ -552,8 +552,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
Values values;
values.insert(c1, cam1);

87
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -262,8 +262,8 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, bodyPose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, bodyPose2, noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, bodyPose1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, bodyPose2, noisePrior);
// Check errors at ground truth poses
Values gtValues;
@ -319,8 +319,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
Values groundTruth;
groundTruth.insert(x1, cam1.pose());
@ -547,8 +547,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -614,8 +614,8 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -675,8 +675,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -747,8 +747,8 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(smartFactor4);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
Values values;
values.insert(x1, cam1.pose());
@ -800,8 +800,8 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
@ -830,30 +830,21 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
NonlinearFactorGraph graph;
// Project three landmarks into three cameras
graph.push_back(
ProjectionFactor(cam1.project(landmark1), model, x1, L(1), sharedK2));
graph.push_back(
ProjectionFactor(cam2.project(landmark1), model, x2, L(1), sharedK2));
graph.push_back(
ProjectionFactor(cam3.project(landmark1), model, x3, L(1), sharedK2));
graph.emplace_shared<ProjectionFactor>(cam1.project(landmark1), model, x1, L(1), sharedK2);
graph.emplace_shared<ProjectionFactor>(cam2.project(landmark1), model, x2, L(1), sharedK2);
graph.emplace_shared<ProjectionFactor>(cam3.project(landmark1), model, x3, L(1), sharedK2);
graph.push_back(
ProjectionFactor(cam1.project(landmark2), model, x1, L(2), sharedK2));
graph.push_back(
ProjectionFactor(cam2.project(landmark2), model, x2, L(2), sharedK2));
graph.push_back(
ProjectionFactor(cam3.project(landmark2), model, x3, L(2), sharedK2));
graph.emplace_shared<ProjectionFactor>(cam1.project(landmark2), model, x1, L(2), sharedK2);
graph.emplace_shared<ProjectionFactor>(cam2.project(landmark2), model, x2, L(2), sharedK2);
graph.emplace_shared<ProjectionFactor>(cam3.project(landmark2), model, x3, L(2), sharedK2);
graph.push_back(
ProjectionFactor(cam1.project(landmark3), model, x1, L(3), sharedK2));
graph.push_back(
ProjectionFactor(cam2.project(landmark3), model, x2, L(3), sharedK2));
graph.push_back(
ProjectionFactor(cam3.project(landmark3), model, x3, L(3), sharedK2));
graph.emplace_shared<ProjectionFactor>(cam1.project(landmark3), model, x1, L(3), sharedK2);
graph.emplace_shared<ProjectionFactor>(cam2.project(landmark3), model, x2, L(3), sharedK2);
graph.emplace_shared<ProjectionFactor>(cam3.project(landmark3), model, x3, L(3), sharedK2);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, level_pose, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, pose_right, noisePrior);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
@ -1000,11 +991,9 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(
PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
graph.push_back(
PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
@ -1068,11 +1057,9 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(
PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
graph.push_back(
PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -1286,8 +1273,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -1362,11 +1349,9 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(
PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
graph.push_back(
PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),

4
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -185,11 +185,11 @@ TEST( StereoFactor, singlePoint)
{
NonlinearFactorGraph graph;
graph.push_back(NonlinearEquality<Pose3>(X(1), camera1));
graph.emplace_shared<NonlinearEquality<Pose3> >(X(1), camera1);
StereoPoint2 measurement(320, 320.0-50, 240);
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
graph.push_back(GenericStereoFactor<Pose3, Point3>(measurement, model, X(1), L(1), K));
graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(measurement, model, X(1), L(1), K);
// Create a configuration corresponding to the ground truth
Values values;

10
gtsam_unstable/examples/ConcurrentCalibration.cpp
View File

@ -61,7 +61,7 @@ int main(int argc, char** argv){
initial_estimate.insert(Symbol('K', 0), *noisy_K);
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished());
graph.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), *noisy_K, calNoise));
graph.emplace_shared<PriorFactor<Cal3_S2> >(Symbol('K', 0), *noisy_K, calNoise);
ifstream pose_file(pose_loc.c_str());
@ -77,7 +77,7 @@ int main(int argc, char** argv){
}
noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01);
graph.push_back(PriorFactor<Pose3>(Symbol('x', pose_id), Pose3(m), poseNoise));
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', pose_id), Pose3(m), poseNoise);
// camera and landmark keys
size_t x, l;
@ -89,9 +89,9 @@ int main(int argc, char** argv){
cout << "Reading stereo factors" << endl;
//read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
// graph.push_back( GenericStereoFactor<Pose3, Point3>(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K));
// graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K);
graph.push_back(GeneralSFMFactor2<Cal3_S2>(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0)));
graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0));
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
@ -107,7 +107,7 @@ int main(int argc, char** argv){
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x',1),first_pose);
cout << "Optimizing" << endl;
LevenbergMarquardtParams params;

2
gtsam_unstable/examples/SmartProjectionFactorExample.cpp
View File

@ -104,7 +104,7 @@ int main(int argc, char** argv){
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(1,firstPose));
graph.emplace_shared<NonlinearEquality<Pose3> >(1,firstPose);
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;

2
gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
View File

@ -126,7 +126,7 @@ int main(int argc, char** argv){
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x',1),first_pose);
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;

7
gtsam_unstable/linear/LPSolver.h
View File

@ -51,7 +51,7 @@ struct LPPolicy {
++it) {
size_t dim = lp.cost.getDim(it);
Vector b = xk.at(*it) - lp.cost.getA(it).transpose(); // b = xk-g
graph.push_back(JacobianFactor(*it, Matrix::Identity(dim, dim), b));
graph.emplace_shared<JacobianFactor>(*it, Matrix::Identity(dim, dim), b);
}
KeySet allKeys = lp.inequalities.keys();
@ -67,8 +67,7 @@ struct LPPolicy {
std::inserter(difference, difference.end()));
for (Key k : difference) {
size_t dim = lp.constrainedKeyDimMap().at(k);
graph.push_back(
JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k)));
graph.emplace_shared<JacobianFactor>(k, Matrix::Identity(dim, dim), xk.at(k));
}
}
return graph;
@ -77,4 +76,4 @@ struct LPPolicy {
using LPSolver = ActiveSetSolver<LP, LPPolicy, LPInitSolver>;
}
}

12
gtsam_unstable/linear/tests/testLPSolver.cpp
View File

@ -189,12 +189,12 @@ TEST(LPSolver, overConstrainedLinearSystem) {
TEST(LPSolver, overConstrainedLinearSystem2) {
GaussianFactorGraph graph;
graph.push_back(JacobianFactor(1, I_1x1, 2, I_1x1, kOne,
noiseModel::Constrained::All(1)));
graph.push_back(JacobianFactor(1, I_1x1, 2, -I_1x1, 5 * kOne,
noiseModel::Constrained::All(1)));
graph.push_back(JacobianFactor(1, I_1x1, 2, 2 * I_1x1, 6 * kOne,
noiseModel::Constrained::All(1)));
graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, I_1x1, kOne,
noiseModel::Constrained::All(1));
graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, -I_1x1, 5 * kOne,
noiseModel::Constrained::All(1));
graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, 2 * I_1x1, 6 * kOne,
noiseModel::Constrained::All(1));
VectorValues x = graph.optimize();
// This check confirms that gtsam linear constraint solver can't handle
// over-constrained system

58
gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
View File

@ -403,9 +403,9 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
NonlinearFactorGraph partialGraph;
partialGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
partialGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
partialGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
partialGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
Values partialValues;
partialValues.insert(1, optimalValues.at<Pose3>(1));
@ -437,10 +437,10 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.push_back(NonlinearFactor::shared_ptr());
// ==========================================================
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
for(const GaussianFactor::shared_ptr& factor: result) {
expectedGraph.push_back(LinearContainerFactor(factor, partialValues));
expectedGraph.emplace_shared<LinearContainerFactor>(factor, partialValues);
}
@ -1126,13 +1126,13 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 )
NonlinearFactorGraph actualGraph = filter.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
// we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
}
@ -1181,11 +1181,11 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_2 )
NonlinearFactorGraph actualGraph = filter.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
// we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
@ -1238,10 +1238,10 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_3 )
NonlinearFactorGraph expectedGraph;
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
}
@ -1258,11 +1258,11 @@ TEST( ConcurrentBatchFilter, removeFactors_values )
// Add some factors to the filter
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
newFactors.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
newFactors.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
newFactors.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
newFactors.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
@ -1291,11 +1291,11 @@ TEST( ConcurrentBatchFilter, removeFactors_values )
// note: factors are removed before the optimization
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
// we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());

48
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
View File

@ -617,13 +617,13 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 )
NonlinearFactorGraph actualGraph = smoother.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
// we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
}
@ -670,11 +670,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_2 )
NonlinearFactorGraph actualGraph = smoother.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
// we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
@ -724,10 +724,10 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_3 )
NonlinearFactorGraph expectedGraph;
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
}
@ -744,11 +744,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_values )
// Add some factors to the Smoother
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
newFactors.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
newFactors.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
newFactors.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
newFactors.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
@ -774,11 +774,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_values )
// note: factors are removed before the optimization
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
// we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());

52
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
View File

@ -423,9 +423,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
// ----------------------------------------------------------------------------------------------
NonlinearFactorGraph partialGraph;
partialGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
partialGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
partialGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
partialGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues);
@ -441,7 +441,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.push_back(NonlinearFactor::shared_ptr());
// ==========================================================
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
for(const GaussianFactor::shared_ptr& factor: marginal) {
// the linearization point for the linear container is optional, but it is not used in the filter,
@ -507,9 +507,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
// ----------------------------------------------------------------------------------------------
NonlinearFactorGraph partialGraph;
partialGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
partialGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
partialGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
partialGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
GaussianFactorGraph linearGraph = *partialGraph.linearize(optimalValues);
@ -525,7 +525,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.push_back(NonlinearFactor::shared_ptr());
// ==========================================================
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
for(const GaussianFactor::shared_ptr& factor: marginal) {
// the linearization point for the linear container is optional, but it is not used in the filter,
@ -1231,13 +1231,13 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 )
NonlinearFactorGraph actualGraph = filter.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery))
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
}
@ -1289,11 +1289,11 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 )
NonlinearFactorGraph actualGraph = filter.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
// we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
@ -1350,10 +1350,10 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 )
NonlinearFactorGraph expectedGraph;
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
}
@ -1406,11 +1406,11 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values )
Values actualValues = filter.getLinearizationPoint();
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
// we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());

8
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
View File

@ -639,13 +639,13 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 )
actualGraph.print("actual graph: \n");
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
// CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
}

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

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

4
gtsam_unstable/slam/tests/testSmartRangeFactor.cpp
View File

@ -116,8 +116,8 @@ TEST( SmartRangeFactor, optimization ) {
graph.push_back(f);
const noiseModel::Base::shared_ptr //
priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI));
graph.push_back(PriorFactor<Pose2>(1, pose1, priorNoise));
graph.push_back(PriorFactor<Pose2>(2, pose2, priorNoise));
graph.emplace_shared<PriorFactor<Pose2> >(1, pose1, priorNoise);
graph.emplace_shared<PriorFactor<Pose2> >(2, pose2, priorNoise);
// Try optimizing
LevenbergMarquardtParams params;

16
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -254,8 +254,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -394,8 +394,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -463,8 +463,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -545,8 +545,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(smartFactor4);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise

2
tests/testGeneralSFMFactorB.cpp
View File

@ -51,7 +51,7 @@ TEST(PinholeCamera, BAL) {
for (size_t j = 0; j < db.number_tracks(); j++) {
for (const SfM_Measurement& m: db.tracks[j].measurements)
graph.push_back(sfmFactor(m.second, unit2, m.first, P(j)));
graph.emplace_shared<sfmFactor>(m.second, unit2, m.first, P(j));
}
Values initial = initialCamerasAndPointsEstimate(db);

2
timing/timeSFMBAL.cpp
View File

@ -40,7 +40,7 @@ int main(int argc, char* argv[]) {
for (const SfM_Measurement& m: db.tracks[j].measurements) {
size_t i = m.first;
Point2 z = m.second;
graph.push_back(SfmFactor(z, gNoiseModel, C(i), P(j)));
graph.emplace_shared<SfmFactor>(z, gNoiseModel, C(i), P(j));
}
}