30 lines
764 B
C++
30 lines
764 B
C++
// Christian Potthast
|
|
// Simulated2D Odometry
|
|
|
|
#include "NonlinearFactor.h"
|
|
#include "simulated2D.h"
|
|
|
|
namespace simulated2D {
|
|
|
|
struct Simulated2DMeasurement: public gtsam::NonlinearFactor2<VectorConfig,
|
|
PoseKey, Vector, PointKey, Vector> {
|
|
|
|
Vector z_;
|
|
|
|
Simulated2DMeasurement(const Vector& z, double sigma, const std::string& j1,
|
|
const std::string& j2) :
|
|
z_(z), gtsam::NonlinearFactor2<VectorConfig, PoseKey, Vector, PointKey,
|
|
Vector>(sigma, j1, j2) {
|
|
}
|
|
|
|
Vector evaluateError(const Vector& x1, const Vector& x2, boost::optional<
|
|
Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
|
|
if (H1) *H1 = Dmea1(x1, x2);
|
|
if (H2) *H2 = Dmea2(x1, x2);
|
|
return mea(x1, x2) - z_;
|
|
}
|
|
|
|
};
|
|
|
|
} // namespace simulated2D
|