Implement NDT optimization problem
Description
Purpose
We need to specify the optimization problem that NDT is solving. In particular, given a map representation and an observation (PointCloud2), the implementer of this ticket should implement the gradient, objective, and hessian calculations. Some caching may or may not be done in the problem.
Desired Behavior
This problem statement should implement the proposed API.
In particular, for a P2D-NDT
algorithm, the following methods should be implemented
operator()
Jacobian
Hessian
Modifications to the API are allowed and welcome.
For more details on the NDT algorithm see Chapter 6 of the 3D-NDT thesis or any of the other links in the literature review
Definition of Done
-
Tests -
Implementation