diff --git a/.cproject b/.cproject index 65ba17ce7..84b258330 100644 --- a/.cproject +++ b/.cproject @@ -687,6 +687,14 @@ true true + + make + -j5 + testDataset.run + true + true + true + make -j2 diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index bd0932516..94dd70700 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -26,6 +26,8 @@ using namespace std; using namespace gtsam; +using symbol_shorthand::C; +using symbol_shorthand::P; // We will be using a projection factor that ties a SFM_Camera to a 3D point. // An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration @@ -35,53 +37,42 @@ typedef GeneralSFMFactor MyFactor; /* ************************************************************************* */ int main (int argc, char* argv[]) { - // default file + // Find default file, but if an argument is given, try loading a file string filename = findExampleDataFile("dubrovnik-3-7-pre"); - - // If an argument is given, try loading a file if (argc>1) filename = string(argv[1]); - ///< The structure where we will save the SfM data - SfM_data mydata; - assert(readBAL(filename, mydata)); + // Load the SfM data from file + SfM_data mydata; assert(readBAL(filename, mydata)); + cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); // Create a factor graph NonlinearFactorGraph graph; - // Define the camera observation noise model + // We share *one* noiseModel between all projection factors noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v // Add measurements to the factor graph size_t j = 0; BOOST_FOREACH(const SfM_Track& track, mydata.tracks) { - BOOST_FOREACH(const SfM_Measurement& measurement, track.measurements) { - size_t i; Point2 uv; - boost::tie(i, uv) = measurement; - graph.push_back(MyFactor(uv, noise, Symbol('x', i), Symbol('p', j))); + BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { + size_t i = m.first; + Point2 uv = m.second; + graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P } j += 1; } // Add a prior on pose x1. This indirectly specifies where the origin is. - graph.push_back( - PriorFactor(Symbol('x', 0), mydata.cameras[0], - noiseModel::Isotropic::Sigma(9, 0.1))); + // and a prior on the position of the first landmark to fix the scale + graph.push_back(PriorFactor(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1))); + graph.push_back(PriorFactor (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1))); - // Add a prior on the position of the first landmark to fix the scale - graph.push_back( - PriorFactor(Symbol('p', 0), mydata.tracks[0].p, - noiseModel::Isotropic::Sigma(3, 0.1))); - - // Create the data structure to hold the initial estimate to the solution - // Intentionally initialize the variables off from the ground truth + // Create initial estimate Values initial; - size_t i = 0; - BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) - initial.insert(Symbol('x', i++), camera); - j = 0; - BOOST_FOREACH(const SfM_Track& track, mydata.tracks) - initial.insert(Symbol('p', j++), track.p); + size_t i = 0; j = 0; + BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera); + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p); /* Optimize the graph and print results */ Values result;