1/**
2 * @file RelativeElevationFactor.cpp
3 *
4 * @date Aug 17, 2012
5 * @author Alex Cunningham
6 */
7
8#include <gtsam_unstable/slam/RelativeElevationFactor.h>
9
10namespace gtsam {
11
12/* ************************************************************************* */
13RelativeElevationFactor::RelativeElevationFactor(Key poseKey, Key pointKey, double measured,
14 const SharedNoiseModel& model)
15: Base(model, poseKey, pointKey), measured_(measured)
16{
17}
18
19/* ************************************************************************* */
20Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& point,
21 OptionalMatrixType H1, OptionalMatrixType H2) const {
22 double hx = pose.z() - point.z();
23 if (H1) {
24 *H1 = Matrix::Zero(rows: 1,cols: 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(rows: 1,cols: 3);
33 (*H2)(0, 2) = -1.0;
34 }
35 return (Vector(1) << hx - measured_).finished();
36}
37
38/* ************************************************************************* */
39bool RelativeElevationFactor::equals(const NonlinearFactor& expected, double tol) const {
40 const This *e = dynamic_cast<const This*> (&expected);
41 return e != nullptr && Base::equals(f: *e, tol) && std::abs(x: this->measured_ - e->measured_) < tol;
42}
43
44/* ************************************************************************* */
45void RelativeElevationFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
46 std::cout << s << "RelativeElevationFactor, relative elevation = " << measured_ << std::endl;
47 Base::print(s: "", keyFormatter);
48}
49/* ************************************************************************* */
50
51} // \namespace gtsam
52
53
54