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

Avoided extra conversions to quaternions

release/4.3a0
Frank dellaert 2020-07-09 17:27:06 -04:00
parent 4960f75595
commit a484c910ab
1 changed files with 11 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/slam/dataset.cpp
View File

@ -442,11 +442,11 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value); auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
if (!p) continue; if (!p) continue;
const Pose3& pose = p->value(); const Pose3& pose = p->value();
Point3 t = pose.translation(); const Point3 t = pose.translation();
Rot3 R = pose.rotation(); const auto q = pose.rotation().toQuaternion();
stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " " << t.y() << " " << t.z() stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " "
<< " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " "
<< " " << R.toQuaternion().w() << endl; << q.z() << " " << q.w() << endl;
} }
// save edges (2D or 3D) // save edges (2D or 3D)
@ -486,13 +486,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
throw invalid_argument("writeG2o: invalid noise model!"); throw invalid_argument("writeG2o: invalid noise model!");
} }
Matrix Info = gaussianModel->R().transpose() * gaussianModel->R(); Matrix Info = gaussianModel->R().transpose() * gaussianModel->R();
Pose3 pose3D = factor3D->measured(); const Pose3 pose3D = factor3D->measured();
Point3 p = pose3D.translation(); const Point3 p = pose3D.translation();
Rot3 R = pose3D.rotation(); const auto q = pose3D.rotation().toQuaternion();
stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2()
stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2() << " " << " " << p.x() << " " << p.y() << " " << p.z() << " " << q.x()
<< p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x() << " " << q.y() << " " << q.z() << " " << q.w();
<< " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w();
Matrix InfoG2o = I_6x6; Matrix InfoG2o = I_6x6;
InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation