Loading...
Searching...
No Matches
OrientedPlane3Factor.h
/*
* @file OrientedPlane3Factor.cpp
* @brief OrientedPlane3 Factor class
* @author Alex Trevor
* @date December 22, 2013
*/
#pragma once
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, OrientedPlane3> {
protected:
OrientedPlane3 measured_p_;
typedef NoiseModelFactorN<Pose3, OrientedPlane3> Base;
public:
OrientedPlane3Factor() {
}
~OrientedPlane3Factor() override {}
OrientedPlane3Factor(const Vector4& z, const SharedGaussian& noiseModel,
Key poseKey, Key landmarkKey)
: Base(noiseModel, poseKey, landmarkKey), measured_p_(z) {}
void print(const std::string& s = "OrientedPlane3Factor",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
Vector evaluateError(
const Pose3& pose, const OrientedPlane3& plane,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const override;
};
// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<OrientedPlane3> {
protected:
OrientedPlane3 measured_p_;
typedef NoiseModelFactorN<OrientedPlane3> Base;
public:
typedef OrientedPlane3DirectionPrior This;
OrientedPlane3DirectionPrior() {
}
OrientedPlane3DirectionPrior(Key key, const Vector4& z,
const SharedGaussian& noiseModel)
: Base(noiseModel, key), measured_p_(z) {}
void print(const std::string& s = "OrientedPlane3DirectionPrior",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
Vector evaluateError(const OrientedPlane3& plane,
boost::optional<Matrix&> H = boost::none) const override;
};
} // gtsam
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::noiseModel
All noise models live in the noiseModel namespace.
Definition LossFunctions.cpp:27
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::OrientedPlane3
Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
Definition OrientedPlane3.h:36
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NoiseModelFactorN< Pose3, OrientedPlane3 >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::NoiseModelFactorN< OrientedPlane3 >::key
Key key() const
Definition NonlinearFactor.h:518
gtsam::OrientedPlane3Factor
Factor to measure a planar landmark from a given pose.
Definition OrientedPlane3Factor.h:18
gtsam::OrientedPlane3Factor::OrientedPlane3Factor
OrientedPlane3Factor(const Vector4 &z, const SharedGaussian &noiseModel, Key poseKey, Key landmarkKey)
Constructor with measured plane (a,b,c,d) coefficients.
Definition OrientedPlane3Factor.h:36
gtsam::OrientedPlane3Factor::OrientedPlane3Factor
OrientedPlane3Factor()
Constructor.
Definition OrientedPlane3Factor.h:25
gtsam::OrientedPlane3DirectionPrior::OrientedPlane3DirectionPrior
OrientedPlane3DirectionPrior(Key key, const Vector4 &z, const SharedGaussian &noiseModel)
Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol.
Definition OrientedPlane3Factor.h:64
gtsam::OrientedPlane3DirectionPrior::Base
NoiseModelFactorN< OrientedPlane3 > Base
measured plane parameters
Definition OrientedPlane3Factor.h:55
gtsam::OrientedPlane3DirectionPrior::OrientedPlane3DirectionPrior
OrientedPlane3DirectionPrior()
Constructor.
Definition OrientedPlane3Factor.h:60