-
Notifications
You must be signed in to change notification settings - Fork 94
/
GPSPose2Factor.h
60 lines (44 loc) · 1.65 KB
/
GPSPose2Factor.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
/**
* @file GPSPose2Factor.h
* @brief 2D 'GPS' like factor for Pose2
* @date Nov 8, 2016
* @author Jing Dong
*/
/**
* A simple 2D 'GPS' like factor
* The factor contains a X-Y position measurement (mx, my) for a Pose2, but no rotation information
* The error vector will be [x-mx, y-my]'
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Pose2.h>
// you can custom namespace (if needed for your project)
namespace gtsamexamples {
class GPSPose2Factor: public gtsam::NoiseModelFactor1<gtsam::Pose2> {
private:
// measurement information
double mx_, my_;
public:
/**
* Constructor
* @param poseKey associated pose varible key
* @param model noise model for GPS snesor, in X-Y
* @param m Point2 measurement
*/
GPSPose2Factor(gtsam::Key poseKey, const gtsam::Point2 m, gtsam::SharedNoiseModel model) :
gtsam::NoiseModelFactor1<gtsam::Pose2>(model, poseKey), mx_(m.x()), my_(m.y()) {}
// error function
// @param p the pose in Pose2
// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const gtsam::Pose2& p, boost::optional<gtsam::Matrix&> H = boost::none) const {
// note that use boost optional like a pointer
// only calculate jacobian matrix when non-null pointer exists
if (H) *H = (gtsam::Matrix23() << 1.0, 0.0, 0.0,
0.0, 1.0, 0.0).finished();
// return error vector
return (gtsam::Vector2() << p.x() - mx_, p.y() - my_).finished();
}
};
} // namespace gtsamexamples