Sanitized G2o I/O interface to conform to what we had before. No sense in having many different styles, and this works better for MATLAB (now wrapped, as well).
BAL reading/writing should be similarly cleaned up.release/4.3a0
dellaert
parent
470527ff99
commit
7119d0c3c2
|
|
@ -35,18 +35,18 @@ int main(const int argc, const char *argv[]) {
|
|||
else
|
||||
g2oFile = argv[1];
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
Values initial;
|
||||
readG2o(g2oFile, graph, initial);
|
||||
NonlinearFactorGraph::shared_ptr graph;
|
||||
Values::shared_ptr initial;
|
||||
boost::tie(graph, initial) = readG2o(g2oFile);
|
||||
|
||||
// Add prior on the pose having index (key) = 0
|
||||
NonlinearFactorGraph graphWithPrior = graph;
|
||||
NonlinearFactorGraph graphWithPrior = *graph;
|
||||
noiseModel::Diagonal::shared_ptr priorModel = //
|
||||
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
|
||||
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
|
||||
|
||||
std::cout << "Optimizing the factor graph" << std::endl;
|
||||
GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters);
|
||||
GaussNewtonOptimizer optimizer(graphWithPrior, *initial);
|
||||
Values result = optimizer.optimize();
|
||||
std::cout << "Optimization complete" << std::endl;
|
||||
|
||||
|
|
@ -55,7 +55,7 @@ int main(const int argc, const char *argv[]) {
|
|||
} else {
|
||||
const string outputFile = argv[2];
|
||||
std::cout << "Writing results to file: " << outputFile << std::endl;
|
||||
writeG2o(outputFile, graph, result);
|
||||
writeG2o(*graph, result, outputFile);
|
||||
std::cout << "done! " << std::endl;
|
||||
}
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -36,12 +36,12 @@ int main(const int argc, const char *argv[]) {
|
|||
else
|
||||
g2oFile = argv[1];
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
Values initial;
|
||||
readG2o(g2oFile, graph, initial);
|
||||
NonlinearFactorGraph::shared_ptr graph;
|
||||
Values::shared_ptr initial;
|
||||
boost::tie(graph, initial) = readG2o(g2oFile);
|
||||
|
||||
// Add prior on the pose having index (key) = 0
|
||||
NonlinearFactorGraph graphWithPrior = graph;
|
||||
NonlinearFactorGraph graphWithPrior = *graph;
|
||||
noiseModel::Diagonal::shared_ptr priorModel = //
|
||||
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
|
||||
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
|
||||
|
|
@ -56,7 +56,7 @@ int main(const int argc, const char *argv[]) {
|
|||
} else {
|
||||
const string outputFile = argv[2];
|
||||
std::cout << "Writing results to file: " << outputFile << std::endl;
|
||||
writeG2o(outputFile, graph, estimateLago);
|
||||
writeG2o(*graph, estimateLago, outputFile);
|
||||
std::cout << "done! " << std::endl;
|
||||
}
|
||||
|
||||
|
|
|
|||
7
gtsam.h
7
gtsam.h
|
|
@ -2249,6 +2249,13 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
|
|||
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
|
||||
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
|
||||
gtsam::noiseModel::Base* model);
|
||||
void save2D(const gtsam::NonlinearFactorGraph& graph,
|
||||
const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
|
||||
string filename);
|
||||
|
||||
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
|
||||
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
|
||||
const gtsam::Values& estimate, string filename);
|
||||
|
||||
//*************************************************************************
|
||||
// Navigation
|
||||
|
|
|
|||
|
|
@ -258,13 +258,13 @@ TEST( Lago, smallGraph2 ) {
|
|||
/* *************************************************************************** */
|
||||
TEST( Lago, largeGraphNoisy_orientations ) {
|
||||
|
||||
NonlinearFactorGraph g;
|
||||
Values initial;
|
||||
string inputFile = findExampleDataFile("noisyToyGraph");
|
||||
readG2o(inputFile, g, initial);
|
||||
NonlinearFactorGraph::shared_ptr g;
|
||||
Values::shared_ptr initial;
|
||||
boost::tie(g, initial) = readG2o(inputFile);
|
||||
|
||||
// Add prior on the pose having index (key) = 0
|
||||
NonlinearFactorGraph graphWithPrior = g;
|
||||
NonlinearFactorGraph graphWithPrior = *g;
|
||||
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
|
||||
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
|
||||
|
||||
|
|
@ -279,40 +279,40 @@ TEST( Lago, largeGraphNoisy_orientations ) {
|
|||
actual.insert(key, poseLago);
|
||||
}
|
||||
}
|
||||
NonlinearFactorGraph gmatlab;
|
||||
Values expected;
|
||||
string matlabFile = findExampleDataFile("orientationsNoisyToyGraph");
|
||||
readG2o(matlabFile, gmatlab, expected);
|
||||
NonlinearFactorGraph::shared_ptr gmatlab;
|
||||
Values::shared_ptr expected;
|
||||
boost::tie(gmatlab, expected) = readG2o(matlabFile);
|
||||
|
||||
BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){
|
||||
BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){
|
||||
Key k = key_val.key;
|
||||
EXPECT(assert_equal(expected.at<Pose2>(k), actual.at<Pose2>(k), 1e-5));
|
||||
EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-5));
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************************** */
|
||||
TEST( Lago, largeGraphNoisy ) {
|
||||
|
||||
NonlinearFactorGraph g;
|
||||
Values initial;
|
||||
string inputFile = findExampleDataFile("noisyToyGraph");
|
||||
readG2o(inputFile, g, initial);
|
||||
NonlinearFactorGraph::shared_ptr g;
|
||||
Values::shared_ptr initial;
|
||||
boost::tie(g, initial) = readG2o(inputFile);
|
||||
|
||||
// Add prior on the pose having index (key) = 0
|
||||
NonlinearFactorGraph graphWithPrior = g;
|
||||
NonlinearFactorGraph graphWithPrior = *g;
|
||||
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
|
||||
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
|
||||
|
||||
Values actual = lago::initialize(graphWithPrior);
|
||||
|
||||
NonlinearFactorGraph gmatlab;
|
||||
Values expected;
|
||||
string matlabFile = findExampleDataFile("optimizedNoisyToyGraph");
|
||||
readG2o(matlabFile, gmatlab, expected);
|
||||
NonlinearFactorGraph::shared_ptr gmatlab;
|
||||
Values::shared_ptr expected;
|
||||
boost::tie(gmatlab, expected) = readG2o(matlabFile);
|
||||
|
||||
BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){
|
||||
BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){
|
||||
Key k = key_val.key;
|
||||
EXPECT(assert_equal(expected.at<Pose2>(k), actual.at<Pose2>(k), 1e-2));
|
||||
EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-2));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@ string findExampleDataFile(const string& name) {
|
|||
|
||||
// If we did not return already, then we did not find the file
|
||||
throw
|
||||
std::invalid_argument(
|
||||
invalid_argument(
|
||||
"gtsam::findExampleDataFile could not find a matching file in\n"
|
||||
SOURCE_TREE_DATASET_DIR " or\n"
|
||||
INSTALLED_DATASET_DIR " named\n" +
|
||||
|
|
@ -73,7 +73,7 @@ std::invalid_argument(
|
|||
string createRewrittenFileName(const string& name) {
|
||||
// Search source tree and installed location
|
||||
if (!exists(fs::path(name))) {
|
||||
throw std::invalid_argument(
|
||||
throw invalid_argument(
|
||||
"gtsam::createRewrittenFileName could not find a matching file in\n"
|
||||
+ name);
|
||||
}
|
||||
|
|
@ -89,9 +89,8 @@ string createRewrittenFileName(const string& name) {
|
|||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
||||
pair<string, SharedNoiseModel> dataset, int maxID, bool addNoise,
|
||||
bool smart, NoiseFormat noiseFormat,
|
||||
GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, int maxID,
|
||||
bool addNoise, bool smart, NoiseFormat noiseFormat,
|
||||
KernelFunctionType kernelFunctionType) {
|
||||
return load2D(dataset.first, dataset.second, maxID, addNoise, smart,
|
||||
noiseFormat, kernelFunctionType);
|
||||
|
|
@ -111,7 +110,7 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
|
|||
case NoiseFormatCOV:
|
||||
// i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
|
||||
if (v1 == 0.0 || v4 == 0.0 || v6 == 0.0)
|
||||
throw std::runtime_error(
|
||||
throw runtime_error(
|
||||
"load2D::readNoiseModel looks like this is not G2O matrix order");
|
||||
M << v1, v2, v3, v2, v4, v5, v3, v5, v6;
|
||||
break;
|
||||
|
|
@ -121,12 +120,12 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
|
|||
// inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
|
||||
// i.e., [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ]
|
||||
if (v1 == 0.0 || v3 == 0.0 || v4 == 0.0)
|
||||
throw std::invalid_argument(
|
||||
throw invalid_argument(
|
||||
"load2D::readNoiseModel looks like this is not TORO matrix order");
|
||||
M << v1, v2, v5, v2, v3, v6, v5, v6, v4;
|
||||
break;
|
||||
default:
|
||||
throw std::runtime_error("load2D: invalid noise format");
|
||||
throw runtime_error("load2D: invalid noise format");
|
||||
}
|
||||
|
||||
// Now, create a Gaussian noise model
|
||||
|
|
@ -144,11 +143,11 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
|
|||
model = noiseModel::Gaussian::Covariance(M, smart);
|
||||
break;
|
||||
default:
|
||||
throw std::invalid_argument("load2D: invalid noise format");
|
||||
throw invalid_argument("load2D: invalid noise format");
|
||||
}
|
||||
|
||||
switch (kernelFunctionType) {
|
||||
case KernelFunctionTypeQUADRATIC:
|
||||
case KernelFunctionTypeNONE:
|
||||
return model;
|
||||
break;
|
||||
case KernelFunctionTypeHUBER:
|
||||
|
|
@ -160,20 +159,18 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
|
|||
noiseModel::mEstimator::Tukey::Create(4.6851), model);
|
||||
break;
|
||||
default:
|
||||
throw std::invalid_argument("load2D: invalid kernel function type");
|
||||
throw invalid_argument("load2D: invalid kernel function type");
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
||||
const string& filename, SharedNoiseModel model, int maxID, bool addNoise,
|
||||
bool smart, NoiseFormat noiseFormat,
|
||||
GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
|
||||
bool addNoise, bool smart, NoiseFormat noiseFormat,
|
||||
KernelFunctionType kernelFunctionType) {
|
||||
|
||||
cout << "Will try to read " << filename << endl;
|
||||
ifstream is(filename.c_str());
|
||||
if (!is)
|
||||
throw std::invalid_argument("load2D: can not find file " + filename);
|
||||
throw invalid_argument("load2D: can not find file " + filename);
|
||||
|
||||
Values::shared_ptr initial(new Values);
|
||||
NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
|
||||
|
|
@ -270,8 +267,8 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
|||
is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3;
|
||||
|
||||
// Convert x,y to bearing,range
|
||||
bearing = std::atan2(lmy, lmx);
|
||||
range = std::sqrt(lmx * lmx + lmy * lmy);
|
||||
bearing = atan2(lmy, lmx);
|
||||
range = sqrt(lmx * lmx + lmy * lmy);
|
||||
|
||||
// In our experience, the x-y covariance on landmark sightings is not very good, so assume
|
||||
// it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty.
|
||||
|
|
@ -319,12 +316,15 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
|||
is.ignore(LINESIZE, '\n');
|
||||
}
|
||||
|
||||
cout << "load2D read a graph file with " << initial->size()
|
||||
<< " vertices and " << graph->nrFactors() << " factors" << endl;
|
||||
|
||||
return make_pair(graph, initial);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GraphAndValues load2D_robust(const string& filename,
|
||||
noiseModel::Base::shared_ptr& model, int maxID) {
|
||||
return load2D(filename, model, maxID);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void save2D(const NonlinearFactorGraph& graph, const Values& config,
|
||||
const noiseModel::Diagonal::shared_ptr model, const string& filename) {
|
||||
|
|
@ -357,6 +357,54 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
|
|||
stream.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GraphAndValues readG2o(const string& g2oFile,
|
||||
KernelFunctionType kernelFunctionType) {
|
||||
// just call load2D
|
||||
int maxID = 0;
|
||||
bool addNoise = false;
|
||||
bool smart = true;
|
||||
return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
|
||||
NoiseFormatG2O, kernelFunctionType);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
|
||||
const string& filename) {
|
||||
|
||||
fstream stream(filename.c_str(), fstream::out);
|
||||
|
||||
// save poses
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) {
|
||||
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
|
||||
stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
|
||||
<< pose.y() << " " << pose.theta() << endl;
|
||||
}
|
||||
|
||||
// save edges
|
||||
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
|
||||
boost::shared_ptr<BetweenFactor<Pose2> > factor =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
|
||||
if (!factor)
|
||||
continue;
|
||||
|
||||
SharedNoiseModel model = factor->get_noiseModel();
|
||||
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
|
||||
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
|
||||
if (!diagonalModel)
|
||||
throw invalid_argument(
|
||||
"writeG2o: invalid noise model (current version assumes diagonal noise model)!");
|
||||
|
||||
Pose2 pose = factor->measured(); //.inverse();
|
||||
stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
|
||||
<< pose.x() << " " << pose.y() << " " << pose.theta() << " "
|
||||
<< diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " "
|
||||
<< diagonalModel->precision(1) << " " << 0.0 << " "
|
||||
<< diagonalModel->precision(2) << endl;
|
||||
}
|
||||
stream.close();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool load3D(const string& filename) {
|
||||
ifstream is(filename.c_str());
|
||||
|
|
@ -399,105 +447,6 @@ bool load3D(const string& filename) {
|
|||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
|
||||
const string& filename, noiseModel::Base::shared_ptr& model, int maxID) {
|
||||
cout << "Will try to read " << filename << endl;
|
||||
ifstream is(filename.c_str());
|
||||
if (!is)
|
||||
throw std::invalid_argument("load2D: can not find the file!");
|
||||
|
||||
Values::shared_ptr initial(new Values);
|
||||
NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
|
||||
|
||||
string tag;
|
||||
|
||||
// load the poses
|
||||
while (is) {
|
||||
is >> tag;
|
||||
|
||||
if ((tag == "VERTEX2") || (tag == "VERTEX")) {
|
||||
int id;
|
||||
double x, y, yaw;
|
||||
is >> id >> x >> y >> yaw;
|
||||
// optional filter
|
||||
if (maxID && id >= maxID)
|
||||
continue;
|
||||
initial->insert(id, Pose2(x, y, yaw));
|
||||
}
|
||||
is.ignore(LINESIZE, '\n');
|
||||
}
|
||||
is.clear(); /* clears the end-of-file and error flags */
|
||||
is.seekg(0, ios::beg);
|
||||
|
||||
// Create a sampler with random number generator
|
||||
Sampler sampler(42u);
|
||||
|
||||
// load the factors
|
||||
while (is) {
|
||||
is >> tag;
|
||||
|
||||
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) {
|
||||
int id1, id2;
|
||||
double x, y, yaw;
|
||||
|
||||
is >> id1 >> id2 >> x >> y >> yaw;
|
||||
Matrix m = eye(3);
|
||||
is >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >> m(1, 2);
|
||||
m(2, 0) = m(0, 2);
|
||||
m(2, 1) = m(1, 2);
|
||||
m(1, 0) = m(0, 1);
|
||||
|
||||
// optional filter
|
||||
if (maxID && (id1 >= maxID || id2 >= maxID))
|
||||
continue;
|
||||
|
||||
Pose2 l1Xl2(x, y, yaw);
|
||||
|
||||
// Insert vertices if pure odometry file
|
||||
if (!initial->exists(id1))
|
||||
initial->insert(id1, Pose2());
|
||||
if (!initial->exists(id2))
|
||||
initial->insert(id2, initial->at<Pose2>(id1) * l1Xl2);
|
||||
|
||||
NonlinearFactor::shared_ptr factor(
|
||||
new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
|
||||
graph->push_back(factor);
|
||||
}
|
||||
if (tag == "BR") {
|
||||
int id1, id2;
|
||||
double bearing, range, bearing_std, range_std;
|
||||
|
||||
is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std;
|
||||
|
||||
// optional filter
|
||||
if (maxID && (id1 >= maxID || id2 >= maxID))
|
||||
continue;
|
||||
|
||||
noiseModel::Diagonal::shared_ptr measurementNoise =
|
||||
noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std));
|
||||
*graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range,
|
||||
measurementNoise);
|
||||
|
||||
// Insert poses or points if they do not exist yet
|
||||
if (!initial->exists(id1))
|
||||
initial->insert(id1, Pose2());
|
||||
if (!initial->exists(id2)) {
|
||||
Pose2 pose = initial->at<Pose2>(id1);
|
||||
Point2 local(cos(bearing) * range, sin(bearing) * range);
|
||||
Point2 global = pose.transform_from(local);
|
||||
initial->insert(id2, global);
|
||||
}
|
||||
}
|
||||
is.ignore(LINESIZE, '\n');
|
||||
}
|
||||
|
||||
cout << "load2D read a graph file with " << initial->size()
|
||||
<< " vertices and " << graph->nrFactors() << " factors" << endl;
|
||||
|
||||
return make_pair(graph, initial);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 openGLFixedRotation() { // this is due to different convention for cameras in gtsam and openGL
|
||||
/* R = [ 1 0 0
|
||||
|
|
@ -617,61 +566,6 @@ bool readBundler(const string& filename, SfM_data &data) {
|
|||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph,
|
||||
Values& initial, KernelFunctionType kernelFunctionType) {
|
||||
|
||||
// just call load2D
|
||||
NonlinearFactorGraph::shared_ptr graph_ptr;
|
||||
Values::shared_ptr initial_ptr;
|
||||
int maxID = 0;
|
||||
bool addNoise = false;
|
||||
bool smart = true;
|
||||
boost::tie(graph_ptr, initial_ptr) = load2D(g2oFile, SharedNoiseModel(),
|
||||
maxID, addNoise, smart, NoiseFormatG2O, kernelFunctionType);
|
||||
graph = *graph_ptr;
|
||||
initial = *initial_ptr;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph,
|
||||
const Values& estimate) {
|
||||
|
||||
fstream stream(filename.c_str(), fstream::out);
|
||||
|
||||
// save poses
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) {
|
||||
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
|
||||
stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
|
||||
<< pose.y() << " " << pose.theta() << endl;
|
||||
}
|
||||
|
||||
// save edges
|
||||
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
|
||||
boost::shared_ptr<BetweenFactor<Pose2> > factor =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
|
||||
if (!factor)
|
||||
continue;
|
||||
|
||||
SharedNoiseModel model = factor->get_noiseModel();
|
||||
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
|
||||
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
|
||||
if (!diagonalModel)
|
||||
throw std::invalid_argument(
|
||||
"writeG2o: invalid noise model (current version assumes diagonal noise model)!");
|
||||
|
||||
Pose2 pose = factor->measured(); //.inverse();
|
||||
stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
|
||||
<< pose.x() << " " << pose.y() << " " << pose.theta() << " "
|
||||
<< diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " "
|
||||
<< diagonalModel->precision(1) << " " << 0.0 << " "
|
||||
<< diagonalModel->precision(2) << endl;
|
||||
}
|
||||
stream.close();
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool readBAL(const string& filename, SfM_data &data) {
|
||||
// Load the data file
|
||||
|
|
|
|||
|
|
@ -60,10 +60,14 @@ enum NoiseFormat {
|
|||
NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33
|
||||
};
|
||||
|
||||
/// Robust kernel type to wrap around quadratic noise model
|
||||
enum KernelFunctionType {
|
||||
KernelFunctionTypeQUADRATIC, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
|
||||
KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
|
||||
};
|
||||
|
||||
/// Return type for load functions
|
||||
typedef std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> GraphAndValues;
|
||||
|
||||
/**
|
||||
* Load TORO 2D Graph
|
||||
* @param dataset/model pair as constructed by [dataset]
|
||||
|
|
@ -71,36 +75,58 @@ enum KernelFunctionType {
|
|||
* @param addNoise add noise to the edges
|
||||
* @param smart try to reduce complexity of covariance to cheapest model
|
||||
*/
|
||||
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
||||
GTSAM_EXPORT GraphAndValues load2D(
|
||||
std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
|
||||
bool addNoise = false,
|
||||
bool smart = true, //
|
||||
NoiseFormat noiseFormat = NoiseFormatGRAPH,
|
||||
KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC);
|
||||
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
|
||||
|
||||
/**
|
||||
* Load TORO 2D Graph
|
||||
* Load TORO/G2O style graph files
|
||||
* @param filename
|
||||
* @param model optional noise model to use instead of one specified by file
|
||||
* @param maxID if non-zero cut out vertices >= maxID
|
||||
* @param addNoise add noise to the edges
|
||||
* @param smart try to reduce complexity of covariance to cheapest model
|
||||
* @param noiseFormat how noise parameters are stored
|
||||
* @param kernelFunctionType whether to wrap the noise model in a robust kernel
|
||||
* @return graph and initial values
|
||||
*/
|
||||
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
|
||||
const std::string& filename, SharedNoiseModel model = SharedNoiseModel(),
|
||||
int maxID = 0, bool addNoise = false, bool smart = true,
|
||||
NoiseFormat noiseFormat = NoiseFormatGRAPH, //
|
||||
KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC);
|
||||
GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
|
||||
SharedNoiseModel model = SharedNoiseModel(), int maxID = 0, bool addNoise =
|
||||
false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatGRAPH, //
|
||||
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
|
||||
|
||||
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
|
||||
const std::string& filename, noiseModel::Base::shared_ptr& model,
|
||||
int maxID = 0);
|
||||
/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel
|
||||
GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename,
|
||||
noiseModel::Base::shared_ptr& model, int maxID = 0);
|
||||
|
||||
/** save 2d graph */
|
||||
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
|
||||
const Values& config, const noiseModel::Diagonal::shared_ptr model,
|
||||
const std::string& filename);
|
||||
|
||||
/**
|
||||
* @brief This function parses a g2o file and stores the measurements into a
|
||||
* NonlinearFactorGraph and the initial guess in a Values structure
|
||||
* @param filename The name of the g2o file
|
||||
* @param kernelFunctionType whether to wrap the noise model in a robust kernel
|
||||
* @return graph and initial values
|
||||
*/
|
||||
GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile,
|
||||
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
|
||||
|
||||
/**
|
||||
* @brief This function writes a g2o file from
|
||||
* NonlinearFactorGraph and a Values structure
|
||||
* @param filename The name of the g2o file to write
|
||||
* @param graph NonlinearFactor graph storing the measurements
|
||||
* @param estimate Values
|
||||
*/
|
||||
GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
|
||||
const Values& estimate, const std::string& filename);
|
||||
|
||||
/**
|
||||
* Load TORO 3D Graph
|
||||
*/
|
||||
|
|
@ -143,27 +169,6 @@ struct SfM_data {
|
|||
*/
|
||||
GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
|
||||
|
||||
/**
|
||||
* @brief This function parses a g2o file and stores the measurements into a
|
||||
* NonlinearFactorGraph and the initial guess in a Values structure
|
||||
* @param filename The name of the g2o file
|
||||
* @param graph NonlinearFactor graph storing the measurements (EDGE_SE2). NOTE: information matrix is assumed diagonal.
|
||||
* @return initial Values containing the initial guess (VERTEX_SE2)
|
||||
*/
|
||||
GTSAM_EXPORT bool readG2o(const std::string& g2oFile,
|
||||
NonlinearFactorGraph& graph, Values& initial,
|
||||
KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC);
|
||||
|
||||
/**
|
||||
* @brief This function writes a g2o file from
|
||||
* NonlinearFactorGraph and a Values structure
|
||||
* @param filename The name of the g2o file to write
|
||||
* @param graph NonlinearFactor graph storing the measurements (EDGE_SE2)
|
||||
* @return estimate Values containing the values (VERTEX_SE2)
|
||||
*/
|
||||
GTSAM_EXPORT bool writeG2o(const std::string& filename,
|
||||
const NonlinearFactorGraph& graph, const Values& estimate);
|
||||
|
||||
/**
|
||||
* @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
|
||||
* SfM_data structure
|
||||
|
|
|
|||
|
|
@ -81,9 +81,9 @@ TEST( dataSet, Balbianello)
|
|||
TEST( dataSet, readG2o)
|
||||
{
|
||||
const string g2oFile = findExampleDataFile("pose2example");
|
||||
NonlinearFactorGraph actualGraph;
|
||||
Values actualValues;
|
||||
readG2o(g2oFile, actualGraph, actualValues);
|
||||
NonlinearFactorGraph::shared_ptr actualGraph;
|
||||
Values::shared_ptr actualValues;
|
||||
boost::tie(actualGraph, actualValues) = readG2o(g2oFile);
|
||||
|
||||
Values expectedValues;
|
||||
expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000));
|
||||
|
|
@ -97,7 +97,7 @@ TEST( dataSet, readG2o)
|
|||
expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391));
|
||||
expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016));
|
||||
expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934));
|
||||
EXPECT(assert_equal(expectedValues,actualValues,1e-5));
|
||||
EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
|
||||
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
|
||||
NonlinearFactorGraph expectedGraph;
|
||||
|
|
@ -113,16 +113,16 @@ TEST( dataSet, readG2o)
|
|||
expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
|
||||
expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
|
||||
expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
|
||||
EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
|
||||
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( dataSet, readG2oHuber)
|
||||
{
|
||||
const string g2oFile = findExampleDataFile("pose2example");
|
||||
NonlinearFactorGraph actualGraph;
|
||||
Values actualValues;
|
||||
readG2o(g2oFile, actualGraph, actualValues, KernelFunctionTypeHUBER);
|
||||
NonlinearFactorGraph::shared_ptr actualGraph;
|
||||
Values::shared_ptr actualValues;
|
||||
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeHUBER);
|
||||
|
||||
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
|
||||
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel);
|
||||
|
|
@ -140,16 +140,16 @@ TEST( dataSet, readG2oHuber)
|
|||
expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
|
||||
expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
|
||||
expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
|
||||
EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
|
||||
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( dataSet, readG2oTukey)
|
||||
{
|
||||
const string g2oFile = findExampleDataFile("pose2example");
|
||||
NonlinearFactorGraph actualGraph;
|
||||
Values actualValues;
|
||||
readG2o(g2oFile, actualGraph, actualValues, KernelFunctionTypeTUKEY);
|
||||
NonlinearFactorGraph::shared_ptr actualGraph;
|
||||
Values::shared_ptr actualValues;
|
||||
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeTUKEY);
|
||||
|
||||
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
|
||||
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
|
||||
|
|
@ -167,25 +167,25 @@ TEST( dataSet, readG2oTukey)
|
|||
expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
|
||||
expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
|
||||
expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
|
||||
EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
|
||||
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( dataSet, writeG2o)
|
||||
{
|
||||
const string g2oFile = findExampleDataFile("pose2example");
|
||||
NonlinearFactorGraph expectedGraph;
|
||||
Values expectedValues;
|
||||
readG2o(g2oFile, expectedGraph, expectedValues);
|
||||
NonlinearFactorGraph::shared_ptr expectedGraph;
|
||||
Values::shared_ptr expectedValues;
|
||||
boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile);
|
||||
|
||||
const string filenameToWrite = createRewrittenFileName(g2oFile);
|
||||
writeG2o(filenameToWrite, expectedGraph, expectedValues);
|
||||
writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
|
||||
|
||||
NonlinearFactorGraph actualGraph;
|
||||
Values actualValues;
|
||||
readG2o(filenameToWrite, actualGraph, actualValues);
|
||||
EXPECT(assert_equal(expectedValues,actualValues,1e-5));
|
||||
EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
|
||||
NonlinearFactorGraph::shared_ptr actualGraph;
|
||||
Values::shared_ptr actualValues;
|
||||
boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite);
|
||||
EXPECT(assert_equal(*expectedValues,*actualValues,1e-5));
|
||||
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue