ros-perception#180 Check if all distortion coefficients are zero. Add…

… a test next. Currently testing manually with vimjay image_rect.launch (which brings up an rqt gui and camera info distortion coefficients can be dynamically reconfigured.

image_proc/src/nodelets/rectify.cpp

@@ -36,6 +36,7 @@
 #include <boost/thread/lock_guard.hpp>
 #endif
 
+#include <algorithm>
 #include <ros/ros.h>
 #include <nodelet/nodelet.h>
 #include <image_transport/image_transport.h>
@@ -122,7 +123,17 @@ void RectifyNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg,
   }
 
   // If zero distortion, just pass the message along
-  if (info_msg->D.empty() || info_msg->D[0] == 0.0)
+  bool zero_distortion = true;
+  for (size_t i = 0; i < info_msg->D.size(); ++i)
+  {
+    if (info_msg->D[i] != 0.0)
+    {
+      zero_distortion = false;
+      break;
+    }
+  }
+  // This will be true if D is empty/zero sized
+  if (zero_distortion)
   {
     pub_rect_.publish(image_msg);
     return;