Function BuildLinearSystem

cpp/map_closures/GroundAlign.cpp:131–153

}

LinearSystem BuildLinearSystem(const Vector3dVector &points) {
    auto compute_jacobian_and_residual =
        [](const Eigen::Vector3d &point) -> std::pair<Eigen::Matrix<double, 1, 3>, double> {
        return {Eigen::Matrix<double, 1, 3>(1.0, point.y(), -point.x()), point.z()};
    };

    auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) -> LinearSystem {
        a.first += b.first;
        a.second += b.second;
        return a;
    };

    const auto linear_system =
        std::transform_reduce(points.cbegin(), points.cend(),
                              LinearSystem(Eigen::Matrix3d::Zero(), Eigen::Vector3d::Zero()),
                              sum_linear_systems, [&](const Eigen::Vector3d &point) {
                                  const auto [J, residual] = compute_jacobian_and_residual(point);
                                  const double w = std::exp(-1.0 * residual * residual);
                                  return LinearSystem(J.transpose() * w * J,          // JTJ
                                                      J.transpose() * w * residual);  // JTr
                              });
    return linear_system;
}
}  // namespace

namespace map_closures {