diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index db5d7d76a..1611d78f6 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -167,10 +167,11 @@ GTSAM_EXPORT GraphAndValues load2D( * @param kernelFunctionType whether to wrap the noise model in a robust kernel * @return graph and initial values */ -GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, - SharedNoiseModel model = SharedNoiseModel(), size_t maxIndex = 0, bool addNoise = - false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, // - KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); +GTSAM_EXPORT GraphAndValues +load2D(const std::string& filename, SharedNoiseModel model = SharedNoiseModel(), + size_t maxIndex = 0, bool addNoise = false, bool smart = true, + NoiseFormat noiseFormat = NoiseFormatAUTO, // + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /** save 2d graph */ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, @@ -185,8 +186,9 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, * @param kernelFunctionType whether to wrap the noise model in a robust kernel * @return graph and initial values */ -GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D = false, - KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); +GTSAM_EXPORT GraphAndValues +readG2o(const std::string& g2oFile, const bool is3D = false, + KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); /** * @brief This function writes a g2o file from diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index e044dd2c1..2b25a4369 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -253,17 +253,27 @@ bool writeBAL(string filename, gtsam::SfmData& data); gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); -pair load2D( - string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, - bool addNoise, bool smart); -pair load2D( - string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, - bool addNoise); -pair load2D( - string filename, gtsam::noiseModel::Diagonal* model, int maxIndex); -pair load2D( - string filename, gtsam::noiseModel::Diagonal* model); -pair load2D(string filename); +enum NoiseFormat { + NoiseFormatG2O, + NoiseFormatTORO, + NoiseFormatGRAPH, + NoiseFormatCOV, + NoiseFormatAUTO +}; + +enum KernelFunctionType { + KernelFunctionTypeNONE, + KernelFunctionTypeHUBER, + KernelFunctionTypeTUKEY +}; + +std::pair load2D( + string filename, gtsam::noiseModel::Diagonal* model = nullptr, + size_t maxIndex = 0, bool addNoise = false, bool smart = true, + gtsam::NoiseFormat noiseFormat = gtsam::NoiseFormatAUTO, + gtsam::KernelFunctionType kernelFunctionType = + gtsam::KernelFunctionTypeNONE); + void save2D(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, string filename); @@ -290,9 +300,10 @@ gtsam::BetweenFactorPose3s parse3DFactors(string filename); pair load3D(string filename); -pair readG2o(string filename); -pair readG2o(string filename, - bool is3D); +pair readG2o( + string filename, const bool is3D = false, + gtsam::KernelFunctionType kernelFunctionType = + gtsam::KernelFunctionTypeNONE); void writeG2o(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& estimate, string filename);