Feature/update draw sonar

[amarburg]

Major rewrite of internals to improve performance and image quality.

I have tried to maintain backwards compatibility both in terms of data output e.g. image size as a function of # of bearings/bins and API, but have not been too precious. Please flag if output is not as expected.

Currently an "early access" draft, still making improvements.

[amarburg]

OK, we're now at "alpha release" stage. Still work to do, but clean-ish. As far as I know, the ROS interfaces are unchanged. The C++ API for drawSonar is also the same ...

I have changed the virtual requirements for AbstractSonarInterface -- it's internally consistent within this repo but may break some stuff elsewhere.

[lindzey]

@amarburg -- this is a rather large PR and I'm having trouble figuring out where to start.

It would be really helpful (and good practice in general) if you could include a narrative description of the changes you made and why as a comment to the PR.

[amarburg]

@amarburg -- this is a rather large PR and I'm having trouble figuring out where to start.

It would be really helpful (and good practice in general) if you could include a narrative description of the changes you made and why as a comment to the PR.

Sure, it did get a bit far-ranging.

Major changes as follows:

• Broke the drawSonar functionality into two parts: drawSonarRectImage applies the color map to render the sonar data as a 2D (numAzimuths x numRanges) RGB image. drawSonar then uses cv::remap to warp that image into the "fan" shape. This is the major update to the aesthetics and performance of the code.
• Moved both of above from a function-based interface to class members in SonarDrawer ... this allows persisting of maps between drawing operations when the sonar geometry doesn't change. Left the functional API in place, it's now a trivial wrapper which creates an SonarDrawer, calls it, then destroys it.
• Changed AbstractSonarInterface s.t. its primary interface requirement for inherited classes is functions which return const std::vector & of azimuths and range bins. Added functions for searching for and caching minimum and maximum azimuth/range values from these vectors.
• Rewrote SonarColorMap with templated lookup functions to return appropriately scaled pixel values for either Vec3f (in range [0,1]) or Vec3b (in range [0,255]). This cleans up the drawSonar code and removes arbitrary "255 *"s

Minor / induced changes:

• Restructuring of sonar_draw_nodelet
• Added AbstractSonarInterface.cpp partly to hold a new static const member, and also to include a few more complex member functions.
• Moved the old functions into their own source file, an a new namespace sonar_image_proc::old_api
• Moved SonarImageMsgInterface into a new ROS-only header file ins src_ros to reduce the size of sonar_draw_nodelet

[lindzey]

Based on our discussion this morning, I'm merging this!

• We checked that it was consistent with the old version's output (just faster and better resolution)
• We checked that it worked with new bagfiles from the tank
• We double-checked that azimuth signs were correct, and that apparent sonar motion matched camera data

[lindzey]

Err... I'll be merging it as soon as I sign in to my other github account...