KinectV2PointCloudProvider.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package VisionX::ArmarXObjects::KinectV2PointCloudProvider
19  * @author Christoph Pohl (christoph dot pohl at kit dot edu)
20  * @date 2019
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 
26 #include "KinectV2PointCloudProvider.h"
27 #include <VisionX/tools/TypeMapping.h>
28 #include <VisionX/tools/ImageUtil.h>
29 #include <ArmarXCore/util/CPPUtility/trace.h>
30 
31 #include <Image/ImageProcessor.h>
32 #include <Calibration/Calibration.h>
33 
34 #include <pcl/io/pcd_io.h>
35 #include <ArmarXCore/core/time/TimeUtil.h>
36 
37 
38 
39 using namespace armarx;
40 using namespace pcl;
41 using namespace cv;
42 
43 namespace visionx
44 {
45  void KinectV2PointCloudProvider::onInitComponent()
46  {
47  ARMARX_TRACE;
48  ARMARX_VERBOSE << "onInitComponent()";
49 
50  ImageProvider::onInitComponent();
51  CapturingPointCloudProvider::onInitComponent();
52  }
53 
54  void KinectV2PointCloudProvider::onConnectComponent()
55  {
56  ARMARX_TRACE;
57  ARMARX_VERBOSE << "onConnectComponent()";
58 
59  ImageProvider::onConnectComponent();
60  CapturingPointCloudProvider::onConnectComponent();
61  }
62 
63  void KinectV2PointCloudProvider::onDisconnectComponent()
64  {
65  ARMARX_TRACE;
66  // hack to prevent deadlock
67  captureEnabled = false;
68  ImageProvider::onDisconnectComponent();
69  CapturingPointCloudProvider::onDisconnectComponent();
70  }
71 
72  void KinectV2PointCloudProvider::onExitComponent()
73  {
74  ARMARX_TRACE;
75  ARMARX_VERBOSE << "onExitComponent()";
76 
77  ImageProvider::onExitComponent();
78  CapturingPointCloudProvider::onExitComponent();
79  ARMARX_INFO << "Resetting grabber interface";
80  }
81 
82  void KinectV2PointCloudProvider::onInitCapturingPointCloudProvider()
83  {
84  ARMARX_TRACE;
85  enable_rgb_ = getProperty<bool>("EnableRGB").getValue();
86  enable_depth_ = getProperty<bool>("EnableDepth").getValue();
87  mirror_ = getProperty<bool>("Mirror").getValue();
88  serial_ = getProperty<std::string>("Serial").getValue();
89  pipeline_type_ = getProperty<KinectProcessorType>("Pipeline").getValue();
90  deviceId_ = -1;
91  dev_ = nullptr;
92  pipeline_ = nullptr;
93  rgb_height_ = 1080;
94  rgb_width_ = 1920;
95  d_height_ = 424;
96  d_width_ = 512;
97 
98  ARMARX_VERBOSE << "onInitCapturingPointCloudProvider()";
99  if (!pipeline_)
100  {
101  ARMARX_TRACE;
102  switch (pipeline_type_)
103  {
104  case CPU:
105  pipeline_ = new libfreenect2::CpuPacketPipeline();
106  break;
107  case OPENGL:
108  pipeline_ = new libfreenect2::OpenGLPacketPipeline();
109  break;
110  case OPENCL:
111  pipeline_ = new libfreenect2::OpenCLPacketPipeline(deviceId_);
112  break;
113  case OPENCLKDE:
114  pipeline_ = new libfreenect2::OpenCLKdePacketPipeline(deviceId_);
115  break;
116  case CUDA:
117  pipeline_ = new libfreenect2::CudaPacketPipeline(deviceId_);
118  break;
119  case CUDAKDE:
120  pipeline_ = new libfreenect2::CudaKdePacketPipeline(deviceId_);
121  break;
122  default:
123  pipeline_ = new libfreenect2::CpuPacketPipeline();
124  break;
125  }
126  }
127 
128  if (!enable_rgb_ && !enable_depth_)
129  {
130  ARMARX_TRACE;
131  ARMARX_ERROR << "Either depth or rgb needs to be enabled";
132  }
133  if (freenect2_.enumerateDevices() == 0)
134  {
135  ARMARX_ERROR << "no device connected!";
136  // return -1;
137  }
138  if (serial_.empty())
139  {
140  ARMARX_TRACE;
141  serial_ = freenect2_.getDefaultDeviceSerialNumber();
142  }
143  if (pipeline_)
144  {
145  ARMARX_TRACE;
146  dev_ = freenect2_.openDevice(serial_, pipeline_);
147  }
148  else
149  {
150  ARMARX_TRACE;
151  dev_ = freenect2_.openDevice(serial_);
152  }
153 
154  if (dev_ == nullptr)
155  {
156  ARMARX_ERROR << "failure opening device!";
157  }
158 
159  // init images
160  rgbImages_ = new CByteImage*[2];
161 
162  rgbImages_[0] = new CByteImage(d_width_, d_height_, CByteImage::eRGB24);
163  rgbImages_[1] = new CByteImage(d_width_, d_height_, CByteImage::eRGB24);
164  ::ImageProcessor::Zero(rgbImages_[0]);
165  ::ImageProcessor::Zero(rgbImages_[1]);
166 
167  }
168 
169 
170  void KinectV2PointCloudProvider::onStartCapture(float frameRate)
171  {
172  if (enable_rgb_ && enable_depth_)
173  {
174  ARMARX_TRACE;
175  if (!dev_->start())
176  {
177  ARMARX_ERROR << "Could not Start dev stream";
178  }
179  }
180  else
181  {
182  ARMARX_TRACE;
183  if (!dev_->startStreams(enable_rgb_, enable_depth_))
184  {
185  ARMARX_ERROR << "Could not Start dev stream";
186  }
187  }
188  libfreenect2::Freenect2Device::IrCameraParams depth_p = dev_->getIrCameraParams(); ///< Depth camera parameters.
189  libfreenect2::Freenect2Device::ColorCameraParams rgb_p = dev_->getColorCameraParams();
190  registration_ = new libfreenect2::Registration(depth_p, rgb_p);
191  dev_->setColorFrameListener(&listener_);
192  dev_->setIrAndDepthFrameListener(&listener_);
193  pclHelper = new KinectToPCLHelper<PointT>(getCameraIrParameters(), mirror_);
194  ivtHelper = new KinectToIVTHelper(mirror_);
195  initializeCameraCalibration();
196 
197  }
198 
199 
200  void KinectV2PointCloudProvider::onStopCapture()
201  {
202  ARMARX_TRACE;
203  ARMARX_INFO << "Stopping KinectV2 Grabber Interface";
204  if (dev_)
205  {
206  dev_->stop();
207  dev_->close();
208  }
209  ARMARX_INFO << "Stopped KinectV2 Grabber Interface";
210  }
211 
212 
213  void KinectV2PointCloudProvider::onExitCapturingPointCloudProvider()
214  {
215  ARMARX_TRACE;
216  ARMARX_VERBOSE << "onExitCapturingPointCloudProvider()";
217 
218  delete rgbImages_[0];
219  delete rgbImages_[1];
220 
221  delete [] rgbImages_;
222  }
223 
224 
225  void KinectV2PointCloudProvider::onInitImageProvider()
226  {
227  ARMARX_TRACE;
228  Eigen::Vector2i dimensions = Eigen::Vector2i(512, 424);
229  setImageFormat(visionx::ImageDimension(dimensions(0), dimensions(1)), visionx::eRgb);
230  setNumberImages(2);
231  }
232 
233 
234  bool KinectV2PointCloudProvider::doCapture()
235  {
236  if (!listener_.waitForNewFrame(frames_, 1 * 1000)) // 100 msconds
237  {
238  ARMARX_ERROR << "TIMEOUT";
239  return false;
240  }
241  IceUtil::Time ts = TimeUtil::GetTime();
242  pcl::PointCloud<PointT>::Ptr outputCloud = pcl::PointCloud<PointT>::Ptr(new pcl::PointCloud<PointT>());
243 
244 
245  libfreenect2::Frame* rgb = frames_[libfreenect2::Frame::Color];
246  // libfreenect2::Frame *ir = frames_[libfreenect2::Frame::Ir];
247  libfreenect2::Frame* depth = frames_[libfreenect2::Frame::Depth];
248  if (enable_rgb_ && enable_depth_)
249  {
250  registration_->apply(rgb, depth, &undistorted_, &registered_, true);
251  }
252  ivtHelper->calculateImages(undistorted_, registered_, rgbImages_);
253  pclHelper->calculatePointCloud(undistorted_, registered_, outputCloud);
254  // outputCloud->header.stamp = depth->timestamp;
255  outputCloud->header.stamp = ts.toMicroSeconds();
256  outputCloud->header.seq = depth->sequence;
257  providePointCloud<PointT>(outputCloud);
258  // provideImages(rgbImages_, IceUtil::Time::microSeconds(rgb->timestamp * 0.125 * 1000));
259  provideImages(rgbImages_, ts);
260  listener_.release(frames_);
261  return true;
262  }
263 
264 
265  PropertyDefinitionsPtr KinectV2PointCloudProvider::createPropertyDefinitions()
266  {
267  return PropertyDefinitionsPtr(new KinectV2PointCloudProviderPropertyDefinitions(getConfigIdentifier()));
268  }
269 
270  libfreenect2::Freenect2Device::IrCameraParams KinectV2PointCloudProvider::getCameraIrParameters()
271  {
272  libfreenect2::Freenect2Device::IrCameraParams ir = dev_->getIrCameraParams();
273  return ir;
274  }
275 
276  libfreenect2::Freenect2Device::ColorCameraParams KinectV2PointCloudProvider::getCameraRGBParameters()
277  {
278  libfreenect2::Freenect2Device::ColorCameraParams rgb = dev_->getColorCameraParams();
279  return rgb;
280  }
281 
282  StereoCalibration KinectV2PointCloudProvider::getStereoCalibration(const Ice::Current& c)
283  {
284  return calibration;
285  }
286 
287  void KinectV2PointCloudProvider::initializeCameraCalibration()
288  {
289  visionx::CameraParameters RGBCameraIntrinsics;
290  RGBCameraIntrinsics.focalLength.resize(2, 0.0f);
291  RGBCameraIntrinsics.principalPoint.resize(2, 0.0f);
292  RGBCameraIntrinsics.distortion.resize(4, 0.0f);
293 
294  //Eigen::Matrix3f id = Eigen::Matrix3f::Identity();
295  RGBCameraIntrinsics.rotation = visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Identity());
296  RGBCameraIntrinsics.translation = visionx::tools::convertEigenVecToVisionX(Eigen::Vector3f::Zero());
297  auto rgb_params = getCameraRGBParameters();
298  ARMARX_TRACE;
299 
300 
301  RGBCameraIntrinsics.principalPoint[0] = rgb_params.cx;
302  RGBCameraIntrinsics.principalPoint[1] = rgb_params.cy;
303  RGBCameraIntrinsics.focalLength[0] = rgb_params.fx;
304  RGBCameraIntrinsics.focalLength[1] = rgb_params.fy;
305  RGBCameraIntrinsics.width = rgb_width_;
306  RGBCameraIntrinsics.height = rgb_height_;
307 
308 
309  visionx::CameraParameters depthCameraIntrinsics;
310  depthCameraIntrinsics.focalLength.resize(2, 0.0f);
311  depthCameraIntrinsics.principalPoint.resize(2, 0.0f);
312  depthCameraIntrinsics.distortion.resize(4, 0.0f);
313  depthCameraIntrinsics.rotation = visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Identity());
314  depthCameraIntrinsics.translation = visionx::tools::convertEigenVecToVisionX(Eigen::Vector3f::Zero());
315 
316  auto d_params = getCameraIrParameters();
317  depthCameraIntrinsics.principalPoint[0] = d_params.cx;
318  depthCameraIntrinsics.principalPoint[1] = d_params.cy;
319  depthCameraIntrinsics.focalLength[0] = d_params.fx;
320  depthCameraIntrinsics.focalLength[1] = d_params.fy;
321  depthCameraIntrinsics.width = d_width_;
322  depthCameraIntrinsics.height = d_height_;
323 
324 
325  ARMARX_TRACE;
326 
327  calibration.calibrationLeft = visionx::tools::createDefaultMonocularCalibration();
328  calibration.calibrationRight = visionx::tools::createDefaultMonocularCalibration();
329  calibration.calibrationLeft.cameraParam = RGBCameraIntrinsics;
330  calibration.calibrationRight.cameraParam = depthCameraIntrinsics;
331  calibration.rectificationHomographyLeft = visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
332  calibration.rectificationHomographyRight = visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
333  }
334 
335 }
336 
337 
pcl
Definition: pcl_point_operators.cpp:4
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::control::njoint_controller::platform::platform_follower_controller::Registration
const armarx::NJointControllerRegistration< Controller > Registration(NAME)
visionx::CUDA
@ CUDA
Definition: KinectV2PointCloudProvider.h:56
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
trace.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
visionx::CPU
@ CPU
Definition: KinectV2PointCloudProvider.h:56
Color
uint32_t Color
RGBA color.
Definition: color.h:8
visionx::tools::convertEigenVecToVisionX
visionx::types::Vec convertEigenVecToVisionX(Eigen::VectorXf v)
Definition: TypeMapping.cpp:655
visionx::OPENCLKDE
@ OPENCLKDE
Definition: KinectV2PointCloudProvider.h:56
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
visionx::OPENCL
@ OPENCL
Definition: KinectV2PointCloudProvider.h:56
TimeUtil.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
IceUtil::Handle< class PropertyDefinitionContainer >
visionx::tools::convertEigenMatToVisionX
visionx::types::Mat convertEigenMatToVisionX(Eigen::MatrixXf m)
Definition: TypeMapping.cpp:677
KinectToIVTHelper
Definition: KinectHelpers.h:133
KinectV2PointCloudProvider.h
KinectToPCLHelper
Definition: KinectHelpers.h:37
ImageUtil.h
cv
Definition: helper.h:35
TypeMapping.h
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
visionx::OPENGL
@ OPENGL
Definition: KinectV2PointCloudProvider.h:56
visionx::CUDAKDE
@ CUDAKDE
Definition: KinectV2PointCloudProvider.h:56
visionx::tools::createDefaultMonocularCalibration
MonocularCalibration createDefaultMonocularCalibration()
Creates a MonocularCalibration with all parameters set to a neutral value.
Definition: ImageUtil.cpp:237
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
visionx::KinectV2PointCloudProviderPropertyDefinitions
Definition: KinectV2PointCloudProvider.h:63