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>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2649,6 +2585,70 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<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
static boost::optional<Vector> checkIfDiagonal(const Matrix M) {
boost::optional<Vector> checkIfDiagonal(const Matrix M) {
size_t m = M.rows(), n = M.cols();
// check all non-diagonal entries
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) {
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;
if (smart)
diagonal = checkIfDiagonal(R);
if (diagonal) return Diagonal::Sigmas(reciprocal(*diagonal),true);
else return shared_ptr(new Gaussian(R.rows(),R));
if (diagonal)
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();
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;
if (smart)
variances = checkIfDiagonal(covariance);
if (variances) return Diagonal::Variances(*variances,true);
else return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
if (variances)
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);
/**
* 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.
* @param covariance The square covariance Matrix
@ -865,6 +872,9 @@ namespace gtsam {
}
};
// Helper function
boost::optional<Vector> checkIfDiagonal(const Matrix M);
} // namespace noiseModel
/** 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));
}
/* ************************************************************************* */
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 )
{

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
*/
#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/BetweenFactor.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;
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
throw std::invalid_argument(
throw
std::invalid_argument(
"gtsam::findExampleDataFile could not find a matching file in\n"
SOURCE_TREE_DATASET_DIR " or\n"
INSTALLED_DATASET_DIR " named\n" +
@ -78,7 +79,8 @@ string createRewrittenFileName(const string& 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();
}
@ -88,15 +90,86 @@ string createRewrittenFileName(const string& name) {
/* ************************************************************************* */
pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
pair<string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset,
int maxID, bool addNoise, bool smart) {
return load2D(dataset.first, dataset.second, maxID, addNoise, smart);
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);
}
/* ************************************************************************* */
// 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(
const string& filename, boost::optional<noiseModel::Diagonal::shared_ptr> model, int maxID,
bool addNoise, bool smart) {
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)
@ -112,13 +185,15 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
if (!(is >> tag))
break;
if ((tag == "VERTEX2") || (tag == "VERTEX")) {
if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (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');
@ -126,8 +201,18 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
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);
// If asked, create a sampler with random number generator
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
int id1, id2;
@ -136,44 +221,27 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
if (!(is >> tag))
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 v1, v2, v3, v4, v5, v6;
is >> id1 >> id2 >> x >> y >> yaw;
is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
Pose2 l1Xl2(x, y, yaw);
// Try to guess covariance matrix layout
Matrix m(3,3);
if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0)
{
// 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");
}
// read noise model
SharedNoiseModel modelInFile = readNoiseModel(is, smart, noiseFormat,
kernelFunctionType);
// optional filter
if (maxID && (id1 >= maxID || id2 >= maxID))
continue;
Pose2 l1Xl2(x, y, yaw);
// 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 (!model)
model = modelInFile;
if (addNoise)
l1Xl2 = l1Xl2.retract(sampler.sampleNewModel(*model));
l1Xl2 = l1Xl2.retract(sampler.sample());
// Insert vertices if pure odometry file
if (!initial->exists(id1))
@ -182,7 +250,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
initial->insert(id2, initial->at<Pose2>(id1) * l1Xl2);
NonlinearFactor::shared_ptr factor(
new BetweenFactor<Pose2>(id1, id2, l1Xl2, *model));
new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
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
// 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);
range_std = sqrt(v1);
}
else
{
} else {
bearing_std = 1;
range_std = 1;
if (!haveLandmark) {
cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n"
"non-uniform covariance on LANDMARK measurements in this file." << endl;
cout
<< "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;
}
}
@ -265,18 +332,16 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
fstream stream(filename.c_str(), fstream::out);
// 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);
stream << "VERTEX2 " << key_value.key << " " << pose.x() << " "
<< pose.y() << " " << pose.theta() << endl;
stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y()
<< " " << pose.theta() << endl;
}
// save edges
Matrix R = model->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::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
if (!factor)
@ -284,9 +349,9 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
Pose2 pose = factor->measured().inverse();
stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " "
<< pose.x() << " " << pose.y() << " " << pose.theta() << " "
<< RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " "
<< RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl;
<< pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0)
<< " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " "
<< RR(0, 2) << " " << RR(1, 2) << endl;
}
stream.close();
@ -411,7 +476,8 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
noiseModel::Diagonal::shared_ptr measurementNoise =
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
if (!initial->exists(id1))
@ -439,13 +505,14 @@ Rot3 openGLFixedRotation(){ // this is due to different convention for cameras i
* 0 0 -1]
*/
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);
}
/* ************************************************************************* */
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 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 cRw_openGL = R90.compose(R.inverse());
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)
{
return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), PoseGTSAM.z());
Pose3 gtsam2openGL(const Pose3& PoseGTSAM) {
return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(),
PoseGTSAM.z());
}
/* ************************************************************************* */
bool readBundler(const string& filename, SfM_data &data)
{
bool readBundler(const string& filename, SfM_data &data) {
// Load the data file
ifstream is(filename.c_str(), ifstream::in);
if(!is)
{
if (!is) {
cout << "Error in readBundler: can not find the file!!" << endl;
return false;
}
@ -488,8 +552,7 @@ bool readBundler(const string& filename, SfM_data &data)
is >> nrPoses >> nrPoints;
// 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
float f, k1, k2;
is >> f >> k1 >> k2;
@ -499,20 +562,15 @@ bool readBundler(const string& filename, SfM_data &data)
float r11, r12, r13;
float r21, r22, r23;
float r31, r32, r33;
is >> r11 >> r12 >> r13
>> r21 >> r22 >> r23
>> r31 >> r32 >> r33;
is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33;
// Bundler-OpenGL rotation matrix
Rot3 R(
r11, r12, r13,
r21, r22, r23,
r31, r32, r33);
Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33);
// Check for all-zero R, in which case quit
if(r11==0 && r12==0 && r13==0)
{
cout << "Error in readBundler: zero rotation matrix for pose " << i << endl;
if (r11 == 0 && r12 == 0 && r13 == 0) {
cout << "Error in readBundler: zero rotation matrix for pose " << i
<< endl;
return false;
}
@ -526,8 +584,7 @@ bool readBundler(const string& filename, SfM_data &data)
}
// 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;
// Get the 3D position
@ -546,8 +603,7 @@ bool readBundler(const string& filename, SfM_data &data)
size_t nvisible = 0;
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;
float 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,
const kernelFunctionType kernelFunction){
bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph,
Values& initial, KernelFunctionType kernelFunctionType) {
ifstream is(g2oFile.c_str());
if (!is){
throw std::invalid_argument("File not found!");
return false;
}
// READ INITIAL GUESS FROM G2O FILE
string tag;
while (is) {
if(! (is >> tag))
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;
}
// 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){
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)
{
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_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
boost::shared_ptr<BetweenFactor<Pose2> > factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
if (!factor)
@ -660,25 +658,25 @@ bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, co
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)!");
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;
<< diagonalModel->precision(1) << " " << 0.0 << " "
<< diagonalModel->precision(2) << endl;
}
stream.close();
return true;
}
/* ************************************************************************* */
bool readBAL(const string& filename, SfM_data &data)
{
bool readBAL(const string& filename, SfM_data &data) {
// Load the data file
ifstream is(filename.c_str(), ifstream::in);
if(!is)
{
if (!is) {
cout << "Error in readBAL: can not find the file!!" << endl;
return false;
}
@ -690,8 +688,7 @@ bool readBAL(const string& filename, SfM_data &data)
data.tracks.resize(nrPoints);
// 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;
float 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
for( size_t i = 0; i < nrPoses; i++ )
{
for (size_t i = 0; i < nrPoses; i++) {
// Get the rodriguez vector
float 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
for( size_t j = 0; j < nrPoints; j++ )
{
for (size_t j = 0; j < nrPoints; j++) {
// Get the 3D position
float 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
ofstream os;
os.open(filename.c_str());
@ -756,7 +750,8 @@ bool writeBAL(const string& filename, SfM_data &data)
}
// Write observations
os << data.number_cameras() << " " << data.number_tracks() << " " << nrObservations << endl;
os << data.number_cameras() << " " << data.number_tracks() << " "
<< nrObservations << endl;
os << endl;
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 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 pixelBALy = -(track.measurements[k].second.y() - v0); // center of image is the origin
Point2 pixelMeasurement(pixelBALx, pixelBALy);
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;
@ -804,7 +804,8 @@ bool writeBAL(const string& filename, SfM_data &data)
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;
@ -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
for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
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;
}
} else {
cout << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " << dataValues.number_cameras()
<<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl;
cout
<< "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= "
<< dataValues.number_cameras() << ") and values (#cameras "
<< valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!"
<< endl;
return false;
}
}
@ -836,8 +841,10 @@ bool writeBALfromValues(const string& filename, const SfM_data &data, Values& va
// Store 3D points in SfM_data
Values valuesPoints = values.filter<Point3>();
if (valuesPoints.size() != dataValues.number_tracks()) {
cout << "writeBALfromValues: different number of points in SfM_dataValues (#points= " << dataValues.number_tracks()
<<") and values (#points " << valuesPoints.size() << ")!!" << endl;
cout
<< "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

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);
#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
* @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
*/
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
std::pair<std::string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset,
int maxID = 0, bool addNoise = false, bool smart = true);
std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
bool addNoise = false,
bool smart = true, //
NoiseFormat noiseFormat = NoiseFormatGRAPH,
KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC);
/**
* 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
*/
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
const std::string& filename,
boost::optional<gtsam::SharedDiagonal> model = boost::optional<
noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false,
bool smart = true);
const std::string& filename, SharedNoiseModel model = SharedNoiseModel(),
int maxID = 0, bool addNoise = false, bool smart = true,
NoiseFormat noiseFormat = NoiseFormatGRAPH, //
KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC);
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
const std::string& filename,
gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0);
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);
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
const Values& config, const noiseModel::Diagonal::shared_ptr model,
const std::string& filename);
/**
* 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);
/// 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
struct SfM_Track
{
gtsam::Point3 p; ///< 3D position of the point
struct SfM_Track {
Point3 p; ///< 3D position of the point
float r, g, b; ///< RGB color of the 3D point
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
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> SfM_Camera;
typedef PinholeCamera<Cal3Bundler> SfM_Camera;
/// Define the structure for SfM data
struct SfM_data
{
struct SfM_data {
std::vector<SfM_Camera> cameras; ///< Set of cameras
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_tracks() const { return tracks.size();} ///< The number of reconstructed 3D points
size_t number_cameras() const {
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.
* @return initial Values containing the initial guess (VERTEX_SE2)
*/
enum kernelFunctionType { QUADRATIC, HUBER, TUKEY };
GTSAM_EXPORT bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction = QUADRATIC);
GTSAM_EXPORT bool readG2o(const std::string& g2oFile,
NonlinearFactorGraph& graph, Values& initial,
KernelFunctionType kernelFunctionType = KernelFunctionTypeQUADRATIC);
/**
* @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)
* @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
@ -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
* @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
@ -214,5 +237,4 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfM_data& db);
*/
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_data& db);
} // namespace gtsam

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

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