19 using namespace gtsam;
30 TEST(TestLinearContainerFactor, generic_jacobian_factor) {
34 0.0, 2.63624).finished();
36 -0.0455167, -0.0443573,
37 -0.0222154, -0.102489).finished();
63 TEST(TestLinearContainerFactor, jacobian_factor_withlinpoints) {
67 0.0, 2.63624).finished();
69 -0.0455167, -0.0443573,
70 -0.0222154, -0.102489).finished();
117 TEST(TestLinearContainerFactor, generic_hessian_factor) {
120 Matrix G13 = (
Matrix(1, 3) << 3.0, 6.0, 9.0).finished();
123 0.0, 6.0).finished();
125 1.0, 2.0, 4.0).finished();
129 0.0, 0.0, 9.0).finished();
138 G11, G12, G13,
g1, G22, G23,
g2, G33, g3,
f);
155 TEST(TestLinearContainerFactor, hessian_factor_withlinpoints) {
162 0.0, 0.0, 9.0).finished();
166 4.0, 6.0).finished();
171 0.0, 0.6).finished();
179 G << G11, G12, Matrix::Zero(2,3), G22;
183 Values linearizationPoint, expLinPoints;
186 expLinPoints = linearizationPoint;
215 Vector dv = (
Vector(5) << 3.0, 4.0, 0.5, 1.0, 2.0).finished();
220 Vector g1_prime = g_prime.head(3);
221 Vector g2_prime = g_prime.tail(2);
222 double f_prime =
f + dv.transpose() *
G.selfadjointView<
Eigen::Upper>() * dv - 2.0 * dv.transpose() *
g;
228 TEST(TestLinearContainerFactor, Creation) {
241 exp_values = full_values;
254 TEST(TestLinearContainerFactor, jacobian_relinearize)
277 double expected_error = betweenFactor.
error(linpoint2);
278 double actual_error = jacobianContainer.
error(linpoint2);
288 TEST(TestLinearContainerFactor, hessian_relinearize)
311 double expected_error = betweenFactor.
error(linpoint2);
312 double actual_error = hessianContainer.
error(linpoint2);
322 TEST(TestLinearContainerFactor, Rekey) {
324 auto nonlinear_factor =
325 std::make_shared<gtsam::BetweenFactor<gtsam::Point3>>(
335 nonlinear_factor->linearize(linearization_pt), linearization_pt);
338 std::map<gtsam::Key, gtsam::Key> key_map;
344 auto lcf_factor_rekeyed = lcf_factor.
rekey(key_map);
348 std::static_pointer_cast<LinearContainerFactor>(lcf_factor_rekeyed);
349 CHECK(lcf_factor_rekey_ptr);
353 for (
auto key : linearization_pt.keys()) {
354 linearization_pt_rekeyed.
insert(key_map.at(
key), linearization_pt.at(
key));
364 TEST(TestLinearContainerFactor, Rekey2) {
366 auto nonlinear_factor =
367 std::make_shared<gtsam::BetweenFactor<gtsam::Point3>>(
377 nonlinear_factor->linearize(linearization_pt), linearization_pt);
381 std::map<gtsam::Key, gtsam::Key> key_map;
386 std::static_pointer_cast<LinearContainerFactor>(
387 lcf_factor.
rekey(key_map));
388 CHECK(lcf_factor_rekey_ptr);