diff --git a/.cproject b/.cproject index 1783efa98..2f42abd79 100644 --- a/.cproject +++ b/.cproject @@ -542,6 +542,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -568,6 +576,7 @@ make + tests/testBayesTree.run true false @@ -575,6 +584,7 @@ make + testBinaryBayesNet.run true false @@ -622,6 +632,7 @@ make + testSymbolicBayesNet.run true false @@ -629,6 +640,7 @@ make + tests/testSymbolicFactor.run true false @@ -636,6 +648,7 @@ make + testSymbolicFactorGraph.run true false @@ -651,19 +664,12 @@ make + tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -896,22 +902,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -928,6 +918,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -1458,6 +1464,7 @@ make + testGraph.run true false @@ -1465,6 +1472,7 @@ make + testJunctionTree.run true false @@ -1472,6 +1480,7 @@ make + testSymbolicBayesNetB.run true false @@ -1639,6 +1648,7 @@ make + testErrors.run true false @@ -1684,22 +1694,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j5 - testParticleFactor.run - true - true - true - make -j2 @@ -1780,6 +1774,22 @@ true true + + make + -j5 + testParticleFactor.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1996,22 +2006,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j2 @@ -2094,7 +2088,6 @@ make - testSimulated2DOriented.run true false @@ -2134,7 +2127,6 @@ make - testSimulated2D.run true false @@ -2142,7 +2134,6 @@ make - testSimulated3D.run true false @@ -2156,6 +2147,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j5 @@ -2364,6 +2371,14 @@ true true + + make + -j5 + SFMExample_SmartFactor.run + true + true + true + make -j5 @@ -2430,7 +2445,6 @@ make - tests/testGaussianISAM2 true false @@ -2452,102 +2466,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2749,6 +2667,7 @@ cpack + -G DEB true false @@ -2756,6 +2675,7 @@ cpack + -G RPM true false @@ -2763,6 +2683,7 @@ cpack + -G TGZ true false @@ -2770,6 +2691,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2943,26 +2865,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - check.wrap + -j2 + testRot2.run true true true - + make - -j5 - wrap + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -3006,6 +3000,30 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index 904919d42..400ddd728 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -14,6 +14,7 @@ * @brief A structure-from-motion problem on a simulated dataset, using smart projection factor * @author Duy-Nguyen Ta * @author Jing Dong + * @author Frank Dellaert */ /** @@ -28,11 +29,6 @@ // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). #include -// Each variable in the system (poses and landmarks) must be identified with a unique key. -// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -// Here we will use Symbols -#include - // In GTSAM, measurement functions are represented as 'factors'. // The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark, // The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration, @@ -65,8 +61,8 @@ using namespace std; using namespace gtsam; // Make the typename short so it looks much cleaner -typedef gtsam::SmartProjectionPoseFactor - SmartFactor; +typedef gtsam::SmartProjectionPoseFactor SmartFactor; /* ************************************************************************* */ int main(int argc, char* argv[]) { @@ -75,93 +71,87 @@ int main(int argc, char* argv[]) { Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); // Define the camera observation noise model - noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + noiseModel::Isotropic::shared_ptr measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v - // Create the set of ground-truth landmarks + // Create the set of ground-truth landmarks and poses vector points = createPoints(); - - // Create the set of ground-truth poses vector poses = createPoses(); // Create a factor graph NonlinearFactorGraph graph; - // A vector saved all Smart factors (for get landmark position after optimization) - vector smartfactors_ptr; - // Simulated measurements from each camera pose, adding them to the factor graph - for (size_t i = 0; i < points.size(); ++i) { + for (size_t j = 0; j < points.size(); ++j) { // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. SmartFactor::shared_ptr smartfactor(new SmartFactor()); - for (size_t j = 0; j < poses.size(); ++j) { + for (size_t i = 0; i < poses.size(); ++i) { // generate the 2D measurement - SimpleCamera camera(poses[j], *K); - Point2 measurement = camera.project(points[i]); + SimpleCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); - // call add() function to add measurment into a single factor, here we need to add: + // call add() function to add measurement into a single factor, here we need to add: // 1. the 2D measurement // 2. the corresponding camera's key // 3. camera noise model // 4. camera calibration - smartfactor->add(measurement, Symbol('x', j), measurementNoise, K); + smartfactor->add(measurement, i, measurementNoise, K); } - // save smartfactors to get landmark position - smartfactors_ptr.push_back(smartfactor); - // insert the smart factor in the graph graph.push_back(smartfactor); } // Add a prior on pose x0. 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))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + // 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))); + graph.push_back(PriorFactor(0, poses[0], poseNoise)); - // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained - // Here we add a prior on the second pose x1, so this will fix the scale by indicating the distance between x0 and x1. - // Because these two are fixed, the rest poses will be alse fixed. - graph.push_back(PriorFactor(Symbol('x', 1), poses[1], poseNoise)); // add directly to graph + // Because the structure-from-motion problem has a scale ambiguity, the problem is + // still under-constrained. Here we add a prior on the second pose x1, so this will + // fix the scale by indicating the distance between x0 and x1. + // Because these two are fixed, the rest of the poses will be also be fixed. + graph.push_back(PriorFactor(1, poses[1], poseNoise)); // add directly to graph graph.print("Factor Graph:\n"); - // Create the data structure to hold the initial estimate to the solution + // Create the initial estimate to the solution // Intentionally initialize the variables off from the ground truth Values initialEstimate; + Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + initialEstimate.insert(i, poses[i].compose(delta)); initialEstimate.print("Initial Estimates:\n"); // Optimize the graph and print results Values result = DoglegOptimizer(graph, initialEstimate).optimize(); result.print("Final results:\n"); - - // Notice: Smart factor represent the 3D point as a factor, not a variable. + // A smart factor represent the 3D point inside the factor, not as a variable. // The 3D position of the landmark is not explicitly calculated by the optimizer. - // If you do want to output the landmark's 3D position, you should use the internal function point() - // of the smart factor to get the 3D point. + // To obtain the landmark's 3D position, we use the "point" method of the smart factor. Values landmark_result; - for (size_t i = 0; i < points.size(); ++i) { + for (size_t j = 0; j < points.size(); ++j) { - // The output of point() is in boost::optional, since sometimes - // the triangulation opterations inside smart factor will encounter degeneracy. - // Check the manual of boost::optional for more details for the usages. + // The output of point() is in boost::optional, as sometimes + // the triangulation operation inside smart factor will encounter degeneracy. boost::optional point; - // here we use the saved smart factors to call, pass in all optimized pose to calculate landmark positions - point = smartfactors_ptr.at(i)->point(result); - - // ignore if boost::optional return NULL - if (point) - landmark_result.insert(Symbol('l', i), *point); + // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first + SmartFactor::shared_ptr smart = dynamic_pointer_cast(graph[j]); + if (smart) { + point = smart->point(result); + if (point) // ignore if boost::optional return NULL + landmark_result.insert(j, *point); + } } landmark_result.print("Landmark results:\n"); - return 0; } /* ************************************************************************* */