Implement P2D NDT optimization problem
This MR implements P2D ndt optimization problem.
- For more details, see the issue which will be linked below.
Notes for Reviewer
- A cached expression template class is implemented for efficient expression computation. A state machine is implemented to track the state of a cached expression at run time.
- Optimization problem API had to be refactored to make the templates work and fix
- A significant portion of the mathematical code is taken from PCL/autoware.ai implementation as they were already nicely named and formulated to begin with.
Closes #140 (closed)