diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index 676dd42ec..204af1fea 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -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 factor; - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(55, 45), Point3(10, 10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(45, 45), Point3(-10, 10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(45, 55), Point3(-10, -10, 0))); - graph.push_back( - boost::make_shared(measurementNoise, X(1), calib, - Point2(55, 55), Point3(10, -10, 0))); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(55, 45), Point3(10, 10, 0)); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(45, 45), Point3(-10, 10, 0)); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(45, 55), Point3(-10, -10, 0)); + graph.emplace_shared(measurementNoise, X(1), calib, + Point2(55, 55), Point3(10, -10, 0)); /* 3. Create an initial estimate for the camera pose */ Values initial; diff --git a/examples/LocalizationExample.cpp b/examples/LocalizationExample.cpp index 84bca65f3..17f921fd1 100644 --- a/examples/LocalizationExample.cpp +++ b/examples/LocalizationExample.cpp @@ -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(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); - graph.add(BetweenFactor(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); + graph.emplace_shared >(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); + graph.emplace_shared >(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(1, 0.0, 0.0, unaryNoise)); - graph.add(boost::make_shared(2, 2.0, 0.0, unaryNoise)); - graph.add(boost::make_shared(3, 4.0, 0.0, unaryNoise)); + graph.emplace_shared(1, 0.0, 0.0, unaryNoise); + graph.emplace_shared(2, 2.0, 0.0, unaryNoise); + graph.emplace_shared(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 diff --git a/examples/METISOrderingExample.cpp b/examples/METISOrderingExample.cpp index eb6bdd49d..3416eb6b7 100644 --- a/examples/METISOrderingExample.cpp +++ b/examples/METISOrderingExample.cpp @@ -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(1, priorMean, priorNoise)); + graph.emplace_shared >(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(1, 2, odometry, odometryNoise)); - graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); + graph.emplace_shared >(1, 2, odometry, odometryNoise); + graph.emplace_shared >(2, 3, odometry, odometryNoise); graph.print("\nFactor Graph:\n"); // print Values initial; diff --git a/examples/OdometryExample.cpp b/examples/OdometryExample.cpp index b5cd681e5..3b547e70c 100644 --- a/examples/OdometryExample.cpp +++ b/examples/OdometryExample.cpp @@ -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(1, priorMean, priorNoise)); + graph.emplace_shared >(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(1, 2, odometry, odometryNoise)); - graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); + graph.emplace_shared >(1, 2, odometry, odometryNoise); + graph.emplace_shared >(2, 3, odometry, odometryNoise); graph.print("\nFactor Graph:\n"); // print // Create the data structure to hold the initialEstimate estimate to the solution diff --git a/examples/PlanarSLAMExample.cpp b/examples/PlanarSLAMExample.cpp index a716f9cd8..7c43c22e2 100644 --- a/examples/PlanarSLAMExample.cpp +++ b/examples/PlanarSLAMExample.cpp @@ -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(x1, prior, priorNoise)); // add directly to graph + graph.emplace_shared >(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(x1, x2, odometry, odometryNoise)); - graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); + graph.emplace_shared >(x1, x2, odometry, odometryNoise); + graph.emplace_shared >(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(x1, l1, bearing11, range11, measurementNoise)); - graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); - graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); + graph.emplace_shared >(x1, l1, bearing11, range11, measurementNoise); + graph.emplace_shared >(x2, l1, bearing21, range21, measurementNoise); + graph.emplace_shared >(x3, l2, bearing32, range32, measurementNoise); // Print graph.print("Factor Graph:\n"); diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index 7991f7fbf..f977a08af 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -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(1, Pose2(0, 0, 0), priorNoise)); + graph.emplace_shared >(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(1, 2, Pose2(2, 0, 0 ), model)); - graph.add(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); - graph.add(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(1, 2, Pose2(2, 0, 0 ), model); + graph.emplace_shared >(2, 3, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(3, 4, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(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(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(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 diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 314ccbdb4..99711da2d 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -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(1, Pose2(0, 0, 0), priorNoise)); + graph.emplace_shared >(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(1, 2, Pose2(2, 0, 0 ), model)); - graph.push_back(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); - graph.push_back(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); - graph.push_back(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(1, 2, Pose2(2, 0, 0 ), model); + graph.emplace_shared >(2, 3, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(3, 4, Pose2(2, 0, M_PI_2), model); + graph.emplace_shared >(4, 5, Pose2(2, 0, M_PI_2), model); // 2c. Add the loop closure constraint - graph.push_back(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + graph.emplace_shared >(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 diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 2c25d2f8e..9b459d7b8 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -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(1, prior, priorNoise)); + graph.emplace_shared >(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(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.push_back(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); - graph.push_back(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); + graph.emplace_shared >(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared >(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared >(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared >(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(5, 1, Pose2(0.0, 0.0, 0.0), model)); + graph.emplace_shared >(5, 1, Pose2(0.0, 0.0, 0.0), model); graph.print("\nFactor Graph:\n"); // print diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 893454936..0e48bb892 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -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(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + graph.emplace_shared >(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(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.emplace_shared >(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(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.emplace_shared >(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 diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index 4112afcad..e9d02b15c 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -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(0, poses[0], noise)); + graph.emplace_shared >(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(1, poses[1], noise)); // add directly to graph + graph.emplace_shared >(1, poses[1], noise); // add directly to graph graph.print("Factor Graph:\n"); diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 7f6c0ba71..da7c5ba9b 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -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(0, poses[0], noise)); + graph.emplace_shared >(0, poses[0], noise); // Fix the scale ambiguity by adding a prior - graph.push_back(PriorFactor(1, poses[0], noise)); + graph.emplace_shared >(1, poses[0], noise); // Create the initial estimate to the solution Values initialEstimate; diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 4c655fb7a..a3a416eb3 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -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(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(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); - graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); + graph.emplace_shared >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + graph.emplace_shared > (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); // Create initial estimate Values initial; diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 44a402b33..13a07dda5 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -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(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(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); - graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); + graph.emplace_shared >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)); + graph.emplace_shared >(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)); // Create initial estimate Values initial; diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index 1c5d155dc..93e010543 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/examples/SelfCalibrationExample.cpp @@ -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(Symbol('x', 0), poses[0], poseNoise)); + graph.emplace_shared >(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(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0))); + graph.emplace_shared >(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(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.emplace_shared >(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(Symbol('K', 0), K, calNoise)); + graph.emplace_shared >(Symbol('K', 0), K, calNoise); // Create the initial estimate to the solution // now including an estimate on the camera calibration parameters diff --git a/examples/StereoVOExample.cpp b/examples/StereoVOExample.cpp index 35d6747bf..27c10deb3 100644 --- a/examples/StereoVOExample.cpp +++ b/examples/StereoVOExample.cpp @@ -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(1, Pose3())); + graph.emplace_shared >(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(StereoPoint2(520, 480, 440), model, 1, 3, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(120, 80, 440), model, 1, 4, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(320, 280, 140), model, 1, 5, K)); + graph.emplace_shared >(StereoPoint2(520, 480, 440), model, 1, 3, K); + graph.emplace_shared >(StereoPoint2(120, 80, 440), model, 1, 4, K); + graph.emplace_shared >(StereoPoint2(320, 280, 140), model, 1, 5, K); //create and add stereo factors between second pose and the three landmarks - graph.push_back(GenericStereoFactor(StereoPoint2(570, 520, 490), model, 2, 3, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(70, 20, 490), model, 2, 4, K)); - graph.push_back(GenericStereoFactor(StereoPoint2(320, 270, 115), model, 2, 5, K)); + graph.emplace_shared >(StereoPoint2(570, 520, 490), model, 2, 3, K); + graph.emplace_shared >(StereoPoint2(70, 20, 490), model, 2, 4, K); + graph.emplace_shared >(StereoPoint2(320, 270, 115), model, 2, 5, K); //create Values object to contain initial estimates of camera poses and landmark locations Values initial_estimate; diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index 8b88c772d..80831bd21 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -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(StereoPoint2(uL, uR, v), model, - Symbol('x', x), Symbol('l', l), K)); + graph.emplace_shared< + GenericStereoFactor >(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(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(Symbol('x',1),first_pose)); + graph.emplace_shared >(Symbol('x',1),first_pose); cout << "Optimizing" << endl; //create Levenberg-Marquardt optimizer to optimize the factor graph diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 78bc463ec..157768be7 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -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(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); + graph.emplace_shared >(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(Symbol('x', 0), poses[0], poseNoise)); + graph.emplace_shared >(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(Symbol('l', 0), points[0], pointNoise)); // add directly to graph + graph.emplace_shared >(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 diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index a839289c2..8ca1be596 100644 --- a/examples/VisualISAMExample.cpp +++ b/examples/VisualISAMExample.cpp @@ -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(measurement, noise, - Symbol('x', i), Symbol('l', j), K)); + graph.emplace_shared >(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(Symbol('x', 0), poses[0], poseNoise)); + graph.emplace_shared >(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(Symbol('l', 0), points[0], pointNoise)); + graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // Add initial guesses to all observed landmarks Point3 noise(-0.25, 0.20, 0.15); diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index b322a40ab..18f311a23 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -56,7 +56,11 @@ protected: // Project and fill error vector Vector b(ZDim * m); for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - b.segment(row) = traits::Local(measured[i], predicted[i]); + Vector bi = traits::Local(measured[i], predicted[i]); + if(ZDim==3 && std::isnan(bi(1))){ // if it is a stereo point and the right pixel is missing (nan) + bi(1) = 0; + } + b.segment(row) = bi; } return b; } diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 5c6646454..c34d4d8f3 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -39,7 +39,10 @@ namespace gtsam { const Point3 q = leftCamPose_.transform_to(point); - if ( q.z() <= 0 ) throw StereoCheiralityException(); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (q.z() <= 0) + throw StereoCheiralityException(); +#endif // get calibration const Cal3_S2Stereo& K = *K_; diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 497599a6a..bc432cad3 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -104,6 +104,23 @@ TEST( StereoCamera, Dproject) CHECK(assert_equal(expected2,actual2,1e-7)); } +/* ************************************************************************* */ +TEST( StereoCamera, projectCheirality) +{ + // create a Stereo camera + Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); + StereoCamera stereoCam(Pose3(), K); + + // point behind the camera + Point3 p(0, 0, -5); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + CHECK_EXCEPTION(stereoCam.project2(p), StereoCheiralityException); +#else // otherwise project should not throw the exception + StereoPoint2 expected = StereoPoint2(320, 470, 240); + CHECK(assert_equal(expected,stereoCam.project2(p),1e-7)); +#endif +} + /* ************************************************************************* */ TEST( StereoCamera, backproject_case1) { diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index f57ca60d3..9e599f3f5 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -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(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK); + graph.emplace_shared(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK); const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9); - graph.push_back(PriorFactor(Symbol('x',1), Pose3(m1), posePrior)); - graph.push_back(PriorFactor(Symbol('x',2), Pose3(m2), posePrior)); + graph.emplace_shared >(Symbol('x',1), Pose3(m1), posePrior); + graph.emplace_shared >(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(cameras[0], measurements[0], unit, Symbol('l',1))); - graph.push_back(TriangulationFactor(cameras[1], measurements[1], unit, Symbol('l',1))); + graph.emplace_shared >(cameras[0], measurements[0], unit, Symbol('l',1)); + graph.emplace_shared >(cameras[1], measurements[1], unit, Symbol('l',1)); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 912b26262..71093d668 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -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(U(i), data[i], R_prior)); + graph.emplace_shared >(U(i), data[i], R_prior); } // Add process factors using the dot product error function. diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 81c8ed2f3..fdfe32e8b 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -89,8 +89,8 @@ std::pair triangulationGraph( const Pose3& pose_i = poses[i]; typedef PinholePose Camera; Camera camera_i(pose_i, sharedCal); - graph.push_back(TriangulationFactor // - (camera_i, measurements[i], unit2, landmarkKey)); + graph.emplace_shared > // + (camera_i, measurements[i], unit2, landmarkKey); } return std::make_pair(graph, values); } @@ -116,8 +116,8 @@ std::pair triangulationGraph( traits::dimension)); for (size_t i = 0; i < measurements.size(); i++) { const CAMERA& camera_i = cameras[i]; - graph.push_back(TriangulationFactor // - (camera_i, measurements[i], unit, landmarkKey)); + graph.emplace_shared > // + (camera_i, measurements[i], unit, landmarkKey); } return std::make_pair(graph, values); } diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 38f30f5a3..016488701 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -22,16 +22,17 @@ #pragma once +#include +#include +#include + #include #include #include #include -#include -#include -#include -#include -#include +#include +#include namespace gtsam { @@ -151,7 +152,7 @@ namespace gtsam { /** Add a factor directly using a shared_ptr */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(boost::shared_ptr factor) { factors_.push_back(boost::shared_ptr(factor)); } @@ -159,15 +160,22 @@ namespace gtsam { void push_back(const sharedFactor& factor) { factors_.push_back(factor); } + /** Emplace a factor */ + template + typename std::enable_if::value>::type + emplace_shared(Args&&... args) { + factors_.push_back(boost::make_shared(std::forward(args)...)); + } + /** push back many factors with an iterator over shared_ptr (factors are not copied) */ template - typename boost::enable_if >::type + typename std::enable_if::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 boost::enable_if >::type + typename std::enable_if::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 - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const BayesTree& 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 - typename boost::enable_if >::type + typename std::enable_if::value>::type push_back(const DERIVEDFACTOR& factor) { factors_.push_back(boost::make_shared(factor)); } +//#endif /** push back many factors with an iterator over plain factors (factors are copied) */ template - typename boost::enable_if >::type + typename std::enable_if::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 boost::enable_if >::type + typename std::enable_if::value>::type push_back(const CONTAINER& container) { push_back(container.begin(), container.end()); } /** Add a factor directly using a shared_ptr */ template - typename boost::enable_if, + typename std::enable_if::value, boost::assign::list_inserter > >::type operator+=(boost::shared_ptr factor) { return boost::assign::make_list_inserter(RefCallPushBack(*this))(factor); @@ -226,7 +236,7 @@ namespace gtsam { /** Add a factor directly using a shared_ptr */ template - typename boost::enable_if >::type + typename std::enable_if::value>::type add(boost::shared_ptr factor) { push_back(factor); } @@ -355,4 +365,4 @@ namespace gtsam { } // namespace gtsam -#include \ No newline at end of file +#include diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index d1dc6316d..6f2d60f2f 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -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(B(i - 1), B(i), zeroBias, biasNoiseModel)); + graph.emplace_shared(X(i - 1), V(i - 1), X(i), V(i), B(i - 1), pim); + graph.emplace_shared >(B(i - 1), B(i), zeroBias, biasNoiseModel); values.insert(X(i), Pose3()); values.insert(V(i), zeroVel); diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index f049e9d62..58408e7e3 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -120,9 +120,8 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { boost::shared_ptr > pose3Prior = boost::dynamic_pointer_cast >(factor); if (pose3Prior) - pose3Graph.add( - BetweenFactor(keyAnchor, pose3Prior->keys()[0], - pose3Prior->prior(), pose3Prior->noiseModel())); + pose3Graph.emplace_shared >(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(keyAnchor, Pose3(), priorModel)); + pose3graph.emplace_shared >(keyAnchor, Pose3(), priorModel); // Create optimizer GaussNewtonParams params; diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 93d2ff218..1453d80a7 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -202,7 +202,7 @@ public: boost::optional Fs = boost::none, // boost::optional E = boost::none) const { Vector ue = cameras.reprojectionError(point, measured_, Fs, E); - if(body_P_sensor_){ + if(body_P_sensor_ && Fs){ for(size_t i=0; i < Fs->size(); i++){ Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); Matrix J(6, 6); @@ -210,9 +210,17 @@ public: Fs->at(i) = Fs->at(i) * J; } } + correctForMissingMeasurements(cameras, ue, Fs, E); return ue; } + /** + * This corrects the Jacobians for the case in which some pixel measurement is missing (nan) + * In practice, this does not do anything in the monocular case, but it is implemented in the stereo version + */ + virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, boost::optional Fs = boost::none, + boost::optional E = boost::none) const {} + /** * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z] * Noise model applied diff --git a/gtsam/slam/SmartFactorParams.h b/gtsam/slam/SmartFactorParams.h new file mode 100644 index 000000000..761703f8b --- /dev/null +++ b/gtsam/slam/SmartFactorParams.h @@ -0,0 +1,134 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SmartFactorParams + * @brief Collect common parameters for SmartProjection and SmartStereoProjection factors + * @author Luca Carlone + * @author Zsolt Kira + * @author Frank Dellaert + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStereoProjection factors + */ +/// Linearization mode: what factor to linearize to +enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD +}; + +/// How to manage degeneracy +enum DegeneracyMode { + IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY +}; + +/* + * Parameters for the smart (stereo) projection factors + */ +struct GTSAM_EXPORT SmartProjectionParams { + + LinearizationMode linearizationMode; ///< How to linearize the factor + DegeneracyMode degeneracyMode; ///< How to linearize the factor + + /// @name Parameters governing the triangulation + /// @{ + TriangulationParameters triangulation; + double retriangulationThreshold; ///< threshold to decide whether to re-triangulate + /// @} + + /// @name Parameters governing how triangulation result is treated + /// @{ + bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) + bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) + /// @} + + // Constructor + SmartProjectionParams(LinearizationMode linMode = HESSIAN, + DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, + bool verboseCheirality = false, double retriangulationTh = 1e-5) : + linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( + retriangulationTh), throwCheirality(throwCheirality), verboseCheirality( + verboseCheirality) { + } + + virtual ~SmartProjectionParams() { + } + + void print(const std::string& str = "") const { + std::cout << "linearizationMode: " << linearizationMode << "\n"; + std::cout << " degeneracyMode: " << degeneracyMode << "\n"; + std::cout << triangulation << std::endl; + } + + // get class variables + LinearizationMode getLinearizationMode() const { + return linearizationMode; + } + DegeneracyMode getDegeneracyMode() const { + return degeneracyMode; + } + TriangulationParameters getTriangulationParameters() const { + return triangulation; + } + bool getVerboseCheirality() const { + return verboseCheirality; + } + bool getThrowCheirality() const { + return throwCheirality; + } + double getRetriangulationThreshold() const { + return retriangulationThreshold; + } + // set class variables + void setLinearizationMode(LinearizationMode linMode) { + linearizationMode = linMode; + } + void setDegeneracyMode(DegeneracyMode degMode) { + degeneracyMode = degMode; + } + void setRetriangulationThreshold(double retriangulationTh) { + retriangulationThreshold = retriangulationTh; + } + void setRankTolerance(double rankTol) { + triangulation.rankTolerance = rankTol; + } + void setEnableEPI(bool enableEPI) { + triangulation.enableEPI = enableEPI; + } + void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { + triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; + } + void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { + triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(linearizationMode); + ar & BOOST_SERIALIZATION_NVP(degeneracyMode); + ar & BOOST_SERIALIZATION_NVP(triangulation); + ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold); + ar & BOOST_SERIALIZATION_NVP(throwCheirality); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality); + } +}; + +} // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index d34ba11e3..840ecbc4d 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -20,6 +20,7 @@ #pragma once #include +#include #include #include @@ -31,109 +32,6 @@ namespace gtsam { -/// Linearization mode: what factor to linearize to -enum LinearizationMode { - HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD -}; - -/// How to manage degeneracy -enum DegeneracyMode { - IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY -}; - -/* - * Parameters for the smart projection factors - */ -struct GTSAM_EXPORT SmartProjectionParams { - - LinearizationMode linearizationMode; ///< How to linearize the factor - DegeneracyMode degeneracyMode; ///< How to linearize the factor - - /// @name Parameters governing the triangulation - /// @{ - TriangulationParameters triangulation; - double retriangulationThreshold; ///< threshold to decide whether to re-triangulate - /// @} - - /// @name Parameters governing how triangulation result is treated - /// @{ - bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) - bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) - /// @} - - // Constructor - SmartProjectionParams(LinearizationMode linMode = HESSIAN, - DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, - bool verboseCheirality = false, double retriangulationTh = 1e-5) : - linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( - retriangulationTh), throwCheirality(throwCheirality), verboseCheirality( - verboseCheirality) { - } - - virtual ~SmartProjectionParams() { - } - - void print(const std::string& str) const { - std::cout << "linearizationMode: " << linearizationMode << "\n"; - std::cout << " degeneracyMode: " << degeneracyMode << "\n"; - std::cout << triangulation << std::endl; - } - - LinearizationMode getLinearizationMode() const { - return linearizationMode; - } - DegeneracyMode getDegeneracyMode() const { - return degeneracyMode; - } - TriangulationParameters getTriangulationParameters() const { - return triangulation; - } - bool getVerboseCheirality() const { - return verboseCheirality; - } - bool getThrowCheirality() const { - return throwCheirality; - } - double getRetriangulationThreshold() const { - return retriangulationThreshold; - } - void setLinearizationMode(LinearizationMode linMode) { - linearizationMode = linMode; - } - void setRetriangulationThreshold(double retriangulationTh) { - retriangulationThreshold = retriangulationTh; - } - void setDegeneracyMode(DegeneracyMode degMode) { - degeneracyMode = degMode; - } - void setRankTolerance(double rankTol) { - triangulation.rankTolerance = rankTol; - } - void setEnableEPI(bool enableEPI) { - triangulation.enableEPI = enableEPI; - } - void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { - triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; - } - void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { - triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; - } - -private: - - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(linearizationMode); - ar & BOOST_SERIALIZATION_NVP(degeneracyMode); - ar & BOOST_SERIALIZATION_NVP(triangulation); - ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold); - ar & BOOST_SERIALIZATION_NVP(throwCheirality); - ar & BOOST_SERIALIZATION_NVP(verboseCheirality); - } -}; - /** * SmartProjectionFactor: triangulates point and keeps an estimate of it around. * This factor operates with monocular cameras, where a camera is expected to diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 7a95e0fa9..e34e13279 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -90,8 +90,8 @@ TEST( AntiFactor, EquivalentBayesNet) SharedNoiseModel sigma(noiseModel::Unit::Create(6)); NonlinearFactorGraph graph; - graph.push_back(PriorFactor(1, pose1, sigma)); - graph.push_back(BetweenFactor(1, 2, pose1.between(pose2), sigma)); + graph.emplace_shared >(1, pose1, sigma); + graph.emplace_shared >(1, 2, pose1.between(pose2), sigma); // Create a configuration corresponding to the ground truth Values values; diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 1dd4e8abc..b3e208b91 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -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(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); - expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); - expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); - expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); - expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); - expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); - expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); - expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); - expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); - expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); - expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); - expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); + expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); + expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); + expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); + expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); + expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); + expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); + expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); + expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); + expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); + expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); + expectedGraph.emplace_shared >(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(0, 1, Pose3(R01,p01), model)); + expectedGraph.emplace_shared >(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(1, 2, Pose3(R12,p12), model)); + expectedGraph.emplace_shared >(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(2, 3, Pose3(R23,p23), model)); + expectedGraph.emplace_shared >(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(3, 4, Pose3(R34,p34), model)); + expectedGraph.emplace_shared >(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(1, 4, Pose3(R14,p14), model)); + expectedGraph.emplace_shared >(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(3, 0, Pose3(R30,p30), model)); + expectedGraph.emplace_shared >(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(0, 1, Pose3(R01,p01), model)); + expectedGraph.emplace_shared >(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(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); - expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); - expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); - expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); - expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); - expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); - expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); - expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); - expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); - expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); - expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); - expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); + expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); + expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); + expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); + expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); + expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); + expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); + expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); + expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); + expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); + expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); + expectedGraph.emplace_shared >(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(0, 1, Pose2(1.030390, 0.011350, -0.081596), model)); - expectedGraph.add(BetweenFactor(1, 2, Pose2(1.013900, -0.058639, -0.220291), model)); - expectedGraph.add(BetweenFactor(2, 3, Pose2(1.027650, -0.007456, -0.043627), model)); - expectedGraph.add(BetweenFactor(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model)); - expectedGraph.add(BetweenFactor(4, 5, Pose2(1.016030, 0.014565, -0.030930), model)); - expectedGraph.add(BetweenFactor(5, 6, Pose2(1.023890, 0.006808, -0.007452), model)); - expectedGraph.add(BetweenFactor(6, 7, Pose2(0.957734, 0.003159, 0.082836), model)); - expectedGraph.add(BetweenFactor(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model)); - expectedGraph.add(BetweenFactor(8, 9, Pose2(1.023440, 0.013984, -0.127624), model)); - expectedGraph.add(BetweenFactor(9,10, Pose2(1.003350, 0.022250, -0.195918), model)); - expectedGraph.add(BetweenFactor(5, 9, Pose2(0.033943, 0.032439, 3.073637), model)); - expectedGraph.add(BetweenFactor(3,10, Pose2(0.044020, 0.988477, -1.553511), model)); + expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); + expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); + expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); + expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); + expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); + expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); + expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); + expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); + expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); + expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); + expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); + expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 2e3d613d6..d33551edf 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -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(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(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(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(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(100, i, pA(i), pB(i), model2, K); // Check error at ground truth Values truth; diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index b99cb5d9c..8e0b5ad8f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -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(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 10.))); + graph.emplace_shared< + RangeFactor >(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(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 1.))); - graph.push_back( - PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), - noiseModel::Isotropic::Sigma(6, 1.))); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.)); + graph.emplace_shared< + PriorFactor >(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(X(0), CalibratedCamera(), - noiseModel::Isotropic::Sigma(6, 1.))); - graph.push_back( - RangeFactor(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 1.))); - graph.push_back( - PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), - noiseModel::Isotropic::Sigma(6, 1.))); + graph.emplace_shared< + PriorFactor >(X(0), CalibratedCamera(), + noiseModel::Isotropic::Sigma(6, 1.)); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.)); + graph.emplace_shared< + PriorFactor >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + noiseModel::Isotropic::Sigma(6, 1.)); Values init; init.insert(X(0), CalibratedCamera()); diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 9fda7d745..d8ac0aa5b 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -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(X(0), X(1), 2., - noiseModel::Isotropic::Sigma(1, 10.))); + graph.emplace_shared< + RangeFactor >(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.)); const double reproj_error = 1e-5; diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index f955aa191..24b818835 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -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(lB1, tA1, lA1); + graph.emplace_shared(lB2, tA1, lA2); // hard constraints on points double error_gain = 1000.0; - graph.push_back(NonlinearEquality(lA1, local1, error_gain)); - graph.push_back(NonlinearEquality(lA2, local2, error_gain)); - graph.push_back(NonlinearEquality(lB1, global1, error_gain)); - graph.push_back(NonlinearEquality(lB2, global2, error_gain)); + graph.emplace_shared >(lA1, local1, error_gain); + graph.emplace_shared >(lA2, local2, error_gain); + graph.emplace_shared >(lB1, global1, error_gain); + graph.emplace_shared >(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(lB1, global, error_gain)); - graph.push_back(NonlinearEquality(tA1, trans, error_gain)); + graph.emplace_shared(lB1, tA1, lA1); + graph.emplace_shared >(lB1, global, error_gain); + graph.emplace_shared >(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(lA1, local, error_gain)); - graph.push_back(NonlinearEquality(tA1, trans, error_gain)); + graph.emplace_shared(lB1, tA1, lA1); + graph.emplace_shared >(lA1, local, error_gain); + graph.emplace_shared >(tA1, trans, error_gain); // create initial estimate Values init; diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 9bb296444..caf3d31df 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -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(1, i1Ri2, c1Zc2, model); + graph.emplace_shared(1, i2Ri3, c2Zc3, model); + graph.emplace_shared(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(1, p1, z1, model); + graph.emplace_shared(1, p2, z2, model); + graph.emplace_shared(1, p3, z3, model); // Check error at ground truth Values truth; diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 0db8149eb..4feeadc86 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -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(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(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(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(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(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(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(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(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(c1, cam1, noisePrior)); - graph.push_back(PriorFactor(c2, cam2, noisePrior)); + graph.emplace_shared >(c1, cam1, noisePrior); + graph.emplace_shared >(c2, cam2, noisePrior); Values values; values.insert(c1, cam1); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index f0a864fdd..71ee79d86 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -262,8 +262,8 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, bodyPose1, noisePrior)); - graph.push_back(PriorFactor(x2, bodyPose2, noisePrior)); + graph.emplace_shared >(x1, bodyPose1, noisePrior); + graph.emplace_shared >(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(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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(cam1.project(landmark1), model, x1, L(1), sharedK2); + graph.emplace_shared(cam2.project(landmark1), model, x2, L(1), sharedK2); + graph.emplace_shared(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(cam1.project(landmark2), model, x1, L(2), sharedK2); + graph.emplace_shared(cam2.project(landmark2), model, x2, L(2), sharedK2); + graph.emplace_shared(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(cam1.project(landmark3), model, x1, L(3), sharedK2); + graph.emplace_shared(cam2.project(landmark3), model, x2, L(3), sharedK2); + graph.emplace_shared(cam3.project(landmark3), model, x3, L(3), sharedK2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); - graph.push_back(PriorFactor(x2, pose_right, noisePrior)); + graph.emplace_shared >(x1, level_pose, noisePrior); + graph.emplace_shared >(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(x1, cam1.pose(), noisePrior)); - graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(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 @@ -1022,6 +1011,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { + // this test considers a condition in which the cheirality constraint is triggered using namespace vanillaPose; vector views; @@ -1068,11 +1058,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(x1, cam1.pose(), noisePrior)); - graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(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), @@ -1096,8 +1084,14 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY) // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION EXPECT(assert_equal(Pose3(values.at(x3).rotation(), Point3(0,0,1)), result.at(x3))); +#else + // if the check is disabled, no cheirality exception if thrown and the pose converges to the right rotation + // with modest accuracy since the configuration is essentially degenerate without the translation due to noise (noise_pose) + EXPECT(assert_equal(pose3, result.at(x3),1e-3)); +#endif } /* *************************************************************************/ @@ -1286,8 +1280,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back(PriorFactor(x2, cam2.pose(), noisePrior)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(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 +1356,9 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, cam1.pose(), noisePrior)); - graph.push_back( - PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back( - PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); + graph.emplace_shared >(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(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), diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 45b978c27..c802603db 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -185,11 +185,11 @@ TEST( StereoFactor, singlePoint) { NonlinearFactorGraph graph; - graph.push_back(NonlinearEquality(X(1), camera1)); + graph.emplace_shared >(X(1), camera1); StereoPoint2 measurement(320, 320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph.push_back(GenericStereoFactor(measurement, model, X(1), L(1), K)); + graph.emplace_shared >(measurement, model, X(1), L(1), K); // Create a configuration corresponding to the ground truth Values values; diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index 28f7e8444..fe1c83ce3 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -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(Symbol('K', 0), *noisy_K, calNoise)); + graph.emplace_shared >(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(Symbol('x', pose_id), Pose3(m), poseNoise)); + graph.emplace_shared >(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(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K)); +// graph.emplace_shared >(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K); - graph.push_back(GeneralSFMFactor2(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0))); + graph.emplace_shared >(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(Symbol('x',1),first_pose)); + graph.emplace_shared >(Symbol('x',1),first_pose); cout << "Optimizing" << endl; LevenbergMarquardtParams params; diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index c5ba616ed..94a70470a 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -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(1,firstPose)); + graph.emplace_shared >(1,firstPose); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 40ad0cb52..c8023b23c 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -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(Symbol('x',1),first_pose)); + graph.emplace_shared >(Symbol('x',1),first_pose); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 91cee3941..153aa7fda 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -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(*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(k, Matrix::Identity(dim, dim), xk.at(k)); } } return graph; @@ -77,4 +76,4 @@ struct LPPolicy { using LPSolver = ActiveSetSolver; -} \ No newline at end of file +} diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index 8bf6be56b..a105a39f0 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -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(1, I_1x1, 2, I_1x1, kOne, + noiseModel::Constrained::All(1)); + graph.emplace_shared(1, I_1x1, 2, -I_1x1, 5 * kOne, + noiseModel::Constrained::All(1)); + graph.emplace_shared(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 diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index 74ee23fe5..4aff1b465 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -403,9 +403,9 @@ TEST( ConcurrentBatchFilter, update_and_marginalize ) NonlinearFactorGraph partialGraph; - partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + partialGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); Values partialValues; partialValues.insert(1, optimalValues.at(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(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); for(const GaussianFactor::shared_ptr& factor: result) { - expectedGraph.push_back(LinearContainerFactor(factor, partialValues)); + expectedGraph.emplace_shared(factor, partialValues); } @@ -1126,13 +1126,13 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = filter.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + // we removed this one: expectedGraph.emplace_shared >(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(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(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(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(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(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(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(1, poseInitial, noisePrior)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.emplace_shared >(1, poseInitial, noisePrior); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(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(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(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()); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index dc316e2ac..a0b6e2c1b 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -617,13 +617,13 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 ) NonlinearFactorGraph actualGraph = smoother.getFactors(); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + // we removed this one: expectedGraph.emplace_shared >(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(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(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(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(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(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(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(1, poseInitial, noisePrior)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - newFactors.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + newFactors.emplace_shared >(1, poseInitial, noisePrior); + newFactors.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + newFactors.emplace_shared >(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(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(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()); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index 0c922fb9e..d6b7ab851 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -423,9 +423,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 ) // ---------------------------------------------------------------------------------------------- NonlinearFactorGraph partialGraph; - partialGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); - partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + partialGraph.emplace_shared >(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(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(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(1, poseInitial, noisePrior)); - partialGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - partialGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); + partialGraph.emplace_shared >(1, poseInitial, noisePrior); + partialGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + partialGraph.emplace_shared >(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(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(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(1, poseInitial, noisePrior)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); // we removed this one: expectedGraph.push_back(BetweenFactor(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(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(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(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(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(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(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(1, poseInitial, noisePrior)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - // we removed this one: expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + // we removed this one: expectedGraph.emplace_shared >(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()); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 65f74dc3c..fdf9f08b5 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -639,13 +639,13 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 ) actualGraph.print("actual graph: \n"); NonlinearFactorGraph expectedGraph; - expectedGraph.push_back(PriorFactor(1, poseInitial, noisePrior)); + expectedGraph.emplace_shared >(1, poseInitial, noisePrior); // we removed this one: expectedGraph.push_back(BetweenFactor(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(2, 3, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(3, 4, poseOdometry, noiseOdometery)); - expectedGraph.push_back(BetweenFactor(1, 2, poseOdometry, noiseOdometery)); + expectedGraph.emplace_shared >(2, 3, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(3, 4, poseOdometry, noiseOdometery); + expectedGraph.emplace_shared >(1, 2, poseOdometry, noiseOdometery); // CHECK(assert_equal(expectedGraph, actualGraph, 1e-6)); } diff --git a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp index 9391814d4..2240034af 100644 --- a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp +++ b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp @@ -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(x1, prior, priorNoise)); + graph.emplace_shared >(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(x1, x2, odometry, odometryNoise)); - graph.add(BetweenFactor(x2, x3, odometry, odometryNoise)); + graph.emplace_shared >(x1, x2, odometry, odometryNoise); + graph.emplace_shared >(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(x1, l1, bearing11, range11, measurementNoise)); - graph.add(BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise)); - graph.add(BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise)); + graph.emplace_shared >(x1, l1, bearing11, range11, measurementNoise); + graph.emplace_shared >(x2, l1, bearing21, range21, measurementNoise); + graph.emplace_shared >(x3, l2, bearing32, range32, measurementNoise); return graph; } diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 21b0c5eb7..29ae6233b 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -21,6 +21,7 @@ #pragma once #include +#include #include #include @@ -35,91 +36,10 @@ namespace gtsam { -/// Linearization mode: what factor to linearize to - enum LinearizationMode { - HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD - }; - -/// How to manage degeneracy -enum DegeneracyMode { - IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY - }; - - /* - * Parameters for the smart stereo projection factors - */ - struct GTSAM_EXPORT SmartStereoProjectionParams { - - LinearizationMode linearizationMode; ///< How to linearize the factor - DegeneracyMode degeneracyMode; ///< How to linearize the factor - - /// @name Parameters governing the triangulation - /// @{ - TriangulationParameters triangulation; - double retriangulationThreshold; ///< threshold to decide whether to re-triangulate - /// @} - - /// @name Parameters governing how triangulation result is treated - /// @{ - bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) - bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) - /// @} - - - /// Constructor - SmartStereoProjectionParams(LinearizationMode linMode = HESSIAN, - DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, - bool verboseCheirality = false) : - linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( - 1e-5), throwCheirality(throwCheirality), verboseCheirality( - verboseCheirality) { - } - - virtual ~SmartStereoProjectionParams() { - } - - void print(const std::string& str) const { - std::cout << "linearizationMode: " << linearizationMode << "\n"; - std::cout << " degeneracyMode: " << degeneracyMode << "\n"; - std::cout << triangulation << std::endl; - } - - LinearizationMode getLinearizationMode() const { - return linearizationMode; - } - DegeneracyMode getDegeneracyMode() const { - return degeneracyMode; - } - TriangulationParameters getTriangulationParameters() const { - return triangulation; - } - bool getVerboseCheirality() const { - return verboseCheirality; - } - bool getThrowCheirality() const { - return throwCheirality; - } - void setLinearizationMode(LinearizationMode linMode) { - linearizationMode = linMode; - } - void setDegeneracyMode(DegeneracyMode degMode) { - degeneracyMode = degMode; - } - void setRankTolerance(double rankTol) { - triangulation.rankTolerance = rankTol; - } - void setEnableEPI(bool enableEPI) { - triangulation.enableEPI = enableEPI; - } - void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { - triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; - } - void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { - triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; - } - }; - - +/* + * Parameters for the smart stereo projection factors (identical to the SmartProjectionParams) + */ +typedef SmartProjectionParams SmartStereoProjectionParams; /** * SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around. @@ -155,14 +75,19 @@ public: /// Vector of cameras typedef CameraSet Cameras; + /// Vector of monocular cameras (stereo treated as 2 monocular) + typedef PinholeCamera MonoCamera; + typedef CameraSet MonoCameras; + typedef std::vector MonoMeasurements; + /** * Constructor * @param params internal parameters of the smart factors */ SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel, - const SmartStereoProjectionParams& params = - SmartStereoProjectionParams()) : - Base(sharedNoiseModel), // + const SmartStereoProjectionParams& params = SmartStereoProjectionParams(), + const boost::optional body_P_sensor = boost::none) : + Base(sharedNoiseModel, body_P_sensor), // params_(params), // result_(TriangulationResult::Degenerate()) { } @@ -240,75 +165,28 @@ public: size_t m = cameras.size(); bool retriangulate = decideIfTriangulate(cameras); -// if(!retriangulate) -// std::cout << "retriangulate = false" << std::endl; -// -// bool retriangulate = true; - - if (retriangulate) { -// std::cout << "Retriangulate " << std::endl; - std::vector reprojections; - reprojections.reserve(m); - for(size_t i = 0; i < m; i++) { - reprojections.push_back(cameras[i].backproject(measured_[i])); + // triangulate stereo measurements by treating each stereocamera as a pair of monocular cameras + MonoCameras monoCameras; + MonoMeasurements monoMeasured; + for(size_t i = 0; i < m; i++) { + const Pose3 leftPose = cameras[i].pose(); + const Cal3_S2 monoCal = cameras[i].calibration().calibration(); + const MonoCamera leftCamera_i(leftPose,monoCal); + const Pose3 left_Pose_right = Pose3(Rot3(),Point3(cameras[i].baseline(),0.0,0.0)); + const Pose3 rightPose = leftPose.compose( left_Pose_right ); + const MonoCamera rightCamera_i(rightPose,monoCal); + const StereoPoint2 zi = measured_[i]; + monoCameras.push_back(leftCamera_i); + monoMeasured.push_back(Point2(zi.uL(),zi.v())); + if(!std::isnan(zi.uR())){ // if right point is valid + monoCameras.push_back(rightCamera_i); + monoMeasured.push_back(Point2(zi.uR(),zi.v())); } - - Point3 pw_sum(0,0,0); - for(const Point3& pw: reprojections) { - pw_sum = pw_sum + pw; - } - // average reprojected landmark - Point3 pw_avg = pw_sum / double(m); - - double totalReprojError = 0; - - // check if it lies in front of all cameras - for(size_t i = 0; i < m; i++) { - const Pose3& pose = cameras[i].pose(); - const Point3& pl = pose.transform_to(pw_avg); - if (pl.z() <= 0) { - result_ = TriangulationResult::BehindCamera(); - return result_; - } - - // check landmark distance - if (params_.triangulation.landmarkDistanceThreshold > 0 && - pl.norm() > params_.triangulation.landmarkDistanceThreshold) { - result_ = TriangulationResult::FarPoint(); - return result_; - } - - if (params_.triangulation.dynamicOutlierRejectionThreshold > 0) { - const StereoPoint2& zi = measured_[i]; - StereoPoint2 reprojectionError(cameras[i].project(pw_avg) - zi); - totalReprojError += reprojectionError.vector().norm(); - } - } // for - - if (params_.triangulation.dynamicOutlierRejectionThreshold > 0 - && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) { - result_ = TriangulationResult::Outlier(); - return result_; - } - - if(params_.triangulation.enableEPI) { - try { - pw_avg = triangulateNonlinear(cameras, measured_, pw_avg); - } catch(StereoCheiralityException& e) { - if(params_.verboseCheirality) - std::cout << "Cheirality Exception in SmartStereoProjectionFactor" << std::endl; - if(params_.throwCheirality) - throw; - result_ = TriangulationResult::BehindCamera(); - return TriangulationResult::BehindCamera(); - } - } - - result_ = TriangulationResult(pw_avg); - - } // if retriangulate + } + if (retriangulate) + result_ = gtsam::triangulateSafe(monoCameras, monoMeasured, + params_.triangulation); return result_; - } /// triangulate @@ -570,6 +448,32 @@ public: } } + /** + * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan) + */ + virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, + boost::optional Fs = boost::none, + boost::optional E = boost::none) const + { + // when using stereo cameras, some of the measurements might be missing: + for(size_t i=0; i < cameras.size(); i++){ + const StereoPoint2& z = measured_.at(i); + if(std::isnan(z.uR())) // if the right pixel is invalid + { + if(Fs){ // delete influence of right point on jacobian Fs + MatrixZD& Fi = Fs->at(i); + for(size_t ii=0; iirow(ZDim * i + 1) = Matrix::Zero(1, E->cols()); + + // set the corresponding entry of vector ue to zero + ue(ZDim * i + 1) = 0.0; + } + } + } + /** return the landmark */ TriangulationResult point() const { return result_; diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 65134cb96..3b4c5e0db 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -66,9 +66,9 @@ public: * @param params internal parameters of the smart factors */ SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, - const SmartStereoProjectionParams& params = - SmartStereoProjectionParams()) : - Base(sharedNoiseModel, params) { + const SmartStereoProjectionParams& params = SmartStereoProjectionParams(), + const boost::optional body_P_sensor = boost::none) : + Base(sharedNoiseModel, params, body_P_sensor) { } /** Virtual destructor */ @@ -102,7 +102,7 @@ public: /** * Variant of the previous one in which we include a set of measurements with the same noise and calibration - * @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) + * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) * @param poseKeys vector of keys corresponding to the camera observing the same landmark * @param K the (known) camera calibration (same for all measurements) */ @@ -161,7 +161,11 @@ public: Base::Cameras cameras; size_t i=0; for(const Key& k: this->keys_) { - const Pose3& pose = values.at(k); + Pose3 pose = values.at(k); + + if (Base::body_P_sensor_) + pose = pose.compose(*(Base::body_P_sensor_)); + StereoCamera camera(pose, K_all_[i++]); cameras.push_back(camera); } diff --git a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp index 9ae4aa928..e18505af6 100644 --- a/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartRangeFactor.cpp @@ -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(1, pose1, priorNoise)); - graph.push_back(PriorFactor(2, pose2, priorNoise)); + graph.emplace_shared >(1, pose1, priorNoise); + graph.emplace_shared >(2, pose2, priorNoise); // Try optimizing LevenbergMarquardtParams params; diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 8051e238a..a018ec7ee 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -18,7 +18,7 @@ * @date Sept 2013 */ -// TODO #include +#include #include #include #include @@ -33,8 +33,6 @@ using namespace boost::assign; using namespace gtsam; // make a realistic calibration matrix -static double fov = 60; // degrees -static size_t w = 640, h = 480; static double b = 1; static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); @@ -62,6 +60,8 @@ static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more re static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +static double missing_uR = std::numeric_limits::quiet_NaN(); + vector stereo_projectToMultipleCameras(const StereoCamera& cam1, const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { @@ -79,6 +79,35 @@ vector stereo_projectToMultipleCameras(const StereoCamera& cam1, LevenbergMarquardtParams lm_params; +/* ************************************************************************* */ +TEST( SmartStereoProjectionPoseFactor, params) { + SmartStereoProjectionParams p; + + // check default values and "get" + EXPECT(p.getLinearizationMode() == HESSIAN); + EXPECT(p.getDegeneracyMode() == IGNORE_DEGENERACY); + EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-5, 1e-9); + EXPECT(p.getVerboseCheirality() == false); + EXPECT(p.getThrowCheirality() == false); + + // check "set" + p.setLinearizationMode(JACOBIAN_SVD); + p.setDegeneracyMode(ZERO_ON_DEGENERACY); + p.setRankTolerance(100); + p.setEnableEPI(true); + p.setLandmarkDistanceThreshold(200); + p.setDynamicOutlierRejectionThreshold(3); + p.setRetriangulationThreshold(1e-2); + + EXPECT(p.getLinearizationMode() == JACOBIAN_SVD); + EXPECT(p.getDegeneracyMode() == ZERO_ON_DEGENERACY); + EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().rankTolerance, 100, 1e-5); + EXPECT(p.getTriangulationParameters().enableEPI == true); + EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().landmarkDistanceThreshold, 200, 1e-5); + EXPECT_DOUBLES_EQUAL(p.getTriangulationParameters().dynamicOutlierRejectionThreshold, 3, 1e-5); + EXPECT_DOUBLES_EQUAL(p.getRetriangulationThreshold(), 1e-2, 1e-5); +} + /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Constructor) { SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); @@ -151,6 +180,60 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) { //EXPECT(assert_equal(zero(4),actual,1e-8)); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, noiselessWithMissingMeasurements ) { + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 level_pose = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0, 0, 1)); + StereoCamera level_camera(level_pose, K2); + + // create second camera 1 meter to the right of first camera + Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera level_camera_right(level_pose_right, K2); + + // landmark ~5 meters in front of camera + Point3 landmark(5, 0.5, 1.2); + + // 1. Project two landmarks into two cameras and triangulate + StereoPoint2 level_uv = level_camera.project(landmark); + StereoPoint2 level_uv_right = level_camera_right.project(landmark); + StereoPoint2 level_uv_right_missing(level_uv_right.uL(),missing_uR,level_uv_right.v()); + + Values values; + values.insert(x1, level_pose); + values.insert(x2, level_pose_right); + + SmartStereoProjectionPoseFactor factor1(model); + factor1.add(level_uv, x1, K2); + factor1.add(level_uv_right_missing, x2, K2); + + double actualError = factor1.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + // TEST TRIANGULATION WITH MISSING VALUES: i) right pixel of second camera is missing: + SmartStereoProjectionPoseFactor::Cameras cameras = factor1.cameras(values); + double actualError2 = factor1.totalReprojectionError(cameras); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); + + CameraSet cams; + cams += level_camera; + cams += level_camera_right; + TriangulationResult result = factor1.triangulateSafe(cams); + CHECK(result); + EXPECT(assert_equal(landmark, *result, 1e-7)); + + // TEST TRIANGULATION WITH MISSING VALUES: ii) right pixels of both cameras are missing: + SmartStereoProjectionPoseFactor factor2(model); + StereoPoint2 level_uv_missing(level_uv.uL(),missing_uR,level_uv.v()); + factor2.add(level_uv_missing, x1, K2); + factor2.add(level_uv_right_missing, x2, K2); + result = factor2.triangulateSafe(cams); + CHECK(result); + EXPECT(assert_equal(landmark, *result, 1e-7)); +} + /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, noisy ) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) @@ -248,14 +331,12 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(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), @@ -273,7 +354,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { Point3(0.1, -0.1, 1.9)), values.at(x3))); // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl; - EXPECT_DOUBLES_EQUAL(797312.95069157204, graph.error(values), 1e-7); + EXPECT_DOUBLES_EQUAL(833953.92789459578, graph.error(values), 1e-7); // initial error // get triangulated landmarks from smart factors Point3 landmark1_smart = *smartFactor1->point(); @@ -335,7 +416,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { graph2.push_back(ProjectionFactor(measurements_l3[2], model, x3, L(3), K2, false, verboseCheirality)); // cout << std::setprecision(10) << "\n----StereoFactor graph initial error: " << graph2.error(values) << endl; - EXPECT_DOUBLES_EQUAL(797312.95069157204, graph2.error(values), 1e-7); + EXPECT_DOUBLES_EQUAL(833953.92789459578, graph2.error(values), 1e-7); LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params); Values result2 = optimizer2.optimize(); @@ -344,7 +425,192 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { // cout << std::setprecision(10) << "StereoFactor graph optimized error: " << graph2.error(result2) << endl; } +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) { + // camera has some displacement + Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.01, 0., -0.05), Point3(0.1, 0, 0.1)); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + StereoCamera cam1(pose1.compose(body_P_sensor), K2); + + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera cam2(pose2.compose(body_P_sensor), K2); + + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); + StereoCamera cam3(pose3.compose(body_P_sensor), K2); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + // 1. Project three landmarks into three cameras and triangulate + vector measurements_l1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_l2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_l3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + SmartStereoProjectionParams smart_params; + smart_params.triangulation.enableEPI = true; + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor)); + smartFactor1->add(measurements_l1, views, K2); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor)); + smartFactor2->add(measurements_l2, views, K2); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params, body_P_sensor)); + smartFactor3->add(measurements_l3, views, K2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(PriorFactor(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(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), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); + + // cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl; + EXPECT_DOUBLES_EQUAL(953392.32838422502, graph.error(values), 1e-7); // initial error + + Values result; + gttic_(SmartStereoProjectionPoseFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); + result = optimizer.optimize(); + gttoc_(SmartStereoProjectionPoseFactor); + tictoc_finishedIteration_(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + EXPECT(assert_equal(pose3, result.at(x3))); +} +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){ + // make a realistic calibration matrix + double fov = 60; // degrees + size_t w=640,h=480; + + Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 cameraPose1 = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses + Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0)); + Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0)); + + SimpleCamera cam1(cameraPose1, *K); // with camera poses + SimpleCamera cam2(cameraPose2, *K); + SimpleCamera cam3(cameraPose3, *K); + + // create arbitrary body_Pose_sensor (transforms from sensor to body) + Pose3 sensor_to_body = Pose3(Rot3::Ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); // + + // These are the poses we want to estimate, from camera measurements + Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse()); + Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse()); + Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse()); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(5, 0, 3.0); + + vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // Create smart factors + std::vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // convert measurement to (degenerate) stereoPoint2 (with right pixel being NaN) + vector measurements_cam1_stereo, measurements_cam2_stereo, measurements_cam3_stereo; + for(size_t k=0; k(x1, bodyPose1, noisePrior)); + graph.push_back(PriorFactor(x2, bodyPose2, noisePrior)); + + // Check errors at ground truth poses + Values gtValues; + gtValues.insert(x1, bodyPose1); + gtValues.insert(x2, bodyPose2); + gtValues.insert(x3, bodyPose3); + double actualError = graph.error(gtValues); + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-7) + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); + Values values; + values.insert(x1, bodyPose1); + values.insert(x2, bodyPose2); + // initialize third pose with some noise, we expect it to move back to original pose3 + values.insert(x3, bodyPose3*noise_pose); + + LevenbergMarquardtParams lmParams; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(bodyPose3,result.at(x3))); +} /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { @@ -390,6 +656,78 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(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), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3 * noise_pose); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3, result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) { + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + Pose3 pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); + StereoCamera cam1(pose1, K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera cam2(pose2, K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); + StereoCamera cam3(pose3, K); + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(3, 0, 3.0); + + // 1. Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + + // DELETE SOME MEASUREMENTS + StereoPoint2 sp = measurements_cam1[1]; + measurements_cam1[1] = StereoPoint2(sp.uL(), missing_uR, sp.v()); + sp = measurements_cam2[2]; + measurements_cam2[2] = StereoPoint2(sp.uL(), missing_uR, sp.v()); + + SmartStereoProjectionParams params; + params.setLinearizationMode(JACOBIAN_SVD); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); @@ -408,7 +746,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3, result.at(x3))); + EXPECT(assert_equal(pose3, result.at(x3),1e-7)); } /* *************************************************************************/ @@ -463,8 +801,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(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 +883,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); + graph.emplace_shared >(x1, pose1, noisePrior); + graph.emplace_shared >(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 @@ -562,7 +900,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9); // dynamic outlier rejection is off - EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9); + EXPECT_DOUBLES_EQUAL(6147.3947317473921, smartFactor4b->error(values), 1e-9); // Factors 1-3 should have valid point, factor 4 should not EXPECT(smartFactor1->point()); @@ -1039,7 +1377,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { } /* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { +TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) { vector views; views.push_back(x1); @@ -1072,6 +1410,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactor = smartFactor->linearize( values); + // check that it is non degenerate + EXPECT(smartFactor->isValid()); + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; @@ -1082,6 +1423,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactorRot = smartFactor->linearize( rotValues); + // check that it is non degenerate + EXPECT(smartFactor->isValid()); + // Hessian is invariant to rotations in the nondegenerate case EXPECT( assert_equal(hessianFactor->information(), @@ -1098,10 +1442,14 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); - // Hessian is invariant to rotations and translations in the nondegenerate case + // Hessian is invariant to rotations and translations in the degenerate case EXPECT( assert_equal(hessianFactor->information(), +#ifdef GTSAM_USE_EIGEN_MKL + hessianFactorRotTran->information(), 1e-5)); +#else hessianFactorRotTran->information(), 1e-6)); +#endif } /* ************************************************************************* */ diff --git a/tests/testGeneralSFMFactorB.cpp b/tests/testGeneralSFMFactorB.cpp index 410617cbf..95caaaf9c 100644 --- a/tests/testGeneralSFMFactorB.cpp +++ b/tests/testGeneralSFMFactorB.cpp @@ -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(m.second, unit2, m.first, P(j)); } Values initial = initialCamerasAndPointsEstimate(db); diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index c1e695b3a..be1104b1a 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -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(z, gNoiseModel, C(i), P(j)); } }