diff --git a/.cproject b/.cproject index d759a7a65..d5a6ca4d4 100644 --- a/.cproject +++ b/.cproject @@ -2185,70 +2185,6 @@ true true - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j6 -j8 - testAntiFactor.run - true - true - true - - - make - -j6 -j8 - testBetweenFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - make -j2 @@ -2649,6 +2585,70 @@ true true + + make + -j5 + testAntiFactor.run + true + true + true + + + make + -j5 + testBetweenFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + make -j2 diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 45314e023..fcaec3afa 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -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 checkIfDiagonal(const Matrix M) { +boost::optional 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 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 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 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 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))); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index e709ea543..55d01c92f 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -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 @@ -864,6 +871,9 @@ namespace gtsam { ar & boost::serialization::make_nvp("noise_", const_cast(noise_)); } }; + + // Helper function + boost::optional checkIfDiagonal(const Matrix M); } // namespace noiseModel diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index d68caeabe..df0f8a774 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -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 ) { diff --git a/gtsam/nonlinear/tests/CMakeLists.txt b/gtsam/nonlinear/tests/CMakeLists.txt index 378d93de4..69a3700f2 100644 --- a/gtsam/nonlinear/tests/CMakeLists.txt +++ b/gtsam/nonlinear/tests/CMakeLists.txt @@ -1 +1 @@ -gtsamAddTestsGlob(nonlinear "*.cpp" "" "gtsam") +gtsamAddTestsGlob(nonlinear "test*.cpp" "" "gtsam") diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index cb2d44224..b397ab60d 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -16,18 +16,18 @@ * @brief utility functions for loading datasets */ -#include -#include -#include - -#include - -#include -#include -#include #include #include #include +#include +#include +#include + +#include + +#include +#include +#include using namespace std; namespace fs = boost::filesystem; @@ -43,7 +43,7 @@ string findExampleDataFile(const string& name) { // Search source tree and installed location vector rootsToSearch; rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt - rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt + rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Defined by CMake, see gtsam/gtsam/CMakeLists.txt // Search for filename as given, and with .graph and .txt extensions vector namesToSearch; @@ -55,32 +55,34 @@ string findExampleDataFile(const string& name) { // Find first name that exists BOOST_FOREACH(const fs::path& root, rootsToSearch) { BOOST_FOREACH(const fs::path& name, namesToSearch) { - if(fs::is_regular_file(root / name)) + if (fs::is_regular_file(root / name)) return (root / name).string(); } } // If we did not return already, then we did not find the file - 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" + - name + ", " + name + ".graph, or " + name + ".txt"); + 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" + + name + ", " + name + ".graph, or " + name + ".txt"); } /* ************************************************************************* */ string createRewrittenFileName(const string& name) { // Search source tree and installed location - if(!exists(fs::path(name))) { - throw std::invalid_argument( - "gtsam::createRewrittenFileName could not find a matching file in\n" - + name); + if (!exists(fs::path(name))) { + throw std::invalid_argument( + "gtsam::createRewrittenFileName could not find a matching file in\n" + + name); } - fs::path p(name); - fs::path newpath = fs::path(p.parent_path().string()) / fs::path(p.stem().string() + "-rewritten.txt" ); + fs::path p(name); + 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 load2D( - pair > dataset, - int maxID, bool addNoise, bool smart) { - return load2D(dataset.first, dataset.second, maxID, addNoise, smart); + pair 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 load2D( - const string& filename, boost::optional 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) @@ -109,16 +182,18 @@ pair load2D( // load the poses while (is) { - if(! (is >> tag)) + 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,54 +201,47 @@ pair 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(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; bool haveLandmark = false; while (is) { - if(! (is >> tag)) + 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 load2D( initial->insert(id2, initial->at(id1) * l1Xl2); NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, l1Xl2, *model)); + new BetweenFactor(id1, id2, l1Xl2, model)); graph->push_back(factor); } @@ -203,22 +271,21 @@ pair load2D( // Convert x,y to bearing,range bearing = std::atan2(lmy, lmx); - range = std::sqrt(lmx*lmx + lmy*lmy); + range = std::sqrt(lmx * lmx + lmy * lmy); // In our experience, the x-y covariance on landmark sightings is not very good, so assume // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty. - 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; + 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; haveLandmark = true; } } @@ -244,7 +311,7 @@ pair load2D( initial->insert(id1, Pose2()); if (!initial->exists(L(id2))) { Pose2 pose = initial->at(id1); - Point2 local(cos(bearing)*range,sin(bearing)*range); + Point2 local(cos(bearing) * range, sin(bearing) * range); Point2 global = pose.transform_from(local); initial->insert(L(id2), global); } @@ -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(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 factor_, graph) - { + BOOST_FOREACH(boost::shared_ptr factor_, graph) { boost::shared_ptr > factor = boost::dynamic_pointer_cast >(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,14 +476,15 @@ pair load2D_robust( noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std)); - *graph += BearingRangeFactor(id1, id2, bearing, range, measurementNoise); + *graph += BearingRangeFactor(id1, id2, bearing, range, + measurementNoise); // Insert poses or points if they do not exist yet if (!initial->exists(id1)) initial->insert(id1, Pose2()); if (!initial->exists(id2)) { Pose2 pose = initial->at(id1); - Point2 local(cos(bearing)*range,sin(bearing)*range); + Point2 local(cos(bearing) * range, sin(bearing) * range); Point2 global = pose.transform_from(local); initial->insert(id2, global); } @@ -427,69 +493,66 @@ pair load2D_robust( } cout << "load2D read a graph file with " << initial->size() - << " vertices and " << graph->nrFactors() << " factors" << endl; + << " vertices and " << graph->nrFactors() << " factors" << endl; return make_pair(graph, initial); } /* ************************************************************************* */ -Rot3 openGLFixedRotation(){ // this is due to different convention for cameras in gtsam and openGL +Rot3 openGLFixedRotation() { // this is due to different convention for cameras in gtsam and openGL /* R = [ 1 0 0 * 0 -1 0 * 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; + 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; 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); + Rot3 wRc = (R.inverse()).compose(R90); // Our camera-to-world translation wTc = -R'*t - return Pose3 (wRc, R.unrotate(Point3(-tx,-ty,-tz))); + return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -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)); + Rot3 cRw_openGL = R90.compose(R.inverse()); + Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz)); return Pose3(cRw_openGL, t_openGL); } /* ************************************************************************* */ -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) - { + ifstream is(filename.c_str(), ifstream::in); + if (!is) { cout << "Error in readBundler: can not find the file!!" << endl; return false; } // Ignore the first line char aux[500]; - is.getline(aux,500); + is.getline(aux, 500); // Get the number of camera poses and 3D points size_t nrPoses, nrPoints; 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; } @@ -520,38 +578,36 @@ bool readBundler(const string& filename, SfM_data &data) float tx, ty, tz; is >> tx >> ty >> tz; - Pose3 pose = openGL2gtsam(R,tx,ty,tz); + Pose3 pose = openGL2gtsam(R, tx, ty, tz); - data.cameras.push_back(SfM_Camera(pose,K)); + data.cameras.push_back(SfM_Camera(pose, K)); } // 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 float x, y, z; is >> x >> y >> z; - track.p = Point3(x,y,z); + track.p = Point3(x, y, z); // Get the color information float r, g, b; is >> r >> g >> b; - track.r = r/255.f; - track.g = g/255.f; - track.b = b/255.f; + track.r = r / 255.f; + track.g = g / 255.f; + track.b = b / 255.f; // Now get the visibility information 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; - track.measurements.push_back(make_pair(cam_idx,Point2(u,-v))); + track.measurements.push_back(make_pair(cam_idx, Point2(u, -v))); } data.tracks.push_back(track); @@ -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(id1, id2, l1Xl2, model)); - graph.add(factor); - break;} - {case HUBER: - NonlinearFactor::shared_ptr huberFactor(new BetweenFactor(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(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(key_value.value); stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; } // save edges - BOOST_FOREACH(boost::shared_ptr factor_, graph) - { + BOOST_FOREACH(boost::shared_ptr factor_, graph) { boost::shared_ptr > factor = boost::dynamic_pointer_cast >(factor_); if (!factor) @@ -660,25 +658,25 @@ bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, co boost::shared_ptr diagonalModel = boost::dynamic_pointer_cast(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) - { + ifstream is(filename.c_str(), ifstream::in); + if (!is) { cout << "Error in readBAL: can not find the file!!" << endl; return false; } @@ -690,44 +688,41 @@ 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; - data.tracks[j].measurements.push_back(make_pair(i,Point2(u,-v))); + data.tracks[j].measurements.push_back(make_pair(i, Point2(u, -v))); } // 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; - Rot3 R = Rot3::rodriguez(wx, wy, wz);// BAL-OpenGL rotation matrix + Rot3 R = Rot3::rodriguez(wx, wy, wz); // BAL-OpenGL rotation matrix // Get the translation vector float tx, ty, tz; is >> tx >> ty >> tz; - Pose3 pose = openGL2gtsam(R,tx,ty,tz); + Pose3 pose = openGL2gtsam(R, tx, ty, tz); // Get the focal length and the radial distortion parameters float f, k1, k2; is >> f >> k1 >> k2; Cal3Bundler K(f, k1, k2); - data.cameras.push_back(SfM_Camera(pose,K)); + data.cameras.push_back(SfM_Camera(pose, K)); } // 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; SfM_Track& track = data.tracks[j]; - track.p = Point3(x,y,z); + track.p = Point3(x, y, z); track.r = 0.4f; track.g = 0.4f; track.b = 0.4f; @@ -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()); @@ -750,49 +744,55 @@ bool writeBAL(const string& filename, SfM_data &data) } // Write the number of camera poses and 3D points - size_t nrObservations=0; - for (size_t j = 0; j < data.number_tracks(); j++){ + size_t nrObservations = 0; + for (size_t j = 0; j < data.number_tracks(); j++) { nrObservations += data.tracks[j].number_measurements(); } // 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 + for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j SfM_Track track = data.tracks[j]; - for(size_t k = 0; k < track.number_measurements(); k++){ // for each observation of the 3D point j + for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j size_t i = track.measurements[k].first; // camera id 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 + 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; + os << i /*camera id*/<< " " << j /*point id*/<< " " + << pixelMeasurement.x() /*u of the pixel*/<< " " + << pixelMeasurement.y() /*v of the pixel*/<< endl; } } os << endl; // Write cameras - for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera + for (size_t i = 0; i < data.number_cameras(); i++) { // for each camera Pose3 poseGTSAM = data.cameras[i].pose(); Cal3Bundler cameraCalibration = data.cameras[i].calibration(); Pose3 poseOpenGL = gtsam2openGL(poseGTSAM); - os << Rot3::Logmap(poseOpenGL.rotation()) << endl; - os << poseOpenGL.translation().vector() << endl; - os << cameraCalibration.fx() << endl; - os << cameraCalibration.k1() << endl; - os << cameraCalibration.k2() << endl; + os << Rot3::Logmap(poseOpenGL.rotation()) << endl; + os << poseOpenGL.translation().vector() << endl; + os << cameraCalibration.fx() << endl; + os << cameraCalibration.k1() << endl; + os << cameraCalibration.k2() << endl; os << endl; } // Write the points - 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 Point3 point = data.tracks[j].p; os << point.x() << endl; os << point.y() << endl; @@ -804,48 +804,55 @@ 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; // Store poses or cameras in SfM_data Values valuesPoses = values.filter(); - if( valuesPoses.size() == dataValues.number_cameras() ){ // we only estimated camera poses - for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera - Key poseKey = symbol('x',i); + if (valuesPoses.size() == dataValues.number_cameras()) { // we only estimated camera poses + for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera + Key poseKey = symbol('x', i); Pose3 pose = values.at(poseKey); Cal3Bundler K = dataValues.cameras[i].calibration(); PinholeCamera camera(pose, K); dataValues.cameras[i] = camera; } } else { - Values valuesCameras = values.filter< PinholeCamera >(); - 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 + Values valuesCameras = values.filter >(); + 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 camera = values.at >(cameraKey); + PinholeCamera camera = + values.at >(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; + } else { + cout + << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " + << dataValues.number_cameras() << ") and values (#cameras " + << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" + << endl; return false; } } // Store 3D points in SfM_data Values valuesPoints = values.filter(); - 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; + 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; } - 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 Key pointKey = P(j); - if(values.exists(pointKey)){ + if (values.exists(pointKey)) { Point3 point = values.at(pointKey); dataValues.tracks[j].p = point; - }else{ + } else { dataValues.tracks[j].r = 1.0; dataValues.tracks[j].g = 0.0; dataValues.tracks[j].b = 0.0; @@ -861,7 +868,7 @@ Values initialCamerasEstimate(const SfM_data& db) { Values initial; size_t i = 0; // NO POINTS: j = 0; BOOST_FOREACH(const SfM_Camera& camera, db.cameras) - initial.insert(i++, camera); + initial.insert(i++, camera); return initial; } @@ -869,9 +876,9 @@ Values initialCamerasAndPointsEstimate(const SfM_data& db) { Values initial; size_t i = 0, j = 0; BOOST_FOREACH(const SfM_Camera& camera, db.cameras) - initial.insert((i++), camera); + initial.insert((i++), camera); BOOST_FOREACH(const SfM_Track& track, db.tracks) - initial.insert(P(j++), track.p); + initial.insert(P(j++), track.p); return initial; } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 854daa237..771328d6d 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -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 load2D( - std::pair > dataset, - int maxID = 0, bool addNoise = false, bool smart = true); + std::pair 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 loa * @param smart try to reduce complexity of covariance to cheapest model */ GTSAM_EXPORT std::pair load2D( - const std::string& filename, - boost::optional 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 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 SfM_Measurement; +typedef std::pair SfM_Measurement; /// Define the structure for the 3D points -struct SfM_Track -{ - gtsam::Point3 p; ///< 3D position of the point - float r,g,b; ///< RGB color of the 3D point +struct SfM_Track { + Point3 p; ///< 3D position of the point + float r, g, b; ///< RGB color of the 3D point std::vector 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 SfM_Camera; +typedef PinholeCamera SfM_Camera; /// Define the structure for SfM data -struct SfM_data -{ - std::vector cameras; ///< Set of cameras +struct SfM_data { + std::vector cameras; ///< Set of cameras std::vector 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 diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 1ab4c9bb4..8789c4085 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -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 expected(1, 0, Pose2(-0.99879,0.0417574,-0.00818381), model); + BetweenFactor::shared_ptr actual = boost::dynamic_pointer_cast< + BetweenFactor >(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);