1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file Simulated3D.h
14 * @brief measurement functions and derivatives for simulated 3D robot
15 * @author Alex Cunningham
16 **/
17
18// \callgraph
19
20#pragma once
21
22#include <gtsam/base/Matrix.h>
23#include <gtsam/geometry/Point3.h>
24#include <gtsam/linear/VectorValues.h>
25#include <gtsam/nonlinear/NonlinearFactor.h>
26#include "gtsam/base/OptionalJacobian.h"
27
28// \namespace
29
30namespace gtsam {
31namespace simulated3D {
32
33/**
34 * This is a linear SLAM domain where both poses and landmarks are
35 * 3D points, without rotation. The structure and use is based on
36 * the simulated2D domain.
37 */
38
39/**
40 * Prior on a single pose
41 */
42Point3 prior(const Point3& x, OptionalJacobian<3,3> H = OptionalNone) {
43 if (H) *H = I_3x3;
44 return x;
45}
46
47/**
48 * odometry between two poses
49 */
50Point3 odo(const Point3& x1, const Point3& x2,
51 OptionalJacobian<3,3> H1 = OptionalNone,
52 OptionalJacobian<3,3> H2 = OptionalNone) {
53 if (H1) *H1 = -1 * I_3x3;
54 if (H2) *H2 = I_3x3;
55 return x2 - x1;
56}
57
58/**
59 * measurement between landmark and pose
60 */
61Point3 mea(const Point3& x, const Point3& l,
62 OptionalJacobian<3,3> H1 = OptionalNone,
63 OptionalJacobian<3,3> H2 = OptionalNone) {
64 if (H1) *H1 = -1 * I_3x3;
65 if (H2) *H2 = I_3x3;
66 return l - x;
67}
68
69/**
70 * A prior factor on a single linear robot pose
71 */
72struct PointPrior3D: public NoiseModelFactor1<Point3> {
73 using NoiseModelFactor1<Point3>::evaluateError;
74
75 Point3 measured_; ///< The prior pose value for the variable attached to this factor
76
77 /**
78 * Constructor for a prior factor
79 * @param measured is the measured/prior position for the pose
80 * @param model is the measurement model for the factor (Dimension: 3)
81 * @param key is the key for the pose
82 */
83 PointPrior3D(const Point3& measured, const SharedNoiseModel& model, Key key) :
84 NoiseModelFactorN<Point3> (model, key), measured_(measured) {
85 }
86
87 /**
88 * Evaluates the error at a given value of x,
89 * with optional derivatives.
90 * @param x is the current value of the variable
91 * @param H is an optional Jacobian matrix (Dimension: 3x3)
92 * @return Vector error between prior value and x (Dimension: 3)
93 */
94 Vector evaluateError(const Point3& x, OptionalMatrixType H) const override {
95 return simulated3D::prior(x, H) - measured_;
96 }
97};
98
99/**
100 * Models a linear 3D measurement between 3D points
101 */
102struct Simulated3DMeasurement: public NoiseModelFactorN<Point3, Point3> {
103 using NoiseModelFactor2<Point3, Point3>::evaluateError;
104
105 Point3 measured_; ///< Linear displacement between a pose and landmark
106
107 /**
108 * Creates a measurement factor with a given measurement
109 * @param measured is the measurement, a linear displacement between poses and landmarks
110 * @param model is a measurement model for the factor (Dimension: 3)
111 * @param poseKey is the pose key of the robot
112 * @param pointKey is the point key for the landmark
113 */
114 Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, Key i, Key j) :
115 NoiseModelFactorN<Point3, Point3>(model, i, j), measured_(measured) {}
116
117 /**
118 * Error function with optional derivatives
119 * @param x1 a robot pose value
120 * @param x2 a landmark point value
121 * @param H1 is an optional Jacobian matrix in terms of x1 (Dimension: 3x3)
122 * @param H2 is an optional Jacobian matrix in terms of x2 (Dimension: 3x3)
123 * @return vector error between measurement and prediction (Dimension: 3)
124 */
125 Vector evaluateError(const Point3& x1, const Point3& x2,
126 OptionalMatrixType H1, OptionalMatrixType H2) const override {
127 return mea(x: x1, l: x2, H1, H2) - measured_;
128 }
129};
130
131}} // namespace simulated3D
132