diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index a9817a8ea..24bca54cb 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -12,7 +12,7 @@ /** * @file dataset.cpp * @date Jan 22, 2010 - * @author nikai + * @author nikai, Luca Carlone * @brief utility functions for loading datasets */ @@ -605,7 +605,7 @@ bool readBAL(const string& filename, SfM_data &data) /* ************************************************************************* */ bool writeBAL(const string& filename, SfM_data &data) { - // Load the data file + // Open the output file ofstream os; os.open(filename.c_str()); os.precision(20); @@ -614,6 +614,8 @@ bool writeBAL(const string& filename, SfM_data &data) return false; } + cout << "writeBAL assumes the origin of the camera frame to coincide with camera center!!" << endl; + // Write the number of camera poses and 3D points int nrObservations=0; for (size_t j = 0; j < data.number_tracks(); j++){ @@ -660,5 +662,46 @@ bool writeBAL(const string& filename, SfM_data &data) return true; } +bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){ + + // CHECKS + Values valuesPoses = values.filter(); + if( valuesPoses.size() != data.number_cameras()){ + cout << "writeBALfromValues: different number of cameras in SfM_data (#cameras= " << data.number_cameras() + <<") and values (#cameras " << valuesPoses.size() << ")!!" << endl; + return false; + } + Values valuesPoints = values.filter(); + if( valuesPoints.size() != data.number_tracks()){ + cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks() + <<") and values (#points " << valuesPoints.size() << ")!!" << endl; + return false; + } + if(valuesPoints.size() + valuesPoses.size() != values.size()){ + cout << "writeBALfromValues write only poses and points values!!" << endl; + return false; + } + if(valuesPoints.size()==0 || valuesPoses.size()==0){ + cout << "writeBALfromValues: No point or pose in values!!" << endl; + return false; + } + + for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera + Key poseKey = symbol('x',i); + Pose3 pose = values.at(poseKey); + Cal3Bundler K = data.cameras[i].calibration(); + PinholeCamera camera(pose, K); + data.cameras[i] = camera; + } + + for (size_t j = 0; j < data.number_tracks(); j++){ // for each point + Key pointKey = symbol('l',j); + Point3 point = values.at(pointKey); + data.tracks[j].p = point; + } + + return writeBAL(filename, data); +} + } // \namespace gtsam diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 252450c5d..1762f4a21 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -12,7 +12,7 @@ /** * @file dataset.h * @date Jan 22, 2010 - * @author nikai + * @author nikai, Luca Carlone * @brief utility functions for loading datasets */ @@ -135,6 +135,17 @@ bool readBAL(const std::string& filename, SfM_data &data); */ bool writeBAL(const std::string& filename, SfM_data &data); +/** + * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a + * SfM_data structure and a value structure (measurements are the same as the SfM input data, + * while camera poses and values are read from Values) + * @param filename The name of the BAL file to write + * @param data SfM structure where the data is stored + * @param values structure where the graph values are stored + * @return true if the parsing was successful, false otherwise + */ +bool writeBALfromValues(const std::string& filename, SfM_data &data, Values& values); + /** * @brief This function converts an openGL camera pose to an GTSAM camera pose * @param R rotation in openGL diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 79311fa92..f9a2cb34f 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -12,7 +12,7 @@ /** * @file testDataset.cpp * @brief Unit test for dataset.cpp - * @author Richard Roberts + * @author Richard Roberts, Luca Carlone */ #include @@ -20,6 +20,7 @@ #include #include +#include #include using namespace std; @@ -116,7 +117,7 @@ TEST( dataSet, writeBAL_Dubrovnik) ///< Read a file using the unit tested readBAL const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); SfM_data readData; - CHECK(readBAL(filenameToRead, readData)); + readBAL(filenameToRead, readData); // Write readData to file filenameToWrite const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten"); @@ -157,6 +158,62 @@ TEST( dataSet, writeBAL_Dubrovnik) } } + +/* ************************************************************************* */ +TEST( dataSet, writeBALfromValues_Dubrovnik){ + + ///< Read a file using the unit tested readBAL + const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); + SfM_data readData; + readBAL(filenameToRead, readData); + + Pose3 poseChange = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3)); + + Values value; + for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera + Key poseKey = symbol('x',i); + Pose3 pose = poseChange.compose(readData.cameras[i].pose()); + value.insert(poseKey, pose); + } + for(size_t j=0; j < readData.number_tracks(); j++){ // for each point + Key pointKey = symbol('l',j); + Point3 point = poseChange.transform_from( readData.tracks[j].p ); + value.insert(pointKey, point); + } + + // Write values and readData to a file + const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten"); + writeBALfromValues(filenameToWrite, readData, value); + + // Read the file we wrote + SfM_data writtenData; + readBAL(filenameToWrite, writtenData); + + // Check that the reprojection errors are the same and the poses are correct + // Check number of things + EXPECT_LONGS_EQUAL(3,writtenData.number_cameras()); + EXPECT_LONGS_EQUAL(7,writtenData.number_tracks()); + const SfM_Track& track0 = writtenData.tracks[0]; + EXPECT_LONGS_EQUAL(3,track0.number_measurements()); + + // Check projection of a given point + EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); + const SfM_Camera& camera0 = writtenData.cameras[0]; + Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; + EXPECT(assert_equal(expected,actual,12)); + + Pose3 expectedPose = camera0.pose(); + Key poseKey = symbol('x',0); + Pose3 actualPose = value.at(poseKey); + EXPECT(assert_equal(expectedPose,actualPose, 1e-7)); + + Point3 expectedPoint = track0.p; + Key pointKey = symbol('l',0); + Point3 actualPoint = value.at(pointKey); + EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6)); +} + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam_unstable/examples/KITTItoBALConverter.cpp b/gtsam_unstable/examples/KITTItoBALConverter.cpp index 78f246893..27c01305a 100644 --- a/gtsam_unstable/examples/KITTItoBALConverter.cpp +++ b/gtsam_unstable/examples/KITTItoBALConverter.cpp @@ -41,7 +41,7 @@ // have been provided with the library for solving robotics SLAM problems. #include #include -#include +#include // We need to use SFM_data to save it to BAL format #include diff --git a/gtsam_unstable/examples/SmartProjectionFactorExampleBAL.cpp b/gtsam_unstable/examples/SmartProjectionFactorExampleBAL.cpp index 9da60bea7..d73a42bcf 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExampleBAL.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExampleBAL.cpp @@ -59,7 +59,6 @@ using namespace gtsam; using namespace boost::assign; namespace NM = gtsam::noiseModel; -#define USE_BUNDLER using symbol_shorthand::X; using symbol_shorthand::L; @@ -67,13 +66,8 @@ using symbol_shorthand::L; typedef PriorFactor Pose3Prior; typedef FastMap OrderingMap; -#ifdef USE_BUNDLER typedef SmartProjectionFactorsCreator SmartFactorsCreator; typedef GenericProjectionFactorsCreator ProjectionFactorsCreator; -#else -typedef SmartProjectionFactorsCreator SmartFactorsCreator; -typedef GenericProjectionFactorsCreator ProjectionFactorsCreator; -#endif bool debug = false; @@ -120,68 +114,58 @@ void optimizeGraphLM(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr grap params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; params.verbosity = NonlinearOptimizerParams::ERROR; params.lambdaInitial = 1; - params.lambdaFactor = 10; + // Other parameters: if needed + // params.lambdaFactor = 10; // Profile a single iteration - // params.maxIterations = 1; - params.maxIterations = 100; - std::cout << " LM max iterations: " << params.maxIterations << std::endl; - // // params.relativeErrorTol = 1e-5; - params.absoluteErrorTol = 1.0; - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - params.verbosity = NonlinearOptimizerParams::ERROR; - params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY; + params.maxIterations = 1; + // params.relativeErrorTol = 1e-5; + // params.absoluteErrorTol = 1.0; + cout << "==================== Optimization ==================" << endl; + cout << "- Number of factors: " << graph.size() << endl; + cout << "- Number of variables: " << graphValues->size() << endl; - cout << "Graph size: " << graph.size() << endl; - cout << "Number of variables: " << graphValues->size() << endl; - std::cout << " OPTIMIZATION " << std::endl; + params.print("PARAMETERS FOR LM: \n"); if (debug) { - std::cout << "\n\n=================================================\n\n"; + cout << "\n\n===============================================\n\n"; graph.print("thegraph"); } - std::cout << "\n\n=================================================\n\n"; + cout << "-----------------------------------------------------" << endl; if (ordering && ordering->size() > 0) { - if (debug) { - std::cout << "Have an ordering\n" << std::endl; - BOOST_FOREACH(const Key& key, *ordering) { - std::cout << key << " "; - } - std::cout << std::endl; - } - + std::cout << "Starting graph optimization with user specified ordering" << std::endl; params.ordering = *ordering; - - //for (int i = 0; i < 3; i++) { LevenbergMarquardtOptimizer optimizer(graph, *graphValues, params); gttic_(GenericProjectionFactorExample_kitti); result = optimizer.optimize(); gttoc_(GenericProjectionFactorExample_kitti); tictoc_finishedIteration_(); - //} - } else { - std::cout << "Using COLAMD ordering\n" << std::endl; - //boost::shared_ptr ordering2(new Ordering()); ordering = ordering2; + cout << "-----------------------------------------------------" << endl; + std::cout << "Number of outer LM iterations: " << optimizer.state().iterations << std::endl; + std::cout << "Total number of LM iterations (inner and outer): " << optimizer.getInnerIterations() << std::endl; + } else { + std::cout << "Starting graph optimization with COLAMD ordering" << std::endl; LevenbergMarquardtOptimizer optimizer(graph, *graphValues, params); params.ordering = Ordering::COLAMD(graph); - gttic_(SmartProjectionFactorExample_kitti); + gttic_(smartProjectionFactorExample); result = optimizer.optimize(); - gttoc_(SmartProjectionFactorExample_kitti); + gttoc_(smartProjectionFactorExample); tictoc_finishedIteration_(); - + cout << "-----------------------------------------------------" << endl; std::cout << "Number of outer LM iterations: " << optimizer.state().iterations << std::endl; std::cout << "Total number of LM iterations (inner and outer): " << optimizer.getInnerIterations() << std::endl; //*ordering = params.ordering; if (params.ordering) { - std::cout << "Graph size: " << graph.size() << " ORdering: " << params.ordering->size() << std::endl; + if(debug) std::cout << "Graph size: " << graph.size() << " Ordering: " << params.ordering->size() << std::endl; ordering = boost::make_shared(*(new Ordering())); *ordering = *params.ordering; } else { std::cout << "WARNING: NULL ordering!" << std::endl; } } + cout << "======================================================" << endl; } void optimizeGraphGN(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr graphValues, Values &result) { @@ -190,31 +174,31 @@ void optimizeGraphGN(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr grap params.verbosity = NonlinearOptimizerParams::DELTA; GaussNewtonOptimizer optimizer(graph, *graphValues, params); - gttic_(SmartProjectionFactorExample_kitti); + gttic_(smartProjectionFactorExample); result = optimizer.optimize(); - gttoc_(SmartProjectionFactorExample_kitti); + gttoc_(smartProjectionFactorExample); tictoc_finishedIteration_(); - } void optimizeGraphISAM2(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr graphValues, Values &result) { ISAM2 isam; - gttic_(SmartProjectionFactorExample_kitti); + gttic_(smartProjectionFactorExample); isam.update(graph, *graphValues); result = isam.calculateEstimate(); - gttoc_(SmartProjectionFactorExample_kitti); + gttoc_(smartProjectionFactorExample); tictoc_finishedIteration_(); } +// ************************************************************************************************ +// ************************************************************************************************ // main int main(int argc, char** argv) { - // Set to true to use SmartProjectionFactor. Otherwise GenericProjectionFactor will be used - bool useSmartProjectionFactor = true; - bool doTriangulation = true; // we read points initial guess from file or we triangulate + bool useSmartProjectionFactor = true; // default choice is to use the smart projection factors + bool doTriangulation = true; // default choice is to initialize points from triangulation (only for standard projection factors) + bool addNoise = false; // add (fixed) noise to the initial guess of camera poses bool useLM = true; - bool addNoise = false; // Smart factors settings double linThreshold = -1.0; // negative is disabled @@ -225,6 +209,7 @@ int main(int argc, char** argv) { string datasetFile = HOME + "/data/SfM/BAL/Ladybug/problem-1031-110968-pre.txt"; // --------------- READ USER INPUTS (main arguments) ------------------------------------ + // COMMAND TO RUN (EXAMPLE): ./SmartProjectionFactorExampleBAL smart triangulation=0 /home/aspn/data/SfM/BAL/Ladybug/problem-1031-110968-pre.txt if(argc>1){ // if we have any input arguments // Arg1: "smart" or "standard" (select if to use smart factors or standard projection factors) // Arg2: "triangulation=0" or "triangulation=1" (select whether to initialize initial guess for points using triangulation) @@ -260,18 +245,19 @@ int main(int argc, char** argv) { std::cout << "- datasetFile: " << datasetFile << std::endl; if (linThreshold >= 0) - std::cout << "PARAM linThreshold (negative is disabled): " << linThreshold << std::endl; + std::cout << "- linThreshold (negative is disabled): " << linThreshold << std::endl; if(addNoise) - std::cout << "PARAM Noise: " << addNoise << std::endl; + std::cout << "- Noise: " << addNoise << std::endl; // --------------- READ INPUT DATA ---------------------------------------- + std::cout << "- reading dataset from file... " << std::endl; SfM_data inputData; readBAL(datasetFile, inputData); - std::cout << "read data from file... " << std::endl; // --------------- CREATE FACTOR GRAPH ------------------------------------ - static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(2)); + std::cout << "- creating factor graph... " << std::endl; + static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(2)); // pixel noise boost::shared_ptr ordering(new Ordering()); NonlinearFactorGraph graphSmart; @@ -280,13 +266,8 @@ int main(int argc, char** argv) { NonlinearFactorGraph graphProjection; gtsam::Values::shared_ptr graphProjectionValues(new gtsam::Values()); -#ifdef USE_BUNDLER std::vector< boost::shared_ptr > K_cameras; boost::shared_ptr K(new Cal3Bundler()); -#else - std::vector< boost::shared_ptr > K_cameras; - Cal3_S2::shared_ptr K(new Cal3_S2()); -#endif SmartFactorsCreator smartCreator(pixel_sigma, K, rankTolerance, linThreshold); // this initial K is not used ProjectionFactorsCreator projectionCreator(pixel_sigma, K); // this initial K is not used @@ -306,8 +287,8 @@ int main(int argc, char** argv) { Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.3,0.1,0.3)); cameraPose = cameraPose.compose(noise_pose); } - loadedValues->insert(X(i), cameraPose); - graphSmartValues->insert(X(i), cameraPose); + loadedValues->insert(X(i), cameraPose); // this will be used for the graphProjection + graphSmartValues->insert(X(i), cameraPose); // we insert the value for the graphSmart that only contains poses } if(debug) std::cout << "Initialized values " << std::endl; @@ -324,15 +305,13 @@ int main(int argc, char** argv) { for (size_t k = 0; k < track.number_measurements(); k++){ // for each measurement of the point SfM_Measurement measurement = track.measurements[k]; - int i = measurement.first; + int i = measurement.first; // camera id double u = measurement.second.x(); double v = measurement.second.y(); boost::shared_ptr Ki(new Cal3Bundler(inputData.cameras[i].calibration())); - //boost::shared_ptr Ki(new Cal3_S2()); - // insert data in a format that can be understood if (useSmartProjectionFactor) { - // Use smart factors + // use SMART PROJECTION FACTORS smartCreator.add(L(j), X(i), Point2(u,v), pixel_sigma, Ki, graphSmart); numLandmarks = smartCreator.getNumLandmarks(); } else { @@ -342,31 +321,33 @@ int main(int argc, char** argv) { } } } - if(debug) std::cout << "Included measurements in the graph " << std::endl; - - cout << "Number of landmarks " << numLandmarks << endl; - - cout << "Before call to update: ------------------ " << endl; - cout << "Poses in SmartGraph values: "<< graphSmartValues->size() << endl; - Values valuesProjPoses = graphProjectionValues->filter(); - cout << "Poses in ProjectionGraph values: "<< valuesProjPoses.size() << endl; - Values valuesProjPoints = graphProjectionValues->filter(); - cout << "Points in ProjectionGraph values: "<< valuesProjPoints.size() << endl; - cout << "---------------------------------------------------------- " << endl; + if(debug){ + cout << "Included measurements in the graph " << endl; + cout << "Number of landmarks " << numLandmarks << endl; + cout << "Before call to update: ------------------ " << endl; + cout << "Poses in SmartGraph values: "<< graphSmartValues->size() << endl; + Values valuesProjPoses = graphProjectionValues->filter(); + cout << "Poses in ProjectionGraph values: "<< valuesProjPoses.size() << endl; + Values valuesProjPoints = graphProjectionValues->filter(); + cout << "Points in ProjectionGraph values: "<< valuesProjPoints.size() << endl; + cout << "---------------------------------------------------------- " << endl; + } if (!useSmartProjectionFactor) { projectionCreator.update(graphProjection, loadedValues, graphProjectionValues, doTriangulation); - // graphProjectionValues = loadedValues; ordering = projectionCreator.getOrdering(); } - cout << "After call to update: ------------------ " << endl; - cout << "Poses in SmartGraph values: "<< graphSmartValues->size() << endl; - valuesProjPoses = graphProjectionValues->filter(); - cout << "Poses in ProjectionGraph values: "<< valuesProjPoses.size() << endl; - valuesProjPoints = graphProjectionValues->filter(); - cout << "Points in ProjectionGraph values: "<< valuesProjPoints.size() << endl; - cout << "---------------------------------------------------------- " << endl; + if(debug) { + cout << "After call to update: ------------------ " << endl; + cout << "--------------------------------------------------------- " << endl; + cout << "Poses in SmartGraph values: "<< graphSmartValues->size() << endl; + Values valuesProjPoses = graphProjectionValues->filter(); + cout << "Poses in ProjectionGraph values: "<< valuesProjPoses.size() << endl; + Values valuesProjPoints = graphProjectionValues->filter(); + cout << "Points in ProjectionGraph values: "<< valuesProjPoints.size() << endl; + cout << "---------------------------------------------------------- " << endl; + } Values result; if (useSmartProjectionFactor) { @@ -374,20 +355,28 @@ int main(int argc, char** argv) { optimizeGraphLM(graphSmart, graphSmartValues, result, ordering); else optimizeGraphISAM2(graphSmart, graphSmartValues, result); - cout << "Final reprojection error (smart): " << graphSmart.error(result); + cout << "Initial reprojection error (smart): " << graphSmart.error(*graphSmartValues) << endl;; + cout << "Final reprojection error (smart): " << graphSmart.error(result) << endl;; } else { if (useLM) optimizeGraphLM(graphProjection, graphProjectionValues, result, ordering); else - optimizeGraphISAM2(graphProjection, graphProjectionValues, result); // ? - cout << "Final reprojection error (standard): " << graphProjection.error(result); + optimizeGraphISAM2(graphProjection, graphProjectionValues, result); + cout << "Initial reprojection error (standard): " << graphProjection.error(*graphProjectionValues) << endl;; + cout << "Final reprojection error (standard): " << graphProjection.error(result) << endl;; } - cout << "===================================================" << endl; tictoc_print_(); cout << "===================================================" << endl; - writeValues("./", result); + // --------------- WRITE OUTPUT TO BAL FILE ---------------------------------------- + cout << "- writing results to (BAL) file... " << endl; + std::size_t stringCut1 = datasetFile.rfind("/"); + std::size_t stringCut2 = datasetFile.rfind(".txt"); + string outputFile = "." + datasetFile.substr(stringCut1, stringCut2-stringCut1) + "-optimized.txt"; + if(debug) cout << outputFile << endl; + writeBALfromValues(outputFile, inputData, result); + cout << "- mission accomplished! " << endl; exit(0); }