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

Cleanup

release/4.3a0
Frank Dellaert 2021-01-21 15:21:29 -05:00
parent 1b36c7497e
commit 79d42479d0
2 changed files with 22 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
gtsam/slam/OrientedPlane3Factor.cpp
View File

@ -27,7 +27,7 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose,
Matrix33 predicted_H_plane, error_H_predicted; Matrix33 predicted_H_plane, error_H_predicted;
OrientedPlane3 predicted_plane = plane.transform(pose, H2 ? &predicted_H_plane : nullptr, OrientedPlane3 predicted_plane = plane.transform(pose, H2 ? &predicted_H_plane : nullptr,
H1 ? &predicted_H_pose : nullptr); H1 ? &predicted_H_pose : nullptr);
Vector3 err = predicted_plane.error( Vector3 err = predicted_plane.errorVector(
measured_p_, (H1 || H2) ? &error_H_predicted : nullptr); measured_p_, (H1 || H2) ? &error_H_predicted : nullptr);
// Apply the chain rule to calculate the derivatives. // Apply the chain rule to calculate the derivatives.
@ -44,7 +44,7 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose,
//*************************************************************************** //***************************************************************************
void OrientedPlane3DirectionPrior::print(const string& s, void OrientedPlane3DirectionPrior::print(const string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
cout << "Prior Factor on " << keyFormatter(landmarkKey_) << "\n"; cout << "Prior Factor on " << keyFormatter(key()) << "\n";
measured_p_.print("Measured Plane"); measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
@ -58,26 +58,17 @@ bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
} }
//*************************************************************************** //***************************************************************************
Vector OrientedPlane3DirectionPrior::evaluateError(
Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane, const OrientedPlane3& plane, boost::optional<Matrix&> H) const {
boost::optional<Matrix&> H) const { Unit3 n_hat_p = measured_p_.normal();
Unit3 n_hat_q = plane.normal();
Matrix2 H_p;
Vector e = n_hat_p.error(n_hat_q, H ? &H_p : nullptr);
if (H) { if (H) {
Matrix H_p;
Unit3 n_hat_p = measured_p_.normal();
Unit3 n_hat_q = plane.normal();
Vector e = n_hat_p.error(n_hat_q, H_p);
H->resize(2, 3); H->resize(2, 3);
H->block<2, 2>(0, 0) << H_p; *H << H_p, Z_2x1;
H->block<2, 1>(0, 2) << Z_2x1;
return e;
} else {
Unit3 n_hat_p = measured_p_.normal();
Unit3 n_hat_q = plane.normal();
Vector e = n_hat_p.error(n_hat_q);
return e;
} }
return e;
}
} }
} // namespace gtsam

31
gtsam/slam/OrientedPlane3Factor.h
View File

@ -16,15 +16,11 @@ namespace gtsam {
* Factor to measure a planar landmark from a given pose * Factor to measure a planar landmark from a given pose
*/ */
class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> { class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
protected:
protected:
Vector measured_coeffs_;
OrientedPlane3 measured_p_; OrientedPlane3 measured_p_;
typedef NoiseModelFactor2<Pose3, OrientedPlane3> Base; typedef NoiseModelFactor2<Pose3, OrientedPlane3> Base;
public: public:
/// Constructor /// Constructor
OrientedPlane3Factor() { OrientedPlane3Factor() {
} }
@ -37,11 +33,9 @@ public:
* @param landmarkKey Key or symbol for unknown planar landmark * @param landmarkKey Key or symbol for unknown planar landmark
* @return the transformed plane * @return the transformed plane
*/ */
OrientedPlane3Factor(const Vector& z, const SharedGaussian& noiseModel, OrientedPlane3Factor(const Vector4& z, const SharedGaussian& noiseModel,
Key poseKey, Key landmarkKey) Key poseKey, Key landmarkKey)
: Base(noiseModel, poseKey, landmarkKey), measured_coeffs_(z) { : Base(noiseModel, poseKey, landmarkKey), measured_p_(z) {}
measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3));
}
/// print /// print
void print(const std::string& s = "OrientedPlane3Factor", void print(const std::string& s = "OrientedPlane3Factor",
@ -55,24 +49,21 @@ public:
}; };
// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
class OrientedPlane3DirectionPrior: public NoiseModelFactor1<OrientedPlane3> { class OrientedPlane3DirectionPrior : public NoiseModelFactor1<OrientedPlane3> {
protected: protected:
OrientedPlane3 measured_p_; /// measured plane parameters OrientedPlane3 measured_p_; /// measured plane parameters
Key landmarkKey_;
typedef NoiseModelFactor1<OrientedPlane3> Base; typedef NoiseModelFactor1<OrientedPlane3> Base;
public:
public:
typedef OrientedPlane3DirectionPrior This; typedef OrientedPlane3DirectionPrior This;
/// Constructor /// Constructor
OrientedPlane3DirectionPrior() { OrientedPlane3DirectionPrior() {
} }
/// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol /// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol
OrientedPlane3DirectionPrior(Key key, const Vector&z, OrientedPlane3DirectionPrior(Key key, const Vector4& z,
const SharedGaussian& noiseModel) : const SharedGaussian& noiseModel)
Base(noiseModel, key), landmarkKey_(key) { : Base(noiseModel, key), measured_p_(z) {}
measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3));
}
/// print /// print
void print(const std::string& s = "OrientedPlane3DirectionPrior", void print(const std::string& s = "OrientedPlane3DirectionPrior",