20 #include <gtsam_unstable/nonlinear/expressionTesting.h>
22 #include <type_traits>
25 using namespace gtsam;
36 typedef Eigen::Vector3d
type;
42 return origin + Eigen::Vector3d(2.0 *
d[0], 3.0 *
d[1], 0.0);
52 template<>
struct dimension<
ProjectionChart> :
public std::integral_constant<int, 2> {};
60 Eigen::Vector3d pval(1.0, 2.0, 3.0);
67 Eigen::Vector3d pstandard =
point.value(standard);
68 Eigen::Vector3d pcustom =
point.value(custom);
85 std::shared_ptr<GaussianFactor> gfstandard =
f.linearize(standard);
86 std::shared_ptr<JacobianFactor> jfstandard =
87 std::dynamic_pointer_cast<JacobianFactor>(gfstandard);
89 typedef std::pair<Eigen::MatrixXd, Eigen::VectorXd> Jacobian;
90 Jacobian Jstandard = jfstandard->jacobianUnweighted();
93 std::shared_ptr<GaussianFactor> gfcustom =
f.linearize(custom);
94 std::shared_ptr<JacobianFactor> jfcustom =
95 std::dynamic_pointer_cast<JacobianFactor>(gfcustom);
97 Eigen::MatrixXd expectedJacobian = Eigen::MatrixXd::Zero(3,2);
98 expectedJacobian(0,0) = 2.0;
99 expectedJacobian(1,1) = 3.0;
100 Jacobian Jcustom = jfcustom->jacobianUnweighted();
104 const double fd_step = 1
e-5;
105 const double tolerance = 1
e-5;