added functionalities in dataset.cpp (writeBALfromValues) and BAL example
Luca Carlone
parent
916d730fce
commit
796d9c7a67
|
|
@ -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<Pose3>();
|
||||
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<Point3>();
|
||||
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<Pose3>(poseKey);
|
||||
Cal3Bundler K = data.cameras[i].calibration();
|
||||
PinholeCamera<Cal3Bundler> 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<Point3>(pointKey);
|
||||
data.tracks[j].p = point;
|
||||
}
|
||||
|
||||
return writeBAL(filename, data);
|
||||
}
|
||||
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@
|
|||
/**
|
||||
* @file testDataset.cpp
|
||||
* @brief Unit test for dataset.cpp
|
||||
* @author Richard Roberts
|
||||
* @author Richard Roberts, Luca Carlone
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -20,6 +20,7 @@
|
|||
#include <boost/algorithm/string.hpp>
|
||||
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
|
||||
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<Pose3>(poseKey);
|
||||
EXPECT(assert_equal(expectedPose,actualPose, 1e-7));
|
||||
|
||||
Point3 expectedPoint = track0.p;
|
||||
Key pointKey = symbol('l',0);
|
||||
Point3 actualPoint = value.at<Point3>(pointKey);
|
||||
EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -41,7 +41,7 @@
|
|||
// have been provided with the library for solving robotics SLAM problems.
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
#include <gtsam_unstable/slam/SmartProjectionFactor.h>
|
||||
#include <gtsam_unstable/slam/SmartProjectionHessianFactor.h>
|
||||
|
||||
// We need to use SFM_data to save it to BAL format
|
||||
#include <gtsam/slam/dataset.h>
|
||||
|
|
|
|||
|
|
@ -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<Pose3> Pose3Prior;
|
||||
typedef FastMap<Key, int> OrderingMap;
|
||||
|
||||
#ifdef USE_BUNDLER
|
||||
typedef SmartProjectionFactorsCreator<Pose3, Point3, Cal3Bundler> SmartFactorsCreator;
|
||||
typedef GenericProjectionFactorsCreator<Pose3, Point3, Cal3Bundler> ProjectionFactorsCreator;
|
||||
#else
|
||||
typedef SmartProjectionFactorsCreator<Pose3, Point3, Cal3_S2> SmartFactorsCreator;
|
||||
typedef GenericProjectionFactorsCreator<Pose3, Point3, Cal3_S2> 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<Ordering> 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<Ordering>(*(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> 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<Cal3Bundler> > K_cameras;
|
||||
boost::shared_ptr<Cal3Bundler> K(new Cal3Bundler());
|
||||
#else
|
||||
std::vector< boost::shared_ptr<Cal3_S2> > 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<Cal3Bundler> Ki(new Cal3Bundler(inputData.cameras[i].calibration()));
|
||||
//boost::shared_ptr<Cal3_S2> 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,10 +321,9 @@ int main(int argc, char** argv) {
|
|||
}
|
||||
}
|
||||
}
|
||||
if(debug) std::cout << "Included measurements in the graph " << std::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<Pose3>();
|
||||
|
|
@ -353,20 +331,23 @@ int main(int argc, char** argv) {
|
|||
Values valuesProjPoints = graphProjectionValues->filter<Point3>();
|
||||
cout << "Points in ProjectionGraph values: "<< valuesProjPoints.size() << endl;
|
||||
cout << "---------------------------------------------------------- " << endl;
|
||||
}
|
||||
|
||||
if (!useSmartProjectionFactor) {
|
||||
projectionCreator.update(graphProjection, loadedValues, graphProjectionValues, doTriangulation);
|
||||
// graphProjectionValues = loadedValues;
|
||||
ordering = projectionCreator.getOrdering();
|
||||
}
|
||||
|
||||
if(debug) {
|
||||
cout << "After call to update: ------------------ " << endl;
|
||||
cout << "--------------------------------------------------------- " << endl;
|
||||
cout << "Poses in SmartGraph values: "<< graphSmartValues->size() << endl;
|
||||
valuesProjPoses = graphProjectionValues->filter<Pose3>();
|
||||
Values valuesProjPoses = graphProjectionValues->filter<Pose3>();
|
||||
cout << "Poses in ProjectionGraph values: "<< valuesProjPoses.size() << endl;
|
||||
valuesProjPoints = graphProjectionValues->filter<Point3>();
|
||||
Values valuesProjPoints = graphProjectionValues->filter<Point3>();
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue