Eigen-based PnP Cost Function and Unit Test

[marip8]

This PR adds a unit test for the 2D PnP optimization. I used this as an attempt to understand how to use Eigen objects directly in optimizations for use in the development of future cost functions (i.e. kinematic calibration). Some changes to note:

1. The cost function takes two arguments (camera to target quaternion and position), opposed to the single argument previously (camera to target position and un-normalized axis angle rotation).
2. The cost function uses the Eigen::Map class to construct an Eigen object directly from a template data pointer
3. Uses Ceres local parameterization to reduce the 4 quaternion values into the minimal number of variables for optimization (i.e. 3)
   • Supposedly you can create an SE(3) local parameterization, such that the cost function input can be a single 6 DOF pose. I wasn't able to figure out how that would work with Eigen objects yet though

Overall writing this cost function using Eigen objects was not difficult, and it makes the functions much more understandable. Also porting this change to the other cost functions would eventually allow us to phase out the custom Pose6d to reduce overall code size/maintenance.

[marip8]

@schornakj FYI d5382d4 was @Levi-Armstrong 's fix for printing CTest output. Hopefully this will help us diagnose unexpected unit test failures more easily

[schornakj]

Is it possible to map target_t directly to an Eigen::Translation instead of needing to go through an Eigen::Matrix<T,3,1> as an intermediary?

[marip8]

Good thought. I'm not sure if the pointer maps the same way, but I can test it out

[marip8]

There appears to be a bug with the way that Eigen maps translations from pointers into objects, so it doesn't compile. I think we'll have to leave it as a vector for now. Ultimately I would like to be able to pass the entire matrix to a local parameterization instead of two separate objects, but I haven't worked that out yet.

[schornakj]

Conceptually this looks good to me! I agree that the Eigen-based approach is more user-friendly than the custom Pose6d type we were using previously.

1. The cost function takes two arguments (camera to target quaternion and position), opposed to the single argument previously (camera to target position and un-normalized axis angle rotation).

Strictly speaking the cost function could still take the quaternion and position as a single argument, you'd just need to do some pointer arithmetic when constructing the Eigen mapping. However, I agree with you that it's better to separate them, since it frees the user of the burden of needing to keep track of which indices of a raw pointer correspond to which coefficients in the parameters.