RelativeElevationFactor.cpp
Go to the documentation of this file.
1 
9 
10 namespace gtsam {
11 
12 /* ************************************************************************* */
14  const SharedNoiseModel& model)
15 : Base(model, poseKey, pointKey), measured_(measured)
16 {
17 }
18 
19 /* ************************************************************************* */
22  double hx = pose.z() - point.z();
23  if (H1) {
24  *H1 = Matrix::Zero(1,6);
25  // Use bottom row of rotation matrix for derivative of translation
26  (*H1)(0, 3) = pose.rotation().r1().z();
27  (*H1)(0, 4) = pose.rotation().r2().z();
28  (*H1)(0, 5) = pose.rotation().r3().z();
29  }
30 
31  if (H2) {
32  *H2 = Matrix::Zero(1,3);
33  (*H2)(0, 2) = -1.0;
34  }
35  return (Vector(1) << hx - measured_).finished();
36 }
37 
38 /* ************************************************************************* */
40  const This *e = dynamic_cast<const This*> (&expected);
41  return e != nullptr && Base::equals(*e, tol) && std::abs(this->measured_ - e->measured_) < tol;
42 }
43 
44 /* ************************************************************************* */
45 void RelativeElevationFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
46  std::cout << s << "RelativeElevationFactor, relative elevation = " << measured_ << std::endl;
47  Base::print("", keyFormatter);
48 }
49 /* ************************************************************************* */
50 
51 } // \namespace gtsam
52 
53 
gtsam::RelativeElevationFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: RelativeElevationFactor.cpp:45
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
pointKey
const gtsam::Key pointKey
Definition: testRelativeElevationFactor.cpp:25
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
measured
Point2 measured(-17, 30)
gtsam::Factor
Definition: Factor.h:70
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::RelativeElevationFactor::measured_
double measured_
Definition: RelativeElevationFactor.h:31
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
gtsam::RelativeElevationFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, OptionalMatrixType H1, OptionalMatrixType H2) const override
Definition: RelativeElevationFactor.cpp:20
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
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::Pose3
Definition: Pose3.h:37
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:74
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:82
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam
traits
Definition: SFMdata.h:40
gtsam::RelativeElevationFactor::RelativeElevationFactor
RelativeElevationFactor()
Definition: RelativeElevationFactor.h:41
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:69
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:56
abs
#define abs(x)
Definition: datatypes.h:17
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
RelativeElevationFactor.h
Factor representing a known relative altitude in global frame.
gtsam::RelativeElevationFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
Definition: RelativeElevationFactor.cpp:39


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:13:06