[Bryan Herbst]

Have you looked through the C++ vision processing sample code?

That should get you most of the way there, and the WPILib website explains the overall strategy pretty well should you decide that you need to make modifications to it.