Add equidistant distortion model

[mintar]

This PR adds support for the equidistant distortion model. In OpenCV, this model is called the fisheye distortion model. It is based on the following publication:

J. Kannala and S. Brandt (2006). A Generic Camera Model and Calibration Method for Conventional, Wide-Angle, and Fish-Eye Lenses, IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 28, no. 8, pp. 1335-1340

The equidistant model was listed in sensor_msgs/distortion_models.h since Kinetic, but image_geometry support was missing until now.

The equidistant distortion model is especially useful for high-FOV cameras:

Original image:

Rectified image:

Left: plumb_bob distortion model
Right: equidistant distortion model

Notice the distortions in the bottom right and top right corners of the plumb_bob image. The camera calibrations for the images above were obtained using Kalibr, and the rectified images were generated using this PR.

---

Adding the equidistant model was tackled in a number of previous PRs (#184, #299, #306, Intermodalics#1). To fully implement a new distortion model, the following functions (and only those) should be modified: rectifyPoint, rectifyImage, unrectifyPoint, unrectifyImage. The last one is not implemented, so that leaves the first three. Here's an overview of the PRs that I could find that implement the equidistant model:

function	#306	#184	#299	Intermodalics#1 (2)
rectifyPoint	yes	no	no	no
rectifyImage (1)	yes	yes	yes	yes
unrectifyPoint	yes	no	no	no

(1) implemented by modifying initRectificationMaps

(2) Intermodalics#1 not only implements the equidistant (aka "fisheye_cv") distortion model, but also the fov model. That would also be great to have here in a later PR.

So only #306 has implementations of all three functions. Unfortunately it's a bit hard to review because it's a huge diff. Therefore I cleaned up #306 and added set of unit tests. During testing, I noticed several bugs in #306, so I fixed them and decided to submit a separate PR. Thanks to @DavidTorresOcana for his work on #306, upon which this PR is based.

[DavidTorresOcana]

Thank you for the review of #306 and contribution.

I successfully tested this PR with ros-perception/image_pipeline#592 in noetic. This PR would render ros-perception/image_pipeline#441 unnecessary.

I'm not a maintainer of this repo, the following is my own personal opinion:
I still find the naming of pinhole_camera_model.cpp misleading as it implements several distortion models. In my opinion, it should be renamed with a more generic name, perhaps on a later PR.

Overall, I hope this PR goes through

[mintar]

@DavidTorresOcana : Thank you for your kind words and for testing this PR! And also for your work on #306 and the other PRs all across the image processing core packages. Let's hope some maintainer finds time to merge all those PRs soon!

I still find the naming of pinhole_camera_model.cpp misleading as it implements several distortion models

Well, as you say it implements different distortion models, but the camera model is always the pinhole model. The distortion and camera models are more or less orthogonal and can be combined arbitrarily.

Look at this Kalibr wiki page. In Kalibr nomenclature, vision_opencv already supports the pinhole-radtan camera+distortion model combination (radtan is just a different name for plumb_bob). This PR adds support for the pinhole-equi camera+distortion model combination. But combinations like omni-equi would not be supported, because the omnidirectional camera model combines a standard pinhole model with an additional hyperbolic, parabolic, or elliptical mirror, and needs an additional intrinsic parameter xi for the mirror, which is not supported by the CameraInfo message. If somebody would add an implementation of the omnidirectional camera model, I think it would be appropriate to add it as omni_camera_model.cpp, but for now we should keep the pinhole_camera_model.cpp name.

[mintar]

I fixed the Travis CI, but now the build farm tests have an unrelated failure. Closing and re-opening to retrigger the build farm.

[knorth55]

Thank you very much, 👍 for this PR.

[mintar]

@mjcarroll @vrabaud Could one of the maintainers review and merge this PR? Looking at the list of open PRs, it seems that vision_opencv is largely unmaintained. It's sad to see such an important ROS package die...

[mintar]

Bump

[mintar]

Could somebody please merge this? Come on, it has tests and everything.

[jokla]

Hi, great work!
Any plan to add the equidistant distortion model in ROS Melodic as well?