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