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

Merge pull request #312 from borglab/feature/parsefactors_w_corruptingnoise 

Include corrupting noise param to parse3DFactors
release/4.3a0
Akshay Krishnan 2020-06-19 23:02:06 -07:00 committed by GitHub
parent 62ec057c5e 5556db5420
commit cb55d81fb5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 16 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/slam/dataset.cpp
View File

@ -37,6 +37,7 @@
#include <boost/assign/list_inserter.hpp> #include <boost/assign/list_inserter.hpp>
#include <boost/filesystem/operations.hpp> #include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp> #include <boost/filesystem/path.hpp>
#include <boost/optional.hpp>
#include <cmath> #include <cmath>
#include <fstream> #include <fstream>
@ -541,10 +542,16 @@ std::map<Key, Pose3> parse3DPoses(const string& filename) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
BetweenFactorPose3s parse3DFactors(const string& filename) { BetweenFactorPose3s parse3DFactors(const string& filename,
const noiseModel::Diagonal::shared_ptr& corruptingNoise) {
ifstream is(filename.c_str()); ifstream is(filename.c_str());
if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename); if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);
boost::optional<Sampler> sampler;
if (corruptingNoise) {
sampler = Sampler(corruptingNoise);
}
std::vector<BetweenFactor<Pose3>::shared_ptr> factors; std::vector<BetweenFactor<Pose3>::shared_ptr> factors;
while (!is.eof()) { while (!is.eof()) {
char buf[LINESIZE]; char buf[LINESIZE];
@ -585,8 +592,13 @@ BetweenFactorPose3s parse3DFactors(const string& filename) {
mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal
SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
auto R12 = Rot3::Quaternion(qw, qx, qy, qz);
if (sampler) {
R12 = R12.retract(sampler->sample());
}
factors.emplace_back(new BetweenFactor<Pose3>( factors.emplace_back(new BetweenFactor<Pose3>(
id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); id1, id2, Pose3(R12, {x, y, z}), model));
} }
} }
return factors; return factors;

3
gtsam/slam/dataset.h
View File

@ -159,7 +159,8 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
/// Parse edges in 3D TORO graph file into a set of BetweenFactors. /// Parse edges in 3D TORO graph file into a set of BetweenFactors.
using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>; using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>;
GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename); GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename,
const noiseModel::Diagonal::shared_ptr& corruptingNoise=nullptr);
/// Parse vertices in 3D TORO graph file into a map of Pose3s. /// Parse vertices in 3D TORO graph file into a map of Pose3s.
GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename); GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);