[add] check OB intrinsics are nan or not

zch-soft · Aug 31, 2019 · 1f69a44 · 1f69a44
1 parent b6ba3e6
commit 1f69a44
Show file tree

Hide file tree

Showing 5 changed files with 26 additions and 3 deletions.
diff --git a/include/astra_camera/astra_device.h b/include/astra_camera/astra_device.h
@@ -128,6 +128,7 @@ class AstraDevice
   float getDepthFocalLength (int output_y_resolution) const;
   float getBaseline () const;
   OBCameraParams getCameraParams() const;
+  bool isCameraParamsValid();
   char* getSerialNumber();
   char* getDeviceType();
   OB_DEVICE_NO getDeviceTypeNo();
@@ -181,6 +182,7 @@ class AstraDevice
   bool use_device_time_;
 
   OBCameraParams m_CamParams;
+  bool m_ParamsValid;
   char serial_number[12];
   char device_type[32];
   OB_DEVICE_NO device_type_no;

diff --git a/include/libuvc_camera/camera_driver.h b/include/libuvc_camera/camera_driver.h
@@ -108,6 +108,7 @@ class CameraDriver {
   sensor_msgs::CameraInfo camera_info_;
   int uvc_flip_;
   OB_DEVICE_NO device_type_no_;
+  bool camera_info_valid_;
 };
 
 };
diff --git a/src/astra_device.cpp b/src/astra_device.cpp
@@ -47,6 +47,7 @@
 #include <boost/make_shared.hpp>
 
 #include <string>
+#include <cmath>
 
 namespace astra_wrapper
 {
@@ -82,6 +83,12 @@ AstraDevice::AstraDevice(const std::string& device_URI):
 
   int param_size = sizeof(OBCameraParams);
   openni_device_->getProperty(openni::OBEXTENSION_ID_CAM_PARAMS, (uint8_t*)&m_CamParams, &param_size);
+  m_ParamsValid = true;
+  if (std::isnan(m_CamParams.l_intr_p[0]) || std::isnan(m_CamParams.l_intr_p[1]) ||
+      std::isnan(m_CamParams.l_intr_p[2]) || std::isnan(m_CamParams.l_intr_p[3]))
+  {
+    m_ParamsValid = false;
+  }
 
   int serial_number_size = sizeof(serial_number);
   memset(serial_number, 0, serial_number_size);
@@ -172,6 +179,11 @@ OBCameraParams AstraDevice::getCameraParams() const
   return m_CamParams;
 }
 
+bool AstraDevice::isCameraParamsValid()
+{
+  return m_ParamsValid;
+}
+
 char* AstraDevice::getSerialNumber()
 {
   return serial_number;

diff --git a/src/astra_driver.cpp b/src/astra_driver.cpp
@@ -790,7 +790,7 @@ sensor_msgs::CameraInfoPtr AstraDriver::getColorCameraInfo(int width, int height
   else
   {
     // If uncalibrated, fill in default values
-    if (astraWithUVC(device_->getDeviceTypeNo()))
+    if (device_->isCameraParamsValid())
     {
       sensor_msgs::CameraInfo cinfo = convertAstraCameraInfo(device_->getCameraParams(), time);
       info = boost::make_shared<sensor_msgs::CameraInfo>(ir_info_manager_->getCameraInfo());
@@ -845,7 +845,8 @@ sensor_msgs::CameraInfoPtr AstraDriver::getIRCameraInfo(int width, int height, r
   {
     // If uncalibrated, fill in default values
     info = getDefaultCameraInfo(width, height, device_->getDepthFocalLength(height));
-    if (astraWithUVC(device_->getDeviceTypeNo()))
+
+    if (device_->isCameraParamsValid())
     {
       OBCameraParams p = device_->getCameraParams();
       info->D.resize(5, 0.0);

diff --git a/src/libuvc_camera/camera_driver.cpp b/src/libuvc_camera/camera_driver.cpp
@@ -41,6 +41,7 @@
 #include <libuvc/libuvc.h>
 #include <astra_camera/GetDeviceType.h>
 #include <astra_camera/GetCameraInfo.h>
+#include <cmath>
 
 #define libuvc_VERSION (libuvc_VERSION_MAJOR * 10000 \
                       + libuvc_VERSION_MINOR * 100 \
@@ -80,6 +81,7 @@ CameraDriver::CameraDriver(ros::NodeHandle nh, ros::NodeHandle priv_nh)
     }
   }
   ns_no_slash = ns.substr(slash_end);
+  camera_info_valid_ = false;
 }
 
 CameraDriver::~CameraDriver() {
@@ -342,14 +344,19 @@ void CameraDriver::ImageCallback(uvc_frame_t *frame) {
     {
       camera_info_ = camera_info_srv.response.info;
       camera_info_init_ = true;
+      camera_info_valid_ = true;
+      if (std::isnan(camera_info_.K[0]) || std::isnan(camera_info_.K[2]) || std::isnan(camera_info_.K[4]) || std::isnan(camera_info_.K[5]))
+      {
+        camera_info_valid_ = false;
+      }
     }
   }
 
   sensor_msgs::CameraInfo::Ptr cinfo(new sensor_msgs::CameraInfo(cinfo_manager_.getCameraInfo()));
   if (device_type_init_ == true && astraWithUVC(device_type_no_))
   {
     // update cinfo
-    if (camera_info_init_ == true)
+    if (camera_info_init_ == true && camera_info_valid_ == true)
     {
       cinfo->height = image->height;
       cinfo->width = image->width;