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

Merge branch 'develop' into feature/cython_wrapper

release/4.3a0
Duy-Nguyen Ta 2017-03-17 11:03:08 -04:00
parent 16a1643d17 fbb9d3bdda
commit ca165daaa8
56 changed files with 975 additions and 660 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
examples/CameraResectioning.cpp
View File

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

10
examples/LocalizationExample.cpp
View File

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

6
examples/METISOrderingExample.cpp
View File

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

6
examples/OdometryExample.cpp
View File

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

12
examples/PlanarSLAMExample.cpp
View File

@ -81,13 +81,13 @@ int main(int argc, char** argv) {
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.add(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph graph.emplace_shared<PriorFactor<Pose2> >(x1, prior, priorNoise); // add directly to graph
// Add two odometry factors // Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise)); graph.emplace_shared<BetweenFactor<Pose2> >(x1, x2, odometry, odometryNoise);
graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise)); graph.emplace_shared<BetweenFactor<Pose2> >(x2, x3, odometry, odometryNoise);
// Add Range-Bearing measurements to two different landmarks // Add Range-Bearing measurements to two different landmarks
// create a noise model for the landmark measurements // create a noise model for the landmark measurements
@ -101,9 +101,9 @@ int main(int argc, char** argv) {
range32 = 2.0; range32 = 2.0;
// Add Bearing-Range factors // Add Bearing-Range factors
graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise)); graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x1, l1, bearing11, range11, measurementNoise);
graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise)); graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x2, l1, bearing21, range21, measurementNoise);
graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise)); graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x3, l2, bearing32, range32, measurementNoise);
// Print // Print
graph.print("Factor Graph:\n"); graph.print("Factor Graph:\n");

12
examples/Pose2SLAMExample.cpp
View File

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

12
examples/Pose2SLAMExample_graphviz.cpp
View File

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

12
examples/Pose2SLAMwSPCG.cpp
View File

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

6
examples/SFMExample.cpp
View File

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

4
examples/SFMExample_SmartFactor.cpp
View File

@ -82,13 +82,13 @@ int main(int argc, char* argv[]) {
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Pose3>(0, poses[0], noise)); graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
// Because the structure-from-motion problem has a scale ambiguity, the problem is // 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 // 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. // 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. // Because these two are fixed, the rest of the poses will be also be fixed.
graph.push_back(PriorFactor<Pose3>(1, poses[1], noise)); // add directly to graph graph.emplace_shared<PriorFactor<Pose3> >(1, poses[1], noise); // add directly to graph
graph.print("Factor Graph:\n"); graph.print("Factor Graph:\n");

4
examples/SFMExample_SmartFactorPCG.cpp
View File

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

6
examples/SFMExample_bal.cpp
View File

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

6
examples/SFMExample_bal_COLAMD_METIS.cpp
View File

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

8
examples/SelfCalibrationExample.cpp
View File

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

14
examples/StereoVOExample.cpp
View File

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

8
examples/StereoVOExample_large.cpp
View File

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

6
examples/VisualISAM2Example.cpp
View File

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

9
examples/VisualISAMExample.cpp
View File

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

6
gtsam/geometry/CameraSet.h
View File

@ -56,7 +56,11 @@ protected:
// Project and fill error vector // Project and fill error vector
Vector b(ZDim * m); Vector b(ZDim * m);
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
b.segment<ZDim>(row) = traits<Z>::Local(measured[i], predicted[i]); Vector bi = traits<Z>::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<ZDim>(row) = bi;
} }
return b; return b;
} }

5
gtsam/geometry/StereoCamera.cpp
View File

@ -39,7 +39,10 @@ namespace gtsam {
const Point3 q = leftCamPose_.transform_to(point); 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 // get calibration
const Cal3_S2Stereo& K = *K_; const Cal3_S2Stereo& K = *K_;

17
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -104,6 +104,23 @@ TEST( StereoCamera, Dproject)
CHECK(assert_equal(expected2,actual2,1e-7)); 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) TEST( StereoCamera, backproject_case1)
{ {

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

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

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

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

8
gtsam/geometry/triangulation.h
View File

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

38
gtsam/inference/FactorGraph.h
View File

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

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

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

7
gtsam/slam/InitializePose3.cpp
View File

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

10
gtsam/slam/SmartFactorBase.h
View File

@ -202,7 +202,7 @@ public:
boost::optional<typename Cameras::FBlocks&> Fs = boost::none, // boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const { boost::optional<Matrix&> E = boost::none) const {
Vector ue = cameras.reprojectionError(point, measured_, Fs, E); 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++){ for(size_t i=0; i < Fs->size(); i++){
Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse()); Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse());
Matrix J(6, 6); Matrix J(6, 6);
@ -210,9 +210,17 @@ public:
Fs->at(i) = Fs->at(i) * J; Fs->at(i) = Fs->at(i) * J;
} }
} }
correctForMissingMeasurements(cameras, ue, Fs, E);
return ue; 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<typename Cameras::FBlocks&> Fs = boost::none,
boost::optional<Matrix&> E = boost::none) const {}
/** /**
* Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z] * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z]
* Noise model applied * Noise model applied

134
gtsam/slam/SmartFactorParams.h Normal file
View File

@ -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 <gtsam/geometry/triangulation.h>
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<class ARCHIVE>
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

104
gtsam/slam/SmartProjectionFactor.h
View File

@ -20,6 +20,7 @@
#pragma once #pragma once
#include <gtsam/slam/SmartFactorBase.h> #include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/slam/SmartFactorParams.h>
#include <gtsam/geometry/triangulation.h> #include <gtsam/geometry/triangulation.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
@ -31,109 +32,6 @@
namespace gtsam { 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<class ARCHIVE>
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. * SmartProjectionFactor: triangulates point and keeps an estimate of it around.
* This factor operates with monocular cameras, where a camera is expected to * This factor operates with monocular cameras, where a camera is expected to

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@ -262,8 +262,8 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, bodyPose1, noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x1, bodyPose1, noisePrior);
graph.push_back(PriorFactor<Pose3>(x2, bodyPose2, noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x2, bodyPose2, noisePrior);
// Check errors at ground truth poses // Check errors at ground truth poses
Values gtValues; Values gtValues;
@ -319,8 +319,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
Values groundTruth; Values groundTruth;
groundTruth.insert(x1, cam1.pose()); groundTruth.insert(x1, cam1.pose());
@ -547,8 +547,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/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), 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(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/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), 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(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/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), 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(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(smartFactor4); graph.push_back(smartFactor4);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
Values values; Values values;
values.insert(x1, cam1.pose()); values.insert(x1, cam1.pose());
@ -800,8 +800,8 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
@ -830,30 +830,21 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
// Project three landmarks into three cameras // Project three landmarks into three cameras
graph.push_back( graph.emplace_shared<ProjectionFactor>(cam1.project(landmark1), model, x1, L(1), sharedK2);
ProjectionFactor(cam1.project(landmark1), model, x1, L(1), sharedK2)); graph.emplace_shared<ProjectionFactor>(cam2.project(landmark1), model, x2, L(1), sharedK2);
graph.push_back( graph.emplace_shared<ProjectionFactor>(cam3.project(landmark1), model, x3, L(1), sharedK2);
ProjectionFactor(cam2.project(landmark1), model, x2, L(1), sharedK2));
graph.push_back(
ProjectionFactor(cam3.project(landmark1), model, x3, L(1), sharedK2));
graph.push_back( graph.emplace_shared<ProjectionFactor>(cam1.project(landmark2), model, x1, L(2), sharedK2);
ProjectionFactor(cam1.project(landmark2), model, x1, L(2), sharedK2)); graph.emplace_shared<ProjectionFactor>(cam2.project(landmark2), model, x2, L(2), sharedK2);
graph.push_back( graph.emplace_shared<ProjectionFactor>(cam3.project(landmark2), model, x3, L(2), sharedK2);
ProjectionFactor(cam2.project(landmark2), model, x2, L(2), sharedK2));
graph.push_back(
ProjectionFactor(cam3.project(landmark2), model, x3, L(2), sharedK2));
graph.push_back( graph.emplace_shared<ProjectionFactor>(cam1.project(landmark3), model, x1, L(3), sharedK2);
ProjectionFactor(cam1.project(landmark3), model, x1, L(3), sharedK2)); graph.emplace_shared<ProjectionFactor>(cam2.project(landmark3), model, x2, L(3), sharedK2);
graph.push_back( graph.emplace_shared<ProjectionFactor>(cam3.project(landmark3), model, x3, L(3), sharedK2);
ProjectionFactor(cam2.project(landmark3), model, x2, L(3), sharedK2));
graph.push_back(
ProjectionFactor(cam3.project(landmark3), model, x3, L(3), sharedK2));
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x1, level_pose, noisePrior);
graph.push_back(PriorFactor<Pose3>(x2, pose_right, noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x2, pose_right, noisePrior);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3)); Point3(0.5, 0.1, 0.3));
@ -1000,11 +991,9 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.push_back( graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation)); graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
graph.push_back(
PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise 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 ) { TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) {
// this test considers a condition in which the cheirality constraint is triggered
using namespace vanillaPose; using namespace vanillaPose;
vector<Key> views; vector<Key> views;
@ -1068,11 +1058,9 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.push_back( graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation)); graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
graph.push_back(
PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/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), 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) // 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 // 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<Pose3>(x3).rotation(), EXPECT(assert_equal(Pose3(values.at<Pose3>(x3).rotation(),
Point3(0,0,1)), result.at<Pose3>(x3))); Point3(0,0,1)), result.at<Pose3>(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<Pose3>(x3),1e-3));
#endif
} }
/* *************************************************************************/ /* *************************************************************************/
@ -1286,8 +1280,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/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), 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(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.push_back( graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation)); graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
graph.push_back(
PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/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), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),

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

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

10
gtsam_unstable/examples/ConcurrentCalibration.cpp
View File

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

2
gtsam_unstable/examples/SmartProjectionFactorExample.cpp
View File

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

2
gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
View File

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

5
gtsam_unstable/linear/LPSolver.h
View File

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

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

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

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

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

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

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

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

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

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

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

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

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

208
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -21,6 +21,7 @@
#pragma once #pragma once
#include <gtsam/slam/SmartFactorBase.h> #include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/slam/SmartFactorParams.h>
#include <gtsam/geometry/triangulation.h> #include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
@ -35,91 +36,10 @@
namespace gtsam { namespace gtsam {
/// Linearization mode: what factor to linearize to /*
enum LinearizationMode { * Parameters for the smart stereo projection factors (identical to the SmartProjectionParams)
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 { typedef SmartProjectionParams 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;
}
};
/** /**
* SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around. * SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.
@ -155,14 +75,19 @@ public:
/// Vector of cameras /// Vector of cameras
typedef CameraSet<StereoCamera> Cameras; typedef CameraSet<StereoCamera> Cameras;
/// Vector of monocular cameras (stereo treated as 2 monocular)
typedef PinholeCamera<Cal3_S2> MonoCamera;
typedef CameraSet<MonoCamera> MonoCameras;
typedef std::vector<Point2> MonoMeasurements;
/** /**
* Constructor * Constructor
* @param params internal parameters of the smart factors * @param params internal parameters of the smart factors
*/ */
SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel, SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params = const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
SmartStereoProjectionParams()) : const boost::optional<Pose3> body_P_sensor = boost::none) :
Base(sharedNoiseModel), // Base(sharedNoiseModel, body_P_sensor), //
params_(params), // params_(params), //
result_(TriangulationResult::Degenerate()) { result_(TriangulationResult::Degenerate()) {
} }
@ -240,77 +165,30 @@ public:
size_t m = cameras.size(); size_t m = cameras.size();
bool retriangulate = decideIfTriangulate(cameras); bool retriangulate = decideIfTriangulate(cameras);
// if(!retriangulate) // triangulate stereo measurements by treating each stereocamera as a pair of monocular cameras
// std::cout << "retriangulate = false" << std::endl; MonoCameras monoCameras;
// MonoMeasurements monoMeasured;
// bool retriangulate = true;
if (retriangulate) {
// std::cout << "Retriangulate " << std::endl;
std::vector<Point3> reprojections;
reprojections.reserve(m);
for(size_t i = 0; i < m; i++) { for(size_t i = 0; i < m; i++) {
reprojections.push_back(cameras[i].backproject(measured_[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 if (retriangulate)
Point3 pw_avg = pw_sum / double(m); result_ = gtsam::triangulateSafe(monoCameras, monoMeasured,
params_.triangulation);
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_; 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
return result_;
}
/// triangulate /// triangulate
bool triangulateForLinearize(const Cameras& cameras) const { bool triangulateForLinearize(const Cameras& cameras) const {
triangulateSafe(cameras); // imperative, might reset result_ triangulateSafe(cameras); // imperative, might reset result_
@ -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<typename Cameras::FBlocks&> Fs = boost::none,
boost::optional<Matrix&> 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; ii<Dim; ii++)
Fi(1,ii) = 0.0;
}
if(E) // delete influence of right point on jacobian E
E->row(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 */ /** return the landmark */
TriangulationResult point() const { TriangulationResult point() const {
return result_; return result_;

14
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -66,9 +66,9 @@ public:
* @param params internal parameters of the smart factors * @param params internal parameters of the smart factors
*/ */
SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params = const SmartStereoProjectionParams& params = SmartStereoProjectionParams(),
SmartStereoProjectionParams()) : const boost::optional<Pose3> body_P_sensor = boost::none) :
Base(sharedNoiseModel, params) { Base(sharedNoiseModel, params, body_P_sensor) {
} }
/** Virtual destructor */ /** 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 * 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 poseKeys vector of keys corresponding to the camera observing the same landmark
* @param K the (known) camera calibration (same for all measurements) * @param K the (known) camera calibration (same for all measurements)
*/ */
@ -161,7 +161,11 @@ public:
Base::Cameras cameras; Base::Cameras cameras;
size_t i=0; size_t i=0;
for(const Key& k: this->keys_) { for(const Key& k: this->keys_) {
const Pose3& pose = values.at<Pose3>(k); Pose3 pose = values.at<Pose3>(k);
if (Base::body_P_sensor_)
pose = pose.compose(*(Base::body_P_sensor_));
StereoCamera camera(pose, K_all_[i++]); StereoCamera camera(pose, K_all_[i++]);
cameras.push_back(camera); cameras.push_back(camera);
} }

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

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

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

@ -18,7 +18,7 @@
* @date Sept 2013 * @date Sept 2013
*/ */
// TODO #include <gtsam/slam/tests/smartFactorScenarios.h> #include <gtsam/slam/tests/smartFactorScenarios.h>
#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h> #include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PoseTranslationPrior.h> #include <gtsam/slam/PoseTranslationPrior.h>
@ -33,8 +33,6 @@ using namespace boost::assign;
using namespace gtsam; using namespace gtsam;
// make a realistic calibration matrix // make a realistic calibration matrix
static double fov = 60; // degrees
static size_t w = 640, h = 480;
static double b = 1; static double b = 1;
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); 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), static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0)); Point3(0.25, -0.10, 1.0));
static double missing_uR = std::numeric_limits<double>::quiet_NaN();
vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1, vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
@ -79,6 +79,35 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
LevenbergMarquardtParams lm_params; 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) { TEST( SmartStereoProjectionPoseFactor, Constructor) {
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model)); SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
@ -151,6 +180,60 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
//EXPECT(assert_equal(zero(4),actual,1e-8)); //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<StereoCamera> 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 ) { TEST( SmartStereoProjectionPoseFactor, noisy ) {
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/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), 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<Pose3>(x3))); Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
// cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl; // 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 // get triangulated landmarks from smart factors
Point3 landmark1_smart = *smartFactor1->point(); 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)); 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; // 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); LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params);
Values result2 = optimizer2.optimize(); 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; // 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<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark2);
vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
vector<Key> 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<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
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<Pose3>(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<Pose3>(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<Point2> 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<Key> views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
// convert measurement to (degenerate) stereoPoint2 (with right pixel being NaN)
vector<StereoPoint2> measurements_cam1_stereo, measurements_cam2_stereo, measurements_cam3_stereo;
for(size_t k=0; k<measurements_cam1.size();k++)
measurements_cam1_stereo.push_back(StereoPoint2(measurements_cam1[k].x() , missing_uR , measurements_cam1[k].y()));
for(size_t k=0; k<measurements_cam2.size();k++)
measurements_cam2_stereo.push_back(StereoPoint2(measurements_cam2[k].x() , missing_uR , measurements_cam2[k].y()));
for(size_t k=0; k<measurements_cam3.size();k++)
measurements_cam3_stereo.push_back(StereoPoint2(measurements_cam3[k].x() , missing_uR , measurements_cam3[k].y()));
SmartStereoProjectionParams params;
params.setRankTolerance(1.0);
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
params.setEnableEPI(false);
Cal3_S2Stereo::shared_ptr Kmono(new Cal3_S2Stereo(fov,w,h,b));
SmartStereoProjectionPoseFactor smartFactor1(model, params, sensor_to_body);
smartFactor1.add(measurements_cam1_stereo, views, Kmono);
SmartStereoProjectionPoseFactor smartFactor2(model, params, sensor_to_body);
smartFactor2.add(measurements_cam2_stereo, views, Kmono);
SmartStereoProjectionPoseFactor smartFactor3(model, params, sensor_to_body);
smartFactor3.add(measurements_cam3_stereo, views, Kmono);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
// Put all factors in factor graph, adding priors
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, bodyPose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, bodyPose2, noisePrior));
// 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<Pose3>(x3)));
}
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
@ -390,6 +656,78 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); 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<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
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<Pose3>(x3)));
}
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) {
vector<Key> 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<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark2);
vector<StereoPoint2> 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; NonlinearFactorGraph graph;
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
@ -408,7 +746,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
Values result; Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize(); result = optimizer.optimize();
EXPECT(assert_equal(pose3, result.at<Pose3>(x3))); EXPECT(assert_equal(pose3, result.at<Pose3>(x3),1e-7));
} }
/* *************************************************************************/ /* *************************************************************************/
@ -463,8 +801,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/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), 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(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(smartFactor4); graph.push_back(smartFactor4);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior)); graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise 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); EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9);
// dynamic outlier rejection is off // 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 // Factors 1-3 should have valid point, factor 4 should not
EXPECT(smartFactor1->point()); EXPECT(smartFactor1->point());
@ -1039,7 +1377,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
} }
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) { TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) {
vector<Key> views; vector<Key> views;
views.push_back(x1); views.push_back(x1);
@ -1072,6 +1410,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize( boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(
values); 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)); Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Values rotValues; Values rotValues;
@ -1082,6 +1423,9 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize( boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(
rotValues); rotValues);
// check that it is non degenerate
EXPECT(smartFactor->isValid());
// Hessian is invariant to rotations in the nondegenerate case // Hessian is invariant to rotations in the nondegenerate case
EXPECT( EXPECT(
assert_equal(hessianFactor->information(), assert_equal(hessianFactor->information(),
@ -1098,10 +1442,14 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactorRotTran = boost::shared_ptr<GaussianFactor> hessianFactorRotTran =
smartFactor->linearize(tranValues); 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( EXPECT(
assert_equal(hessianFactor->information(), assert_equal(hessianFactor->information(),
#ifdef GTSAM_USE_EIGEN_MKL
hessianFactorRotTran->information(), 1e-5));
#else
hessianFactorRotTran->information(), 1e-6)); hessianFactorRotTran->information(), 1e-6));
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
tests/testGeneralSFMFactorB.cpp
View File

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

2
timing/timeSFMBAL.cpp
View File

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