19 lines
641 B
C++
19 lines
641 B
C++
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
|
|
{
|
|
const double cosTheta = cos(q.theta());
|
|
const double sinTheta = sin(q.theta());
|
|
if (H) (*H) = (gtsam::Matrix(2, 3) <<
|
|
cosTheta, -sinTheta, 0.0,
|
|
sinTheta, cosTheta, 0.0).finished();
|
|
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
|
|
}
|
|
};
|