1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
|
class UnaryFactor: public NoiseModelFactor1<Pose2> {
double mx_, my_; ///< X and Y measurements
public:
UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
Vector evaluateError(const Pose2& q,
boost::optional<Matrix&> H = boost::none) const override {
const Rot2& R = q.rotation();
if (H) (*H) = (gtsam::Matrix(2, 3) <<
R.c(), -R.s(), 0.0,
R.s(), R.c(), 0.0).finished();
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
}
};
|
