Add camera frustum marker

[JStech]

I propose adding a marker type that draws a camera frustum given the optical frame, camera intrinsics (sensor_msgs::CameraInfo message), and scale. The marker would look like this:

And the function signature would be something like,

bool publishCameraFrustum(const Eigen::Isometry3d& pose, const sensor_msgs::CameraInfo& camera_intrinsics,
                          colors color, scales scale, const std::string& ns, std::size_t id)

There's already a function to do this in MoveIt Calibration, and I'd basically copy it here (and then use the version here in MoveIt Calibration). I recently ran into another situation where I'd like to draw a frustum, and so I thought it'd be a good addition to this package. Before writing it, though, I thought I'd double-check that people would want that included, since it would add the dependency on sensor_msgs.

[schornakj]

If you want to avoid adding the dependency on sensor_msgs you could pass in the intrinsics as an Eigen matrix or some other generic type (though that would be fairly annoying and you'd need an external conversion function anyway).

[JStech]

Yeah, I really just need the two focal lengths and the image height and width, so I could just make each of those values an argument. I'd prefer using the CameraInfo message, though, because it's convenient and has clear semantics.

[JafarAbdi]

@JStech rviz_visual_tools already depending on sensor_msgs see CMakeLists.txt & pacakge.xml

[JStech]

Ah, so it does--I didn't look beyond the main source file, I admit. I'll go ahead and put together a PR for this.

[tonynajjar]

Cool feature, was this ever implemented or any plans to?