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;
OrientedPlane3 predicted_plane = plane.transform(pose, H2 ? &predicted_H_plane : nullptr,
H1 ? &predicted_H_pose : nullptr);
Vector3 err = predicted_plane.error(
Vector3 err = predicted_plane.errorVector(
measured_p_, (H1 || H2) ? &error_H_predicted : nullptr);
// Apply the chain rule to calculate the derivatives.
@ -44,7 +44,7 @@ Vector OrientedPlane3Factor::evaluateError(const Pose3& pose,
//***************************************************************************
void OrientedPlane3DirectionPrior::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << "Prior Factor on " << keyFormatter(landmarkKey_) << "\n";
cout << "Prior Factor on " << keyFormatter(key()) << "\n";
measured_p_.print("Measured Plane");
this->noiseModel_->print(" noise model: ");
}
@ -58,26 +58,17 @@ bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
}
//***************************************************************************
Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane,
boost::optional<Matrix&> H) const {
Vector OrientedPlane3DirectionPrior::evaluateError(
const OrientedPlane3& plane, 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) {
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->block<2, 2>(0, 0) << H_p;
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;
*H << H_p, Z_2x1;
}
}
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
*/
class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
protected:
Vector measured_coeffs_;
protected:
OrientedPlane3 measured_p_;
typedef NoiseModelFactor2<Pose3, OrientedPlane3> Base;
public:
public:
/// Constructor
OrientedPlane3Factor() {
}
@ -37,11 +33,9 @@ public:
* @param landmarkKey Key or symbol for unknown planar landmark
* @return the transformed plane
*/
OrientedPlane3Factor(const Vector& z, const SharedGaussian& noiseModel,
OrientedPlane3Factor(const Vector4& 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));
}
: Base(noiseModel, poseKey, landmarkKey), measured_p_(z) {}
/// print
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
class OrientedPlane3DirectionPrior: public NoiseModelFactor1<OrientedPlane3> {
protected:
OrientedPlane3 measured_p_; /// measured plane parameters
Key landmarkKey_;
class OrientedPlane3DirectionPrior : public NoiseModelFactor1<OrientedPlane3> {
protected:
OrientedPlane3 measured_p_; /// measured plane parameters
typedef NoiseModelFactor1<OrientedPlane3> Base;
public:
public:
typedef OrientedPlane3DirectionPrior This;
/// Constructor
OrientedPlane3DirectionPrior() {
}
/// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol
OrientedPlane3DirectionPrior(Key key, const Vector&z,
const SharedGaussian& noiseModel) :
Base(noiseModel, key), landmarkKey_(key) {
measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3));
}
OrientedPlane3DirectionPrior(Key key, const Vector4& z,
const SharedGaussian& noiseModel)
: Base(noiseModel, key), measured_p_(z) {}
/// print
void print(const std::string& s = "OrientedPlane3DirectionPrior",