В настоящее время я использую G2o для оптимизации графа поз. Сейчас я использую 3d-3d соответствия для ограничения точка-точка. Теперь я хочу, чтобы это ограничение было двухточечным, например формула (1) в https://www.comp.nus.edu.sg/~lowkl/publications/lowk_point-to-plane_icp_techrep.pdf.
Я знаю, как вычислить функцию стоимости computeError(). Но я не знаю, как вычислить якобиан для линеаризации linearizeOplus().
У вас есть идеи?
С уважением,