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