40 lines
935 B
C++
40 lines
935 B
C++
/**
|
|
* @file Simulated3D.cpp
|
|
* @brief measurement functions and derivatives for simulated 3D robot
|
|
* @author Alex Cunningham
|
|
**/
|
|
|
|
#include <gtsam/slam/Simulated3D.h>
|
|
#include <gtsam/nonlinear/LieValues-inl.h>
|
|
#include <gtsam/nonlinear/TupleValues-inl.h>
|
|
|
|
namespace gtsam {
|
|
|
|
using namespace simulated3D;
|
|
INSTANTIATE_LIE_CONFIG(PointKey)
|
|
INSTANTIATE_LIE_CONFIG(PoseKey)
|
|
INSTANTIATE_TUPLE_CONFIG2(PoseValues, PointValues)
|
|
|
|
namespace simulated3D {
|
|
|
|
Point3 prior (const Point3& x, boost::optional<Matrix&> H) {
|
|
if (H) *H = eye(3);
|
|
return x;
|
|
}
|
|
|
|
Point3 odo(const Point3& x1, const Point3& x2,
|
|
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
|
|
if (H1) *H1 = -1 * eye(3);
|
|
if (H2) *H2 = eye(3);
|
|
return x2 - x1;
|
|
}
|
|
|
|
Point3 mea(const Point3& x, const Point3& l,
|
|
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
|
|
if (H1) *H1 = -1 * eye(3);
|
|
if (H2) *H2 = eye(3);
|
|
return l - x;
|
|
}
|
|
|
|
}} // namespace gtsam::simulated3D
|