Validate more camera intrinsic values

asa_ros/src/asa_ros_node.cpp

@@ -109,6 +109,11 @@ void AsaRosNode::initFromRosParams() {
   }
 }
 
+// Returns true if the camera intrinsics seem to be valid. Does not validate correctness
+bool ValidateCameraIntrinsics(const boost::array<double, 9> K, int image_height, int image_width){
+  return K.at(0) > 0 && K.at(4) > 0 && K.at(2) >= 0 && K.at(5) >= 0 && image_height > 0 && image_width > 0;
+}
+
 void AsaRosNode::imageAndInfoCallback(
     const sensor_msgs::Image::ConstPtr& image,
     const sensor_msgs::CameraInfo::ConstPtr& camera_info) {
@@ -118,9 +123,8 @@ void AsaRosNode::imageAndInfoCallback(
     ROS_INFO_STREAM("Set camera frame ID to " << camera_frame_id_);
   }
 
-  if(camera_info->K[0] == 0 && camera_info->K[4] == 0)
+  if(!ValidateCameraIntrinsics(camera_info->K, image->height, image->width)){
-  {
+    ROS_WARN_ONCE("The camera_info topic reported invalid values. The anchor creation will fail. Check the camera configuration.");
-    ROS_WARN_ONCE("The camera_info topic reported focal lengths of 0 for x & z. The anchor creation will fail.");
   }
 
   // Look up its pose.