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

Using optional sampler outside loop

release/4.3a0
Binit Shah 2020-05-30 17:35:22 -04:00
parent 537155dd05
commit 5556db5420
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
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>
@ -546,6 +547,11 @@ BetweenFactorPose3s parse3DFactors(const string& filename,
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];
@ -587,9 +593,8 @@ BetweenFactorPose3s parse3DFactors(const string& filename,
SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
auto R12 = Rot3::Quaternion(qw, qx, qy, qz); auto R12 = Rot3::Quaternion(qw, qx, qy, qz);
if (corruptingNoise) { if (sampler) {
Sampler sampler(corruptingNoise); R12 = R12.retract(sampler->sample());
R12 = R12.retract(sampler.sample());
} }
factors.emplace_back(new BetweenFactor<Pose3>( factors.emplace_back(new BetweenFactor<Pose3>(