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 /* ************************************************************************* */
21  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
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 
const gtsam::Key poseKey
Point3 r1() const
first column
Definition: Rot3M.cpp:224
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
noiseModel::Diagonal::shared_ptr model
Matrix expected
Definition: testMatrix.cpp:974
const gtsam::Key pointKey
Point3 point(10, 0,-5)
Vector evaluateError(const Pose3 &pose, const Point3 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Eigen::VectorXd Vector
Definition: Vector.h:38
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Point3 r2() const
second column
Definition: Rot3M.cpp:227
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
Point3 r3() const
third column
Definition: Rot3M.cpp:230
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
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
traits
Definition: chartTesting.h:28
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Factor representing a known relative altitude in global frame.
const G double tol
Definition: Group.h:83
Vector3 Point3
Definition: Point3.h:35
Point3 measured
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
#define abs(x)
Definition: datatypes.h:17
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition: Pose3.cpp:266
double z() const
get z
Definition: Pose3.h:279


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:43:51