31 #include <opencv2/imgproc/types_c.h>
34 #include <pcl/io/openni_grabber.h>
35 #include <pcl/io/oni_grabber.h>
36 #include <pcl/io/openni_camera/openni_depth_image.h>
37 #include <pcl/io/openni_camera/openni_image.h>
44 Camera(imageRate, localTransform),
79 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
80 void CameraOpenni::image_cb (
81 const std::shared_ptr<openni_wrapper::Image>& rgb,
82 const std::shared_ptr<openni_wrapper::DepthImage>& depth,
85 void CameraOpenni::image_cb (
93 bool notify = rgb_.empty();
95 cv::Mat rgbFrame(rgb->getHeight(), rgb->getWidth(), CV_8UC3);
96 rgb->fillRGB(rgb->getWidth(), rgb->getHeight(), rgbFrame.data);
97 cv::cvtColor(rgbFrame, rgb_, CV_RGB2BGR);
99 depth_ = cv::Mat(rgb->getHeight(), rgb->getWidth(), CV_16UC1);
100 depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (
unsigned short*)depth_.data);
102 depthConstant_ = constant;
106 dataReady_.release();
113 #ifdef RTABMAP_OPENNI
133 #if PCL_VERSION_COMPARE(>=, 1, 10, 0)
135 const std::shared_ptr<openni_wrapper::Image>&,
136 const std::shared_ptr<openni_wrapper::DepthImage>&,
137 float)>
f = std::bind (&CameraOpenni::image_cb,
this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
139 boost::function<void (
142 float)>
f = boost::bind (&CameraOpenni::image_cb,
this, _1, _2, _3);
148 catch(
const pcl::IOException& ex)
150 UERROR(
"OpenNI exception: %s", ex.what());
160 UERROR(
"PCL not built with OpenNI! Cannot initialize CameraOpenNI");
167 #ifdef RTABMAP_OPENNI
176 #ifdef RTABMAP_OPENNI
188 #ifdef RTABMAP_OPENNI
193 UWARN(
"Not received new frames since 5 seconds, end of stream reached!");
203 float(
rgb_.cols/2) - 0.5f,
204 float(
rgb_.rows/2) - 0.5f,
205 this->getLocalTransform(),
217 UERROR(
"CameraOpenNI: RTAB-Map is not built with PCL having OpenNI support!");