diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index a96edd270..904919d42 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -86,9 +86,8 @@ int main(int argc, char* argv[]) { // Create a factor graph NonlinearFactorGraph graph; - // 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 + // 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) { @@ -110,10 +109,17 @@ int main(int argc, char* argv[]) { smartfactor->add(measurement, Symbol('x', j), 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 + // 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. @@ -133,6 +139,29 @@ int main(int argc, char* argv[]) { result.print("Final results:\n"); + // Notice: Smart factor represent the 3D point as a factor, not 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. + Values landmark_result; + for (size_t i = 0; i < points.size(); ++i) { + + // 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. + 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); + } + + landmark_result.print("Landmark results:\n"); + + return 0; } /* ************************************************************************* */