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, void OrientedPlane3Factor::print(const string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
cout << "OrientedPlane3Factor Factor on " << keyFormatter(landmarkKey_) << "\n"; cout << "OrientedPlane3Factor Factor on " << keyFormatter(key2()) << "\n";
measured_p_.print("Measured Plane"); measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }

17
gtsam/slam/OrientedPlane3Factor.h
View File

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