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

Got rid of extra keys

release/4.3a0
Frank Dellaert 2021-01-21 14:52:58 -05:00
parent 564c8bf98d
commit 68887f2da6
2 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -14,7 +14,7 @@ namespace gtsam {
//***************************************************************************
void OrientedPlane3Factor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << "OrientedPlane3Factor Factor on " << keyFormatter(landmarkKey_) << "\n";
cout << "OrientedPlane3Factor Factor on " << keyFormatter(key2()) << "\n";
measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: ");
}

15
gtsam/slam/OrientedPlane3Factor.h
View File

@ -18,8 +18,6 @@ namespace gtsam {
class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
protected:
Key poseKey_;
Key landmarkKey_;
Vector measured_coeffs_;
OrientedPlane3 measured_p_;
@ -32,11 +30,16 @@ public:
}
virtual ~OrientedPlane3Factor() {}
/// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol
/** Constructor with measured plane (a,b,c,d) coefficients
* @param z measured plane (a,b,c,d) coefficients as 4D vector
* @param noiseModel noiseModel Gaussian noise model
* @param poseKey Key or symbol for unknown pose
* @param landmarkKey Key or symbol for unknown planar landmark
* @return the transformed plane
*/
OrientedPlane3Factor(const Vector& z, const SharedGaussian& noiseModel,
const Key& pose, const Key& landmark) :
Base(noiseModel, pose, landmark), poseKey_(pose), landmarkKey_(landmark), measured_coeffs_(
z) {
Key poseKey, Key landmarkKey)
: Base(noiseModel, poseKey, landmarkKey), measured_coeffs_(z) {
measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3));
}