diff --git a/ntk/camera/nite_rgbd_grabber.cpp b/ntk/camera/nite_rgbd_grabber.cpp index a62ee4d..2540194 100644 --- a/ntk/camera/nite_rgbd_grabber.cpp +++ b/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(); 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(); diff --git a/ntk/camera/nite_rgbd_grabber.h b/ntk/camera/nite_rgbd_grabber.h index 1bb72c8..3e2bd57 100644 --- a/ntk/camera/nite_rgbd_grabber.h +++ b/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 diff --git a/ntk/camera/rgbd_image.h b/ntk/camera/rgbd_image.h index 1641cf8..482673d 100644 --- a/ntk/camera/rgbd_image.h +++ b/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: diff --git a/ntk/camera/rgbd_processor.cpp b/ntk/camera/rgbd_processor.cpp index c9ab1a4..38ceaea 100644 --- a/ntk/camera/rgbd_processor.cpp +++ b/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 diff --git a/ntk/camera/rgbd_processor.h b/ntk/camera/rgbd_processor.h index ab56b11..74d9c92 100644 --- a/ntk/camera/rgbd_processor.h +++ b/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(); }; /*! diff --git a/ntk/geometry/relative_pose_estimator.cpp b/ntk/geometry/relative_pose_estimator.cpp index 48078aa..43ef51e 100644 --- a/ntk/geometry/relative_pose_estimator.cpp +++ b/ntk/geometry/relative_pose_estimator.cpp @@ -241,7 +241,7 @@ 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& best_matches, @@ -249,7 +249,7 @@ estimateDeltaPose(Pose3D& new_pose, { 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 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); diff --git a/ntk/gui/image_widget.cpp b/ntk/gui/image_widget.cpp index d7f0eff..bc4e45d 100644 --- a/ntk/gui/image_widget.cpp +++ b/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) { diff --git a/ntk/image/feature.cpp b/ntk/image/feature.cpp index 33e84da..105bc27 100644 --- a/ntk/image/feature.cpp +++ b/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); } } }