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

Normalized quaternions before converting to Rot3 to account for limited precision in text files.

release/4.3a0
Frank dellaert 2020-07-09 17:16:11 -04:00
parent 5ab16c8b51
commit 4960f75595
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/slam/dataset.cpp
View File

@ -511,6 +511,11 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
stream.close(); stream.close();
} }
/* ************************************************************************* */
static Rot3 NormalizedRot3(double w, double x, double y, double z) {
const double norm = sqrt(w * w + x * x + y * y + z * z), f = 1.0 / norm;
return Rot3::Quaternion(f * w, f * x, f * y, f * z);
}
/* ************************************************************************* */ /* ************************************************************************* */
std::map<Key, Pose3> parse3DPoses(const string& filename) { std::map<Key, Pose3> parse3DPoses(const string& filename) {
ifstream is(filename.c_str()); ifstream is(filename.c_str());
@ -535,14 +540,15 @@ std::map<Key, Pose3> parse3DPoses(const string& filename) {
Key id; Key id;
double x, y, z, qx, qy, qz, qw; double x, y, z, qx, qy, qz, qw;
ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); poses.emplace(id, Pose3(NormalizedRot3(qw, qx, qy, qz), {x, y, z}));
} }
} }
return poses; return poses;
} }
/* ************************************************************************* */ /* ************************************************************************* */
BetweenFactorPose3s parse3DFactors(const string& filename, BetweenFactorPose3s parse3DFactors(
const string& filename,
const noiseModel::Diagonal::shared_ptr& corruptingNoise) { 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);
@ -592,7 +598,7 @@ 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); auto R12 = NormalizedRot3(qw, qx, qy, qz);
if (sampler) { if (sampler) {
R12 = R12.retract(sampler->sample()); R12 = R12.retract(sampler->sample());
} }