12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Finish fixDataset: eliminate copy/paste and handle noise formats sensibly

release/4.3a0
dellaert 2014-05-31 23:26:15 -04:00
parent bc69c0a94e a95cf7c71b
commit cc26fc5dfa
8 changed files with 447 additions and 371 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

128
.cproject
View File

@ -2185,70 +2185,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGeneralSFMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testProjectionFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testProjectionFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGeneralSFMFactor_Cal3Bundler.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testAntiFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>testAntiFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBetweenFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>testBetweenFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDataset.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDataset.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEssentialMatrixFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testEssentialMatrixFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRotateFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRotateFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -2649,6 +2585,70 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testAntiFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testAntiFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBetweenFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBetweenFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDataset.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDataset.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEssentialMatrixFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testEssentialMatrixFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGeneralSFMFactor_Cal3Bundler.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGeneralSFMFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGeneralSFMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testProjectionFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testProjectionFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRotateFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRotateFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>

39
gtsam/linear/NoiseModel.cpp
View File

@ -49,7 +49,7 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) {
/* ************************************************************************* */ /* ************************************************************************* */
// check *above the diagonal* for non-zero entries // check *above the diagonal* for non-zero entries
static boost::optional<Vector> checkIfDiagonal(const Matrix M) { boost::optional<Vector> checkIfDiagonal(const Matrix M) {
size_t m = M.rows(), n = M.cols(); size_t m = M.rows(), n = M.cols();
// check all non-diagonal entries // check all non-diagonal entries
bool full = false; bool full = false;
@ -74,23 +74,46 @@ static boost::optional<Vector> checkIfDiagonal(const Matrix M) {
/* ************************************************************************* */ /* ************************************************************************* */
Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) { Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
size_t m = R.rows(), n = R.cols(); size_t m = R.rows(), n = R.cols();
if (m != n) throw invalid_argument("Gaussian::SqrtInformation: R not square"); if (m != n)
throw invalid_argument("Gaussian::SqrtInformation: R not square");
boost::optional<Vector> diagonal = boost::none; boost::optional<Vector> diagonal = boost::none;
if (smart) if (smart)
diagonal = checkIfDiagonal(R); diagonal = checkIfDiagonal(R);
if (diagonal) return Diagonal::Sigmas(reciprocal(*diagonal),true); if (diagonal)
else return shared_ptr(new Gaussian(R.rows(),R)); return Diagonal::Sigmas(reciprocal(*diagonal), true);
else
return shared_ptr(new Gaussian(R.rows(), R));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) { Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) {
size_t m = M.rows(), n = M.cols();
if (m != n)
throw invalid_argument("Gaussian::Information: R not square");
boost::optional<Vector> diagonal = boost::none;
if (smart)
diagonal = checkIfDiagonal(M);
if (diagonal)
return Diagonal::Precisions(*diagonal, true);
else {
Matrix R = RtR(M);
return shared_ptr(new Gaussian(R.rows(), R));
}
}
/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance,
bool smart) {
size_t m = covariance.rows(), n = covariance.cols(); size_t m = covariance.rows(), n = covariance.cols();
if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square"); if (m != n)
throw invalid_argument("Gaussian::Covariance: covariance not square");
boost::optional<Vector> variances = boost::none; boost::optional<Vector> variances = boost::none;
if (smart) if (smart)
variances = checkIfDiagonal(covariance); variances = checkIfDiagonal(covariance);
if (variances) return Diagonal::Variances(*variances,true); if (variances)
else return shared_ptr(new Gaussian(n, inverse_square_root(covariance))); return Diagonal::Variances(*variances, true);
else
return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
} }
/* ************************************************************************* */ /* ************************************************************************* */

10
gtsam/linear/NoiseModel.h
View File

@ -164,6 +164,13 @@ namespace gtsam {
*/ */
static shared_ptr SqrtInformation(const Matrix& R, bool smart = true); static shared_ptr SqrtInformation(const Matrix& R, bool smart = true);
/**
* A Gaussian noise model created by specifying an information matrix.
* @param M The information matrix
* @param smart check if can be simplified to derived class
*/
static shared_ptr Information(const Matrix& M, bool smart = true);
/** /**
* A Gaussian noise model created by specifying a covariance matrix. * A Gaussian noise model created by specifying a covariance matrix.
* @param covariance The square covariance Matrix * @param covariance The square covariance Matrix
@ -865,6 +872,9 @@ namespace gtsam {
} }
}; };
// Helper function
boost::optional<Vector> checkIfDiagonal(const Matrix M);
} // namespace noiseModel } // namespace noiseModel
/** Note, deliberately not in noiseModel namespace. /** Note, deliberately not in noiseModel namespace.

11
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -285,6 +285,17 @@ TEST(NoiseModel, SmartSqrtInformation2 )
EXPECT(assert_equal(*expected,*actual)); EXPECT(assert_equal(*expected,*actual));
} }
/* ************************************************************************* */
TEST(NoiseModel, SmartInformation )
{
bool smart = true;
gtsam::SharedGaussian expected = Unit::Isotropic::Variance(3,2);
Matrix M = 0.5*eye(3);
EXPECT(checkIfDiagonal(M));
gtsam::SharedGaussian actual = Gaussian::Information(M, smart);
EXPECT(assert_equal(*expected,*actual));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(NoiseModel, SmartCovariance ) TEST(NoiseModel, SmartCovariance )
{ {

2
gtsam/nonlinear/tests/CMakeLists.txt
View File

@ -1 +1 @@
gtsamAddTestsGlob(nonlinear "*.cpp" "" "gtsam") gtsamAddTestsGlob(nonlinear "test*.cpp" "" "gtsam")

387
gtsam/slam/dataset.cpp
View File

@ -16,18 +16,18 @@
* @brief utility functions for loading datasets * @brief utility functions for loading datasets
*/ */
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <boost/filesystem.hpp>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h> #include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/inference/Symbol.h>
#include <boost/filesystem.hpp>
#include <fstream>
#include <sstream>
#include <cstdlib>
using namespace std; using namespace std;
namespace fs = boost::filesystem; namespace fs = boost::filesystem;
@ -61,7 +61,8 @@ string findExampleDataFile(const string& name) {
} }
// If we did not return already, then we did not find the file // If we did not return already, then we did not find the file
throw std::invalid_argument( throw
std::invalid_argument(
"gtsam::findExampleDataFile could not find a matching file in\n" "gtsam::findExampleDataFile could not find a matching file in\n"
SOURCE_TREE_DATASET_DIR " or\n" SOURCE_TREE_DATASET_DIR " or\n"
INSTALLED_DATASET_DIR " named\n" + INSTALLED_DATASET_DIR " named\n" +
@ -78,7 +79,8 @@ string createRewrittenFileName(const string& name) {
} }
fs::path p(name); fs::path p(name);
fs::path newpath = fs::path(p.parent_path().string()) / fs::path(p.stem().string() + "-rewritten.txt" ); fs::path newpath = fs::path(p.parent_path().string())
/ fs::path(p.stem().string() + "-rewritten.txt");
return newpath.string(); return newpath.string();
} }
@ -88,15 +90,86 @@ string createRewrittenFileName(const string& name) {
/* ************************************************************************* */ /* ************************************************************************* */
pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D( pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
pair<string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset, pair<string, SharedNoiseModel> dataset, int maxID, bool addNoise,
int maxID, bool addNoise, bool smart) { bool smart, NoiseFormat noiseFormat,
return load2D(dataset.first, dataset.second, maxID, addNoise, smart); KernelFunctionType kernelFunctionType) {
return load2D(dataset.first, dataset.second, maxID, addNoise, smart,
noiseFormat, kernelFunctionType);
}
/* ************************************************************************* */
// Read noise parameters and interpret them according to flags
static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) {
double v1, v2, v3, v4, v5, v6;
is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
// Read matrix and check that diagonal entries are non-zero
Matrix M(3, 3);
switch (noiseFormat) {
case NoiseFormatG2O:
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(
"load2D::readNoiseModel looks like this is not G2O matrix order");
M << v1, v2, v3, v2, v4, v5, v3, v5, v6;
break;
case NoiseFormatTORO:
case NoiseFormatGRAPH:
// http://www.openslam.org/toro.html
// 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(
"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");
}
// Now, create a Gaussian noise model
// The smart flag will try to detect a simpler model, e.g., unit
SharedNoiseModel model;
switch (noiseFormat) {
case NoiseFormatG2O:
case NoiseFormatTORO:
// In both cases, what is stored in file is the information matrix
model = noiseModel::Gaussian::Information(M, smart);
break;
case NoiseFormatGRAPH:
case NoiseFormatCOV:
// These cases expect covariance matrix
model = noiseModel::Gaussian::Covariance(M, smart);
break;
default:
throw std::invalid_argument("load2D: invalid noise format");
}
switch (kernelFunctionType) {
case KernelFunctionTypeQUADRATIC:
return model;
break;
case KernelFunctionTypeHUBER:
return noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), model);
break;
case KernelFunctionTypeTUKEY:
return noiseModel::Robust::Create(
noiseModel::mEstimator::Tukey::Create(4.6851), model);
break;
default:
throw std::invalid_argument("load2D: invalid kernel function type");
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D( pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
const string& filename, boost::optional<noiseModel::Diagonal::shared_ptr> model, int maxID, const string& filename, SharedNoiseModel model, int maxID, bool addNoise,
bool addNoise, bool smart) { bool smart, NoiseFormat noiseFormat,
KernelFunctionType kernelFunctionType) {
cout << "Will try to read " << filename << endl; cout << "Will try to read " << filename << endl;
ifstream is(filename.c_str()); ifstream is(filename.c_str());
if (!is) if (!is)
@ -112,13 +185,15 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
if (!(is >> tag)) if (!(is >> tag))
break; break;
if ((tag == "VERTEX2") || (tag == "VERTEX")) { if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
int id; int id;
double x, y, yaw; double x, y, yaw;
is >> id >> x >> y >> yaw; is >> id >> x >> y >> yaw;
// optional filter // optional filter
if (maxID && id >= maxID) if (maxID && id >= maxID)
continue; continue;
initial->insert(id, Pose2(x, y, yaw)); initial->insert(id, Pose2(x, y, yaw));
} }
is.ignore(LINESIZE, '\n'); is.ignore(LINESIZE, '\n');
@ -126,8 +201,18 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
is.clear(); /* clears the end-of-file and error flags */ is.clear(); /* clears the end-of-file and error flags */
is.seekg(0, ios::beg); is.seekg(0, ios::beg);
// Create a sampler with random number generator // If asked, create a sampler with random number generator
Sampler sampler(42u); Sampler sampler;
if (addNoise) {
noiseModel::Diagonal::shared_ptr noise;
if (model)
noise = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!noise)
throw invalid_argument(
"gtsam::load2D: invalid noise model for adding noise"
"(current version assumes diagonal noise model)!");
sampler = Sampler(noise);
}
// Parse the pose constraints // Parse the pose constraints
int id1, id2; int id1, id2;
@ -136,44 +221,27 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
if (!(is >> tag)) if (!(is >> tag))
break; break;
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
|| (tag == "ODOMETRY")) {
// Read transform
double x, y, yaw; double x, y, yaw;
double v1, v2, v3, v4, v5, v6;
is >> id1 >> id2 >> x >> y >> yaw; is >> id1 >> id2 >> x >> y >> yaw;
is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; Pose2 l1Xl2(x, y, yaw);
// Try to guess covariance matrix layout // read noise model
Matrix m(3,3); SharedNoiseModel modelInFile = readNoiseModel(is, smart, noiseFormat,
if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0) kernelFunctionType);
{
// Looks like [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ]
m << v1, v2, v5, v2, v3, v6, v5, v6, v4;
}
else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0)
{
// Looks like [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
m << v1, v2, v3, v2, v4, v5, v3, v5, v6;
}
else
{
throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file");
}
// optional filter // optional filter
if (maxID && (id1 >= maxID || id2 >= maxID)) if (maxID && (id1 >= maxID || id2 >= maxID))
continue; continue;
Pose2 l1Xl2(x, y, yaw); if (!model)
model = modelInFile;
// SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart);
if (!model) {
Vector variances = (Vector(3) << m(0, 0), m(1, 1), m(2, 2));
model = noiseModel::Diagonal::Variances(variances, smart);
}
if (addNoise) if (addNoise)
l1Xl2 = l1Xl2.retract(sampler.sampleNewModel(*model)); l1Xl2 = l1Xl2.retract(sampler.sample());
// Insert vertices if pure odometry file // Insert vertices if pure odometry file
if (!initial->exists(id1)) if (!initial->exists(id1))
@ -182,7 +250,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
initial->insert(id2, initial->at<Pose2>(id1) * l1Xl2); initial->insert(id2, initial->at<Pose2>(id1) * l1Xl2);
NonlinearFactor::shared_ptr factor( NonlinearFactor::shared_ptr factor(
new BetweenFactor<Pose2>(id1, id2, l1Xl2, *model)); new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
graph->push_back(factor); graph->push_back(factor);
} }
@ -207,18 +275,17 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
// In our experience, the x-y covariance on landmark sightings is not very good, so assume // 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. // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty.
if(std::abs(v1 - v3) < 1e-4) if (std::abs(v1 - v3) < 1e-4) {
{
bearing_std = sqrt(v1 / 10.0); bearing_std = sqrt(v1 / 10.0);
range_std = sqrt(v1); range_std = sqrt(v1);
} } else {
else
{
bearing_std = 1; bearing_std = 1;
range_std = 1; range_std = 1;
if (!haveLandmark) { if (!haveLandmark) {
cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n" cout
"non-uniform covariance on LANDMARK measurements in this file." << endl; << "Warning: load2D is a very simple dataset loader and is ignoring the\n"
"non-uniform covariance on LANDMARK measurements in this file."
<< endl;
haveLandmark = true; haveLandmark = true;
} }
} }
@ -265,18 +332,16 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
fstream stream(filename.c_str(), fstream::out); fstream stream(filename.c_str(), fstream::out);
// save poses // save poses
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) {
{
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value); const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y()
<< pose.y() << " " << pose.theta() << endl; << " " << pose.theta() << endl;
} }
// save edges // save edges
Matrix R = model->R(); Matrix R = model->R();
Matrix RR = trans(R) * R; //prod(trans(R),R); Matrix RR = trans(R) * R; //prod(trans(R),R);
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
{
boost::shared_ptr<BetweenFactor<Pose2> > factor = boost::shared_ptr<BetweenFactor<Pose2> > factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_); boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
if (!factor) if (!factor)
@ -284,9 +349,9 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
Pose2 pose = factor->measured().inverse(); Pose2 pose = factor->measured().inverse();
stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " " stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " "
<< pose.x() << " " << pose.y() << " " << pose.theta() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0)
<< RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " "
<< RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl; << RR(0, 2) << " " << RR(1, 2) << endl;
} }
stream.close(); stream.close();
@ -411,7 +476,8 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::shared_ptr measurementNoise =
noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std));
*graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise); *graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range,
measurementNoise);
// Insert poses or points if they do not exist yet // Insert poses or points if they do not exist yet
if (!initial->exists(id1)) if (!initial->exists(id1))
@ -439,13 +505,14 @@ Rot3 openGLFixedRotation(){ // this is due to different convention for cameras i
* 0 0 -1] * 0 0 -1]
*/ */
Matrix3 R_mat = Matrix3::Zero(3, 3); Matrix3 R_mat = Matrix3::Zero(3, 3);
R_mat(0,0) = 1.0; R_mat(1,1) = -1.0; R_mat(2,2) = -1.0; R_mat(0, 0) = 1.0;
R_mat(1, 1) = -1.0;
R_mat(2, 2) = -1.0;
return Rot3(R_mat); return Rot3(R_mat);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) {
{
Rot3 R90 = openGLFixedRotation(); Rot3 R90 = openGLFixedRotation();
Rot3 wRc = (R.inverse()).compose(R90); Rot3 wRc = (R.inverse()).compose(R90);
@ -454,8 +521,7 @@ Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz)
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) {
{
Rot3 R90 = openGLFixedRotation(); Rot3 R90 = openGLFixedRotation();
Rot3 cRw_openGL = R90.compose(R.inverse()); Rot3 cRw_openGL = R90.compose(R.inverse());
Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz)); Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz));
@ -463,18 +529,16 @@ Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz)
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 gtsam2openGL(const Pose3& PoseGTSAM) Pose3 gtsam2openGL(const Pose3& PoseGTSAM) {
{ return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(),
return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), PoseGTSAM.z()); PoseGTSAM.z());
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool readBundler(const string& filename, SfM_data &data) bool readBundler(const string& filename, SfM_data &data) {
{
// Load the data file // Load the data file
ifstream is(filename.c_str(), ifstream::in); ifstream is(filename.c_str(), ifstream::in);
if(!is) if (!is) {
{
cout << "Error in readBundler: can not find the file!!" << endl; cout << "Error in readBundler: can not find the file!!" << endl;
return false; return false;
} }
@ -488,8 +552,7 @@ bool readBundler(const string& filename, SfM_data &data)
is >> nrPoses >> nrPoints; is >> nrPoses >> nrPoints;
// Get the information for the camera poses // Get the information for the camera poses
for( size_t i = 0; i < nrPoses; i++ ) for (size_t i = 0; i < nrPoses; i++) {
{
// Get the focal length and the radial distortion parameters // Get the focal length and the radial distortion parameters
float f, k1, k2; float f, k1, k2;
is >> f >> k1 >> k2; is >> f >> k1 >> k2;
@ -499,20 +562,15 @@ bool readBundler(const string& filename, SfM_data &data)
float r11, r12, r13; float r11, r12, r13;
float r21, r22, r23; float r21, r22, r23;
float r31, r32, r33; float r31, r32, r33;
is >> r11 >> r12 >> r13 is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33;
>> r21 >> r22 >> r23
>> r31 >> r32 >> r33;
// Bundler-OpenGL rotation matrix // Bundler-OpenGL rotation matrix
Rot3 R( Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33);
r11, r12, r13,
r21, r22, r23,
r31, r32, r33);
// Check for all-zero R, in which case quit // Check for all-zero R, in which case quit
if(r11==0 && r12==0 && r13==0) if (r11 == 0 && r12 == 0 && r13 == 0) {
{ cout << "Error in readBundler: zero rotation matrix for pose " << i
cout << "Error in readBundler: zero rotation matrix for pose " << i << endl; << endl;
return false; return false;
} }
@ -526,8 +584,7 @@ bool readBundler(const string& filename, SfM_data &data)
} }
// Get the information for the 3D points // Get the information for the 3D points
for( size_t j = 0; j < nrPoints; j++ ) for (size_t j = 0; j < nrPoints; j++) {
{
SfM_Track track; SfM_Track track;
// Get the 3D position // Get the 3D position
@ -546,8 +603,7 @@ bool readBundler(const string& filename, SfM_data &data)
size_t nvisible = 0; size_t nvisible = 0;
is >> nvisible; is >> nvisible;
for( size_t k = 0; k < nvisible; k++ ) for (size_t k = 0; k < nvisible; k++) {
{
size_t cam_idx = 0, point_idx = 0; size_t cam_idx = 0, point_idx = 0;
float u, v; float u, v;
is >> cam_idx >> point_idx >> u >> v; is >> cam_idx >> point_idx >> u >> v;
@ -562,95 +618,37 @@ bool readBundler(const string& filename, SfM_data &data)
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph,
const kernelFunctionType kernelFunction){ Values& initial, KernelFunctionType kernelFunctionType) {
ifstream is(g2oFile.c_str()); // just call load2D
if (!is){ NonlinearFactorGraph::shared_ptr graph_ptr;
throw std::invalid_argument("File not found!"); Values::shared_ptr initial_ptr;
return false; int maxID = 0;
} bool addNoise = false;
bool smart = true;
// READ INITIAL GUESS FROM G2O FILE boost::tie(graph_ptr, initial_ptr) = load2D(g2oFile, SharedNoiseModel(),
string tag; maxID, addNoise, smart, NoiseFormatG2O, kernelFunctionType);
while (is) { graph = *graph_ptr;
if(! (is >> tag)) initial = *initial_ptr;
break;
if (tag == "VERTEX_SE2" || tag == "VERTEX2") {
int id;
double x, y, yaw;
is >> id >> x >> y >> yaw;
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);
// initial.print("initial guess");
// READ MEASUREMENTS FROM G2O FILE
while (is) {
if(! (is >> tag))
break;
if (tag == "EDGE_SE2" || tag == "EDGE2") {
int id1, id2;
double x, y, yaw;
double I11, I12, I13, I22, I23, I33;
is >> id1 >> id2 >> x >> y >> yaw;
is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33;
Pose2 l1Xl2(x, y, yaw);
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << I11, I22, I33));
switch (kernelFunction) {
{case QUADRATIC:
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
graph.add(factor);
break;}
{case HUBER:
NonlinearFactor::shared_ptr huberFactor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model)));
graph.add(huberFactor);
break;}
{case TUKEY:
NonlinearFactor::shared_ptr tukeyFactor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model)));
graph.add(tukeyFactor);
break;}
}
}
is.ignore(LINESIZE, '\n');
}
// Output which kernel is used
switch (kernelFunction) {
case QUADRATIC:
break;
case HUBER:
std::cout << "Robust kernel: Huber" << std::endl; break;
case TUKEY:
std::cout << "Robust kernel: Tukey" << std::endl; break;
}
return true; return true;
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate){ bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph,
const Values& estimate) {
fstream stream(filename.c_str(), fstream::out); fstream stream(filename.c_str(), fstream::out);
// save poses // save poses
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) {
{
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value); const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
<< pose.y() << " " << pose.theta() << endl; << pose.y() << " " << pose.theta() << endl;
} }
// save edges // save edges
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
{
boost::shared_ptr<BetweenFactor<Pose2> > factor = boost::shared_ptr<BetweenFactor<Pose2> > factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_); boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
if (!factor) if (!factor)
@ -660,25 +658,25 @@ bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, co
boost::shared_ptr<noiseModel::Diagonal> diagonalModel = boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model); boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonalModel) if (!diagonalModel)
throw std::invalid_argument("writeG2o: invalid noise model (current version assumes diagonal noise model)!"); throw std::invalid_argument(
"writeG2o: invalid noise model (current version assumes diagonal noise model)!");
Pose2 pose = factor->measured(); //.inverse(); Pose2 pose = factor->measured(); //.inverse();
stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
<< pose.x() << " " << pose.y() << " " << pose.theta() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " "
<< diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " " << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " "
<< diagonalModel->precision(1) << " " << 0.0 << " " << diagonalModel->precision(2) << endl; << diagonalModel->precision(1) << " " << 0.0 << " "
<< diagonalModel->precision(2) << endl;
} }
stream.close(); stream.close();
return true; return true;
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool readBAL(const string& filename, SfM_data &data) bool readBAL(const string& filename, SfM_data &data) {
{
// Load the data file // Load the data file
ifstream is(filename.c_str(), ifstream::in); ifstream is(filename.c_str(), ifstream::in);
if(!is) if (!is) {
{
cout << "Error in readBAL: can not find the file!!" << endl; cout << "Error in readBAL: can not find the file!!" << endl;
return false; return false;
} }
@ -690,8 +688,7 @@ bool readBAL(const string& filename, SfM_data &data)
data.tracks.resize(nrPoints); data.tracks.resize(nrPoints);
// Get the information for the observations // Get the information for the observations
for( size_t k = 0; k < nrObservations; k++ ) for (size_t k = 0; k < nrObservations; k++) {
{
size_t i = 0, j = 0; size_t i = 0, j = 0;
float u, v; float u, v;
is >> i >> j >> u >> v; is >> i >> j >> u >> v;
@ -699,8 +696,7 @@ bool readBAL(const string& filename, SfM_data &data)
} }
// Get the information for the camera poses // Get the information for the camera poses
for( size_t i = 0; i < nrPoses; i++ ) for (size_t i = 0; i < nrPoses; i++) {
{
// Get the rodriguez vector // Get the rodriguez vector
float wx, wy, wz; float wx, wy, wz;
is >> wx >> wy >> wz; is >> wx >> wy >> wz;
@ -721,8 +717,7 @@ bool readBAL(const string& filename, SfM_data &data)
} }
// Get the information for the 3D points // Get the information for the 3D points
for( size_t j = 0; j < nrPoints; j++ ) for (size_t j = 0; j < nrPoints; j++) {
{
// Get the 3D position // Get the 3D position
float x, y, z; float x, y, z;
is >> x >> y >> z; is >> x >> y >> z;
@ -738,8 +733,7 @@ bool readBAL(const string& filename, SfM_data &data)
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool writeBAL(const string& filename, SfM_data &data) bool writeBAL(const string& filename, SfM_data &data) {
{
// Open the output file // Open the output file
ofstream os; ofstream os;
os.open(filename.c_str()); os.open(filename.c_str());
@ -756,7 +750,8 @@ bool writeBAL(const string& filename, SfM_data &data)
} }
// Write observations // Write observations
os << data.number_cameras() << " " << data.number_tracks() << " " << nrObservations << endl; os << data.number_cameras() << " " << data.number_tracks() << " "
<< nrObservations << endl;
os << endl; os << endl;
for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
@ -767,13 +762,18 @@ bool writeBAL(const string& filename, SfM_data &data)
double u0 = data.cameras[i].calibration().u0(); double u0 = data.cameras[i].calibration().u0();
double v0 = data.cameras[i].calibration().v0(); double v0 = data.cameras[i].calibration().v0();
if(u0 != 0 || v0 != 0){cout<< "writeBAL has not been tested for calibration with nonzero (u0,v0)"<< endl;} if (u0 != 0 || v0 != 0) {
cout
<< "writeBAL has not been tested for calibration with nonzero (u0,v0)"
<< endl;
}
double pixelBALx = track.measurements[k].second.x() - u0; // center of image is the origin double pixelBALx = track.measurements[k].second.x() - u0; // center of image is the origin
double pixelBALy = -(track.measurements[k].second.y() - v0); // center of image is the origin double pixelBALy = -(track.measurements[k].second.y() - v0); // center of image is the origin
Point2 pixelMeasurement(pixelBALx, pixelBALy); Point2 pixelMeasurement(pixelBALx, pixelBALy);
os << i /*camera id*/<< " " << j /*point id*/<< " " os << i /*camera id*/<< " " << j /*point id*/<< " "
<< pixelMeasurement.x() /*u of the pixel*/ << " " << pixelMeasurement.y() /*v of the pixel*/ << endl; << pixelMeasurement.x() /*u of the pixel*/<< " "
<< pixelMeasurement.y() /*v of the pixel*/<< endl;
} }
} }
os << endl; os << endl;
@ -804,7 +804,8 @@ bool writeBAL(const string& filename, SfM_data &data)
return true; return true;
} }
bool writeBALfromValues(const string& filename, const SfM_data &data, Values& values){ bool writeBALfromValues(const string& filename, const SfM_data &data,
Values& values) {
SfM_data dataValues = data; SfM_data dataValues = data;
@ -823,12 +824,16 @@ bool writeBALfromValues(const string& filename, const SfM_data &data, Values& va
if (valuesCameras.size() == dataValues.number_cameras()) { // we only estimated camera poses and calibration if (valuesCameras.size() == dataValues.number_cameras()) { // we only estimated camera poses and calibration
for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
Key cameraKey = i; // symbol('c',i); Key cameraKey = i; // symbol('c',i);
PinholeCamera<Cal3Bundler> camera = values.at<PinholeCamera<Cal3Bundler> >(cameraKey); PinholeCamera<Cal3Bundler> camera =
values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
dataValues.cameras[i] = camera; dataValues.cameras[i] = camera;
} }
} else { } else {
cout << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " << dataValues.number_cameras() cout
<<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl; << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= "
<< dataValues.number_cameras() << ") and values (#cameras "
<< valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!"
<< endl;
return false; return false;
} }
} }
@ -836,8 +841,10 @@ bool writeBALfromValues(const string& filename, const SfM_data &data, Values& va
// Store 3D points in SfM_data // Store 3D points in SfM_data
Values valuesPoints = values.filter<Point3>(); Values valuesPoints = values.filter<Point3>();
if (valuesPoints.size() != dataValues.number_tracks()) { if (valuesPoints.size() != dataValues.number_tracks()) {
cout << "writeBALfromValues: different number of points in SfM_dataValues (#points= " << dataValues.number_tracks() cout
<<") and values (#points " << valuesPoints.size() << ")!!" << endl; << "writeBALfromValues: different number of points in SfM_dataValues (#points= "
<< dataValues.number_tracks() << ") and values (#points "
<< valuesPoints.size() << ")!!" << endl;
} }
for (size_t j = 0; j < dataValues.number_tracks(); j++) { // for each point for (size_t j = 0; j < dataValues.number_tracks(); j++) { // for each point

72
gtsam/slam/dataset.h
View File

@ -52,6 +52,18 @@ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name); GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
#endif #endif
/// Indicates how noise parameters are stored in file
enum NoiseFormat {
NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33
NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix !
NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33
};
enum KernelFunctionType {
KernelFunctionTypeQUADRATIC, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
};
/** /**
* Load TORO 2D Graph * Load TORO 2D Graph
* @param dataset/model pair as constructed by [dataset] * @param dataset/model pair as constructed by [dataset]
@ -60,8 +72,11 @@ GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
* @param smart try to reduce complexity of covariance to cheapest model * @param smart try to reduce complexity of covariance to cheapest model
*/ */
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D( GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
std::pair<std::string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset, std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
int maxID = 0, bool addNoise = false, bool smart = true); bool addNoise = false,
bool smart = true, //
NoiseFormat noiseFormat = NoiseFormatGRAPH,
KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC);
/** /**
* Load TORO 2D Graph * Load TORO 2D Graph
@ -72,18 +87,19 @@ GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> loa
* @param smart try to reduce complexity of covariance to cheapest model * @param smart try to reduce complexity of covariance to cheapest model
*/ */
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D( GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
const std::string& filename, const std::string& filename, SharedNoiseModel model = SharedNoiseModel(),
boost::optional<gtsam::SharedDiagonal> model = boost::optional< int maxID = 0, bool addNoise = false, bool smart = true,
noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false, NoiseFormat noiseFormat = NoiseFormatGRAPH, //
bool smart = true); KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC);
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust( GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
const std::string& filename, const std::string& filename, noiseModel::Base::shared_ptr& model,
gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0); int maxID = 0);
/** save 2d graph */ /** save 2d graph */
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config, GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
const noiseModel::Diagonal::shared_ptr model, const std::string& filename); const Values& config, const noiseModel::Diagonal::shared_ptr model,
const std::string& filename);
/** /**
* Load TORO 3D Graph * Load TORO 3D Graph
@ -91,27 +107,31 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config
GTSAM_EXPORT bool load3D(const std::string& filename); GTSAM_EXPORT bool load3D(const std::string& filename);
/// A measurement with its camera index /// A measurement with its camera index
typedef std::pair<size_t,gtsam::Point2> SfM_Measurement; typedef std::pair<size_t, Point2> SfM_Measurement;
/// Define the structure for the 3D points /// Define the structure for the 3D points
struct SfM_Track struct SfM_Track {
{ Point3 p; ///< 3D position of the point
gtsam::Point3 p; ///< 3D position of the point
float r, g, b; ///< RGB color of the 3D point float r, g, b; ///< RGB color of the 3D point
std::vector<SfM_Measurement> measurements; ///< The 2D image projections (id,(u,v)) std::vector<SfM_Measurement> measurements; ///< The 2D image projections (id,(u,v))
size_t number_measurements() const { return measurements.size();} size_t number_measurements() const {
return measurements.size();
}
}; };
/// Define the structure for the camera poses /// Define the structure for the camera poses
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> SfM_Camera; typedef PinholeCamera<Cal3Bundler> SfM_Camera;
/// Define the structure for SfM data /// Define the structure for SfM data
struct SfM_data struct SfM_data {
{
std::vector<SfM_Camera> cameras; ///< Set of cameras std::vector<SfM_Camera> cameras; ///< Set of cameras
std::vector<SfM_Track> tracks; ///< Sparse set of points std::vector<SfM_Track> tracks; ///< Sparse set of points
size_t number_cameras() const { return cameras.size();} ///< The number of camera poses size_t number_cameras() const {
size_t number_tracks() const { return tracks.size();} ///< The number of reconstructed 3D points return cameras.size();
} ///< The number of camera poses
size_t number_tracks() const {
return tracks.size();
} ///< The number of reconstructed 3D points
}; };
/** /**
@ -130,8 +150,9 @@ GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
* @param graph NonlinearFactor graph storing the measurements (EDGE_SE2). NOTE: information matrix is assumed diagonal. * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2). NOTE: information matrix is assumed diagonal.
* @return initial Values containing the initial guess (VERTEX_SE2) * @return initial Values containing the initial guess (VERTEX_SE2)
*/ */
enum kernelFunctionType { QUADRATIC, HUBER, TUKEY }; GTSAM_EXPORT bool readG2o(const std::string& g2oFile,
GTSAM_EXPORT bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction = QUADRATIC); NonlinearFactorGraph& graph, Values& initial,
KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC);
/** /**
* @brief This function writes a g2o file from * @brief This function writes a g2o file from
@ -140,7 +161,8 @@ GTSAM_EXPORT bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& grap
* @param graph NonlinearFactor graph storing the measurements (EDGE_SE2) * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2)
* @return estimate Values containing the values (VERTEX_SE2) * @return estimate Values containing the values (VERTEX_SE2)
*/ */
GTSAM_EXPORT bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate); 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 * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
@ -171,7 +193,8 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
* assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1 * assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
* @return true if the parsing was successful, false otherwise * @return true if the parsing was successful, false otherwise
*/ */
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, const SfM_data &data, Values& values); GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
const SfM_data &data, Values& values);
/** /**
* @brief This function converts an openGL camera pose to an GTSAM camera pose * @brief This function converts an openGL camera pose to an GTSAM camera pose
@ -214,5 +237,4 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfM_data& db);
*/ */
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_data& db); GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_data& db);
} // namespace gtsam } // namespace gtsam

31
gtsam/slam/tests/testDataset.cpp
View File

@ -40,18 +40,21 @@ TEST(dataSet, findExampleDataFile) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
//TEST( dataSet, load2D) TEST( dataSet, load2D)
//{ {
// ///< The structure where we will save the SfM data ///< The structure where we will save the SfM data
// const string filename = findExampleDataFile("smallGraph.g2o"); const string filename = findExampleDataFile("w100.graph");
// boost::tie(graph,initialGuess) = load2D(filename, boost::none, 10000, NonlinearFactorGraph::shared_ptr graph;
// false, false); Values::shared_ptr initial;
//// print boost::tie(graph, initial) = load2D(filename);
//// EXPECT_LONGS_EQUAL(300,graph->size());
//// print EXPECT_LONGS_EQUAL(100,initial->size());
//// noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3);
//// EXPECT(assert_equal(expected,actual,12)); BetweenFactor<Pose2> expected(1, 0, Pose2(-0.99879,0.0417574,-0.00818381), model);
//} BetweenFactor<Pose2>::shared_ptr actual = boost::dynamic_pointer_cast<
BetweenFactor<Pose2> >(graph->at(0));
EXPECT(assert_equal(expected, *actual));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( dataSet, Balbianello) TEST( dataSet, Balbianello)
@ -119,7 +122,7 @@ TEST( dataSet, readG2oHuber)
const string g2oFile = findExampleDataFile("pose2example"); const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph actualGraph; NonlinearFactorGraph actualGraph;
Values actualValues; Values actualValues;
readG2o(g2oFile, actualGraph, actualValues, HUBER); readG2o(g2oFile, actualGraph, actualValues, KernelFunctionTypeHUBER);
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); 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); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel);
@ -146,7 +149,7 @@ TEST( dataSet, readG2oTukey)
const string g2oFile = findExampleDataFile("pose2example"); const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph actualGraph; NonlinearFactorGraph actualGraph;
Values actualValues; Values actualValues;
readG2o(g2oFile, actualGraph, actualValues, TUKEY); readG2o(g2oFile, actualGraph, actualValues, KernelFunctionTypeTUKEY);
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); 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); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);