32 using namespace boost;
33 using namespace std::placeholders;
34 using namespace gtsam;
62 Point2 local(2.0, 3.0), global(-1.0, 2.0);
66 Matrix actualDT, actualDL, actualDF;
69 Matrix numericalDT, numericalDL, numericalDF;
70 numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
71 std::bind(
evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
72 global,
trans, local, 1
e-5);
73 numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
74 std::bind(
evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
75 global,
trans, local, 1
e-5);
76 numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
77 std::bind(
evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
78 global,
trans, local, 1
e-5);
98 Matrix actualDT, actualDL, actualDF;
101 Matrix numericalDT, numericalDL, numericalDF;
102 numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
103 std::bind(
evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
104 global,
trans, local, 1
e-5);
105 numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
106 std::bind(
evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
107 global,
trans, local, 1
e-5);
108 numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
109 std::bind(
evaluateError_, tc, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
110 global,
trans, local, 1
e-5);
121 Point2 local1(2.0, 2.0), local2(4.0, 5.0),
122 global1(-1.0, 5.0), global2(2.0, 3.0);
142 double error_gain = 1000.0;
187 double error_gain = 1000.0;
215 Point2 idealForeign =
trans.inverse().transformFrom(local);
223 double error_gain = 1000.0;