Support high resolution rgb. Fix relative_pose_estimator.

ntk/camera/nite_rgbd_grabber.cpp

@@ -97,6 +97,15 @@ void NiteRGBDGrabber :: initialize()
   status = m_ni_context.FindExistingNode(XN_NODE_TYPE_IMAGE, m_ni_rgb_generator);
   check_error(status, "Find image generator");
 
+  if (m_high_resolution)
+  {
+    XnMapOutputMode rgb_mode;
+    rgb_mode.nFPS = 15;
+    rgb_mode.nXRes = 1280;
+    rgb_mode.nYRes = 1024;
+    m_ni_rgb_generator.SetMapOutputMode(rgb_mode);
+  }
+
   ntk_ensure(m_ni_depth_generator.IsCapabilitySupported(XN_CAPABILITY_ALTERNATIVE_VIEW_POINT), "Cannot register images.");
   m_ni_depth_generator.GetAlternativeViewPointCap().SetViewPoint(m_ni_rgb_generator);
 
@@ -133,9 +142,6 @@ void NiteRGBDGrabber :: estimateCalibration()
   XnPoint3D p;
   p.X = 0; p.Y = 0; p.Z = -1;
   m_ni_depth_generator.ConvertProjectiveToRealWorld(1, &p, &p);
-  ntk_dbg_print(p.X, 1);
-  ntk_dbg_print(p.Y, 1);
-  ntk_dbg_print(p.Z, 1);
 
   p.X = 0; p.Y = 0; p.Z = -1;
   m_ni_depth_generator.ConvertRealWorldToProjective(1, &p, &p);
@@ -147,8 +153,6 @@ void NiteRGBDGrabber :: estimateCalibration()
 
   double fx = -(p.X-cx);
   double fy = p.Y-cy;
-  ntk_dbg_print(fx, 1);
-  ntk_dbg_print(fy, 1);
 
   m_calib_data = new RGBDCalibration();
 
@@ -169,12 +173,23 @@ void NiteRGBDGrabber :: estimateCalibration()
   m_calib_data->raw_depth_size = cv::Size(depth_width, depth_height);
   m_calib_data->depth_size = cv::Size(depth_width, depth_height);
 
+  float width_ratio = float(rgb_width)/depth_width;
+  float height_ratio = float(rgb_height)/depth_height;
+
+  float rgb_fx = fx * width_ratio;
+  // Pixels are square on a Kinect.
+  // Image height gets cropped when going from 1280x1024 in 640x480.
+  // The ratio remains 2.
+  float rgb_fy = rgb_fx;
+  float rgb_cx = cx * width_ratio;
+  float rgb_cy = cy * width_ratio;
+
   m_calib_data->rgb_intrinsics = cv::Mat1d(3,3);
   setIdentity(m_calib_data->rgb_intrinsics);
-  m_calib_data->rgb_intrinsics(0,0) = fx;
-  m_calib_data->rgb_intrinsics(1,1) = fy;
-  m_calib_data->rgb_intrinsics(0,2) = cx;
-  m_calib_data->rgb_intrinsics(1,2) = cy;
+  m_calib_data->rgb_intrinsics(0,0) = rgb_fx;
+  m_calib_data->rgb_intrinsics(1,1) = rgb_fy;
+  m_calib_data->rgb_intrinsics(0,2) = rgb_cx;
+  m_calib_data->rgb_intrinsics(1,2) = rgb_cy;
 
   m_calib_data->rgb_distortion = Mat1d(1,5);
   m_calib_data->rgb_distortion = 0.;
@@ -214,23 +229,49 @@ void NiteRGBDGrabber :: run()
   m_current_image.setCalibration(m_calib_data);
   m_rgbd_image.setCalibration(m_calib_data);
 
-  m_rgbd_image.rawRgbRef() = Mat3b(480, 640);
-  m_rgbd_image.rawDepthRef() = Mat1f(480, 640);
-  m_rgbd_image.rawIntensityRef() = Mat1f(480, 640);
+  m_rgbd_image.rawRgbRef() = Mat3b(m_calib_data->rawRgbSize());
+  m_rgbd_image.rawDepthRef() = Mat1f(m_calib_data->raw_depth_size);
+  m_rgbd_image.rawIntensityRef() = Mat1f(m_calib_data->rawRgbSize());
+
+  m_rgbd_image.rawIntensityRef() = 0.f;
+  m_rgbd_image.rawDepthRef() = 0.f;
+  m_rgbd_image.rawRgbRef() = Vec3b(0,0,0);
+
   m_rgbd_image.rgbRef() = m_rgbd_image.rawRgbRef();
   m_rgbd_image.depthRef() = m_rgbd_image.rawDepthRef();
   m_rgbd_image.intensityRef() = m_rgbd_image.rawIntensityRef();
-  m_rgbd_image.mappedRgbRef() = m_rgbd_image.rawRgbRef();
-  m_rgbd_image.mappedDepthRef() = m_rgbd_image.rawDepthRef();
 
-  m_current_image.rawRgbRef() = Mat3b(480, 640);
-  m_current_image.rawDepthRef() = Mat1f(480, 640);
-  m_current_image.rawIntensityRef() = Mat1f(480, 640);
+  m_current_image.rawRgbRef() = Mat3b(m_calib_data->rawRgbSize());
+  m_current_image.rawRgbRef() = Vec3b(0,0,0);
+  m_current_image.rawDepthRef() = Mat1f(m_calib_data->raw_depth_size);
+  m_current_image.rawDepthRef() = 0.f;
+  m_current_image.rawIntensityRef() = Mat1f(m_calib_data->rawRgbSize());
+  m_current_image.rawIntensityRef() = 0.f;
+
   m_current_image.rgbRef() = m_current_image.rawRgbRef();
   m_current_image.depthRef() = m_current_image.rawDepthRef();
   m_current_image.intensityRef() = m_current_image.rawIntensityRef();
-  m_current_image.mappedRgbRef() = m_rgbd_image.rawRgbRef();
-  m_current_image.mappedDepthRef() = m_rgbd_image.rawDepthRef();
+
+
+  bool mapping_required = m_calib_data->rawRgbSize() != m_calib_data->raw_depth_size;
+  if (!mapping_required)
+  {
+    m_rgbd_image.mappedRgbRef() = m_rgbd_image.rawRgbRef();
+    m_rgbd_image.mappedDepthRef() = m_rgbd_image.rawDepthRef();
+    m_current_image.mappedRgbRef() = m_current_image.rawRgbRef();
+    m_current_image.mappedDepthRef() = m_current_image.rawDepthRef();
+  }
+  else
+  {
+    m_rgbd_image.mappedRgbRef() = Mat3b(m_calib_data->raw_depth_size);
+    m_rgbd_image.mappedRgbRef() = Vec3b(0,0,0);
+    m_rgbd_image.mappedDepthRef() = Mat1f(m_calib_data->rawRgbSize());
+    m_rgbd_image.mappedDepthRef() = 0.f;
+    m_current_image.mappedRgbRef() = Mat3b(m_calib_data->raw_depth_size);
+    m_current_image.mappedRgbRef() = Vec3b(0,0,0);
+    m_current_image.mappedDepthRef() = Mat1f(m_calib_data->rawRgbSize());
+    m_current_image.mappedDepthRef() = 0.f;
+  }
 
   xn::SceneMetaData sceneMD;
   xn::DepthMetaData depthMD;
@@ -250,19 +291,12 @@ void NiteRGBDGrabber :: run()
     ntk_assert((depthMD.XRes() == m_current_image.rawDepth().cols)
                && (depthMD.YRes() == m_current_image.rawDepth().rows),
                "Invalid image size.");
+
+    // Convert to meters.
     float* raw_depth_ptr = m_current_image.rawDepthRef().ptr<float>();
     for (int i = 0; i < depthMD.XRes()*depthMD.YRes(); ++i)
       raw_depth_ptr[i] = pDepth[i]/1000.f;
 
-    float d = m_current_image.rawDepth()(250,240);
-    Point3f p1 = m_calib_data->depth_pose->unprojectFromImage(Point2f(240,250), d);
-    ntk_dbg_print(p1, 2);
-
-    XnPoint3D p; p.X = 240; p.Y = 250; p.Z = d;
-    m_ni_depth_generator.ConvertProjectiveToRealWorld(1, &p, &p);
-    Point3f p2 (p.X, p.Y, p.Z);
-    ntk_dbg_print(p2, 2);
-
     const XnUInt8* pImage = rgbMD.Data();
     ntk_assert(rgbMD.PixelFormat() == XN_PIXEL_FORMAT_RGB24, "Invalid RGB format.");
     uchar* raw_rgb_ptr = m_current_image.rawRgbRef().ptr<uchar>();

ntk/camera/nite_rgbd_grabber.h

@@ -40,7 +40,8 @@ class NiteRGBDGrabber : public ntk::RGBDGrabber
   NiteRGBDGrabber() :
     m_need_pose_to_calibrate(false),
     m_max_num_users(1),
-    m_body_event_detector(0)
+    m_body_event_detector(0),
+    m_high_resolution(false)
   {}
 
   /*! Call it before starting the thread. */
@@ -53,6 +54,9 @@ class NiteRGBDGrabber : public ntk::RGBDGrabber
   /*! Set the maximal number of tracked users. Default is one. */
   void setMaxUsers(int num) { m_max_num_users = num; }
 
+  /*! Set whether color images should be in high resolution 1280x1024. */
+  void setHighRgbResolution(bool hr) { m_high_resolution = hr; }
+
 public:
   // Nite accessors.
   xn::DepthGenerator& niDepthGenerator() { return m_ni_depth_generator; }
@@ -100,6 +104,7 @@ class NiteRGBDGrabber : public ntk::RGBDGrabber
   int m_max_num_users;
   Skeleton m_skeleton_data;
   BodyEventDetector* m_body_event_detector;
+  bool m_high_resolution;
 };
 
 } // ntk

ntk/camera/rgbd_image.h

@@ -148,10 +148,11 @@ class CV_EXPORTS RGBDImage
   const cv::Mat1f& intensity() const { return m_intensity; }
 
   /*! Whether this particular pixel has valid depth information */
-  bool pixelHasDepth(int r, int c) const
-  { return m_depth_mask.data
-        && r < m_depth_mask.rows && c < m_depth_mask.cols && r >= 0 && c >= 0
-        && m_depth_mask(r,c);
+  bool rgbPixelHasDepth(int r, int c) const
+  {
+    return m_mapped_depth.data
+        && r < m_mapped_depth.rows && c < m_mapped_depth.cols && r >= 0 && c >= 0
+        && m_mapped_depth(r,c) > 1e-5;
   }
 
 private:

ntk/camera/rgbd_processor.cpp

@@ -602,4 +602,29 @@ namespace ntk
     }
   }
 
+  void NiteProcessor :: computeMappings()
+  {
+    Size depth_size = m_image->calibration()->raw_depth_size;
+    Size rgb_size = m_image->calibration()->rawRgbSize();
+
+    bool mapping_required = depth_size != rgb_size;
+
+    if (!mapping_required)
+      return;
+
+    const float ratio = float(rgb_size.width) / depth_size.width;
+
+    Size raw_rgb_size(rgb_size.width/ratio, rgb_size.height/ratio);
+    cv::Mat3b raw_mapped_rgb(raw_rgb_size);
+    cv::resize(m_image->rawRgb(), raw_mapped_rgb, raw_rgb_size);
+    m_image->mappedRgbRef() = raw_mapped_rgb(Rect(Point(0,0),
+                                                  depth_size));
+
+    Size raw_depth_size(depth_size.width*ratio, depth_size.height*ratio);
+    cv::Mat1f raw_mapped_depth(raw_depth_size);
+    cv::resize(m_image->rawDepth(), raw_mapped_depth, raw_depth_size, 0, 0, INTER_LINEAR);
+    cv::Mat1f roi = m_image->mappedDepthRef()(Rect(Point(0,0), raw_depth_size));
+    raw_mapped_depth.copyTo(roi);
+  }
+
 } // ntk

ntk/camera/rgbd_processor.h

@@ -97,22 +97,22 @@ class RGBDProcessor
   /*! Postprocess an RGB-D image. */
   virtual void processImage(RGBDImage& image);
 
-  void undistortImages();
-  void fixDepthGeometry();
-  void fixDepthBias();
-  void removeLowAmplitudeOutliers();
-  void removeNormalOutliers();
-  void removeUnstableOutliers();
-  void removeEdgeOutliers();
-  void applyDepthThreshold();
-  void computeNormals();
-  void computeMappings();
-  void medianFilter();
-  void computeKinectDepthLinear();
-  void computeKinectDepthTanh();
-  void computeKinectDepthBaseline();
-  void removeSmallStructures();
-  void fillSmallHoles();
+  virtual void undistortImages();
+  virtual void fixDepthGeometry();
+  virtual void fixDepthBias();
+  virtual void removeLowAmplitudeOutliers();
+  virtual void removeNormalOutliers();
+  virtual void removeUnstableOutliers();
+  virtual void removeEdgeOutliers();
+  virtual void applyDepthThreshold();
+  virtual void computeNormals();
+  virtual void computeMappings();
+  virtual void medianFilter();
+  virtual void computeKinectDepthLinear();
+  virtual void computeKinectDepthTanh();
+  virtual void computeKinectDepthBaseline();
+  virtual void removeSmallStructures();
+  virtual void fillSmallHoles();
 
 protected:
   RGBDImage* m_image;
@@ -146,8 +146,11 @@ class NiteProcessor : public RGBDProcessor
     : RGBDProcessor()
   {
     // Everything is done by the grabber.
-    setFilterFlags(RGBDProcessor::NiteProcessed);
+    setFilterFlags(RGBDProcessor::NiteProcessed | RGBDProcessor::ComputeMapping);
   }
+
+protected:
+  virtual void computeMappings();
 };
 
 /*!

ntk/geometry/relative_pose_estimator.cpp

@@ -241,15 +241,15 @@ computeNumMatchesWithPrevious(const RGBDImage& image,
 }
 
 bool RelativePoseEstimatorFromImage::
-estimateDeltaPose(Pose3D& new_pose,
+estimateDeltaPose(Pose3D& new_rgb_pose,
                   const RGBDImage& image,
                   const FeatureSet& image_features,
                   const std::vector<cv::DMatch>& best_matches,
                   int closest_view_index)
 {
   const float err_threshold = 5;
 
-  ntk_dbg_print(new_pose, 2);
+  ntk_dbg_print(new_rgb_pose, 2);
   const ImageData& ref_image_data = m_image_data[closest_view_index];
 
   ntk_dbg_print(best_matches.size(), 2);
@@ -290,10 +290,10 @@ estimateDeltaPose(Pose3D& new_pose,
 
   // double error = rms_optimize_3d(new_pose, ref_points, img_points);
   std::vector<bool> valid_points;
-  double error = rms_optimize_ransac(new_pose, ref_points, img_points, valid_points);
+  double error = rms_optimize_ransac(new_rgb_pose, ref_points, img_points, valid_points);
 
   ntk_dbg_print(error, 1);
-  ntk_dbg_print(new_pose, 2);
+  ntk_dbg_print(new_rgb_pose, 2);
 
   if (error < err_threshold)
     return true;
@@ -313,6 +313,9 @@ bool RelativePoseEstimatorFromImage::estimateNewPose(const RGBDImage& image)
   image_features.extractFromImage(image, m_feature_parameters);
 
   Pose3D new_pose = *image.calibration()->depth_pose;
+  Pose3D new_rgb_pose = new_pose;
+  new_rgb_pose.toRightCamera(image.calibration()->rgb_intrinsics,
+                         image.calibration()->R, image.calibration()->T);
   bool pose_ok = true;
 
   if (m_image_data.size() > 0)
@@ -324,12 +327,19 @@ bool RelativePoseEstimatorFromImage::estimateNewPose(const RGBDImage& image)
     ntk_dbg_print(best_matches.size(), 1);
 
     new_pose = m_image_data[closest_view_index].pose;
+    new_rgb_pose = new_pose;
+    new_rgb_pose.toRightCamera(image.calibration()->rgb_intrinsics,
+                               image.calibration()->R, image.calibration()->T);
 
     if (best_matches.size() > 0)
     {
       // Estimate the relative pose w.r.t the closest view.
-      if (!estimateDeltaPose(new_pose, image, image_features, best_matches, closest_view_index))
+      if (!estimateDeltaPose(new_rgb_pose, image, image_features, best_matches, closest_view_index))
         pose_ok = false;
+      new_pose = new_rgb_pose;
+      new_pose.toLeftCamera(image.calibration()->depth_intrinsics,
+                            image.calibration()->R, image.calibration()->T);
+
     }
     else
     {
@@ -343,7 +353,7 @@ bool RelativePoseEstimatorFromImage::estimateNewPose(const RGBDImage& image)
     image.rgb().copyTo(image_data.color);
     image_data.pose = new_pose;
     m_current_pose = new_pose;
-    image_features.compute3dLocation(new_pose);
+    image_features.compute3dLocation(new_rgb_pose);
     m_features.push_back(image_features);
     ntk_dbg_print(image_features.locations().size(), 1);
     m_image_data.push_back(image_data);

ntk/gui/image_widget.cpp

@@ -83,7 +83,10 @@ void ImageWidget :: setImage(const cv::Mat1f& im, double* i_min_val, double* i_m
   else
     minMaxLoc(im, &min_val, &max_val);
   if (min_val == max_val)
+  {
+    m_image.fill(qRgb(0,0,0));
     return;
+  }
 
   for (int r = 0; r < im.rows; ++r)
   {

ntk/image/feature.cpp

@@ -118,7 +118,7 @@ void FeatureSet :: extractFromImage(const RGBDImage& image,
   {
     foreach_idx(i, keypoints)
     {
-      if (image.pixelHasDepth(keypoints[i].pt.y, keypoints[i].pt.x))
+      if (image.rgbPixelHasDepth(keypoints[i].pt.y, keypoints[i].pt.x))
         filtered_keypoints.push_back(keypoints[i]);
     }
   }
@@ -171,7 +171,7 @@ void FeatureSet :: extractFromImageUsingSiftGPU(const RGBDImage& image,
     m_locations.reserve(keypoints.size());
     foreach_idx(i, keypoints)
     {
-      if (image.pixelHasDepth(keypoints[i].pt.y, keypoints[i].pt.x))
+      if (image.rgbPixelHasDepth(keypoints[i].pt.y, keypoints[i].pt.x))
       {
         FeatureLocation loc;
         (KeyPoint&)loc = keypoints[i];
@@ -244,7 +244,7 @@ void FeatureSet :: extractFromImageUsingSiftPP(const RGBDImage& image,
   {
     FeatureLocation new_location;
 
-    if (params.only_features_with_depth && !image.pixelHasDepth(iter->y,iter->x))
+    if (params.only_features_with_depth && !image.rgbPixelHasDepth(iter->y,iter->x))
       continue;
 
     // detect orientations
@@ -288,11 +288,10 @@ void FeatureSet :: fillDepthData(const RGBDImage& image)
   foreach_idx(i, m_locations)
   {
     FeatureLocation& loc = m_locations[i];
-    if (is_yx_in_range(image.depthMask(), loc.pt.y, loc.pt.x)
-        && image.depthMask()(loc.pt.y, loc.pt.x))
+    if (image.rgbPixelHasDepth(loc.pt.y, loc.pt.x))
     {
       loc.has_depth = true;
-      loc.depth = image.depth()(loc.pt.y, loc.pt.x);
+      loc.depth = image.mappedDepth()(loc.pt.y, loc.pt.x);
     }
   }
 }