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

Include corrupting noise param to parse3DFactors

release/4.3a0
Binit Shah 2020-05-17 15:44:11 -04:00
parent a76c0a20ee
commit 3a1653dd23
2 changed files with 17 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/slam/dataset.cpp
View File

@ -541,10 +541,18 @@ 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());
if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);
// If asked, create a sampler with random number generator
Sampler sampler;
if (corruptingNoise) {
sampler = Sampler(corruptingNoise);
}
std::vector<BetweenFactor<Pose3>::shared_ptr> factors;
while (!is.eof()) {
char buf[LINESIZE];
@ -585,8 +593,13 @@ BetweenFactorPose3s parse3DFactors(const string& filename) {
mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal
SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
auto R12 = Rot3::Quaternion(qw, qx, qy, qz);
if (corruptingNoise) {
R12 = R12.retract(sampler.sample());
}
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;

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.
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.
GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);