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

Update print method of OrientedPlane3Factor

release/4.3a0
David Wisth 2021-02-15 13:15:38 +00:00
parent a62bdd45e8
commit 0ad488c567
1 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -14,7 +14,9 @@ namespace gtsam {
//***************************************************************************
void OrientedPlane3Factor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << "OrientedPlane3Factor Factor on " << keyFormatter(key2()) << "\n";
cout << s << (s == "" ? "" : "\n");
cout << "OrientedPlane3Factor Factor (" << keyFormatter(key1()) << ", "
<< keyFormatter(key2()) << ")\n";
measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: ");
}
@ -25,8 +27,9 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose,
boost::optional<Matrix&> H2) const {
Matrix36 predicted_H_pose;
Matrix33 predicted_H_plane, error_H_predicted;
OrientedPlane3 predicted_plane = plane.transform(pose, H2 ? &predicted_H_plane : nullptr,
H1 ? &predicted_H_pose : nullptr);
OrientedPlane3 predicted_plane = plane.transform(pose,
H2 ? &predicted_H_plane : nullptr, H1 ? &predicted_H_pose : nullptr);
Vector3 err = predicted_plane.errorVector(
measured_p_, (H1 || H2) ? &error_H_predicted : nullptr);
@ -44,7 +47,8 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose,
//***************************************************************************
void OrientedPlane3DirectionPrior::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << "Prior Factor on " << keyFormatter(key()) << "\n";
cout << s << (s == "" ? "" : "\n");
cout << s << "Prior Factor on " << keyFormatter(key()) << "\n";
measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: ");
}