KinectV1PointCloudProvider.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::KinectV1PointCloudProvider
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 
30 
31 #include <Image/ImageProcessor.h>
32 #include <Calibration/Calibration.h>
33 
34 #include <pcl/io/pcd_io.h>
35 
36 
37 using namespace armarx;
38 using namespace pcl;
39 using namespace cv;
40 
41 namespace visionx
42 {
43  void KinectV1PointCloudProvider::onInitComponent()
44  {
46  ARMARX_VERBOSE << "onInitComponent()";
47 
48  ImageProvider::onInitComponent();
49  CapturingPointCloudProvider::onInitComponent();
50  }
51 
52  void KinectV1PointCloudProvider::onConnectComponent()
53  {
55  ARMARX_VERBOSE << "onConnectComponent()";
56 
57  ImageProvider::onConnectComponent();
58  CapturingPointCloudProvider::onConnectComponent();
59  }
60 
61  void KinectV1PointCloudProvider::onDisconnectComponent()
62  {
64  // hack to prevent deadlock
65  captureEnabled = false;
66  ImageProvider::onDisconnectComponent();
67  CapturingPointCloudProvider::onDisconnectComponent();
68  }
69 
70  void KinectV1PointCloudProvider::onExitComponent()
71  {
73  ARMARX_VERBOSE << "onExitComponent()";
74 
75  ImageProvider::onExitComponent();
76  CapturingPointCloudProvider::onExitComponent();
77  ARMARX_INFO << "Resetting grabber interface";
78  }
79 
80  void KinectV1PointCloudProvider::onInitCapturingPointCloudProvider()
81  {
83 
84 
85  ARMARX_VERBOSE << "onInitCapturingPointCloudProvider()";
86  // from https://jasonchu1313.github.io/2017/10/01/kinect-calibration/
87  cx_ = 343.65;
88  cy_ = 229.81;
89  fx_ = 458.46;
90  fy_ = 458.20;
91  device_ = &freenect_.createDevice<KinectV1Device>(0);
92  }
93 
94 
95  void KinectV1PointCloudProvider::onStartCapture(float frameRate)
96  {
97  device_->startVideo();
98  device_->startDepth();
99  }
100 
101 
102  void KinectV1PointCloudProvider::onStopCapture()
103  {
104  ARMARX_TRACE;
105  ARMARX_INFO << "Stopping KinectV1 Grabber Interface";
106  device_->stopVideo();
107  device_->stopDepth();
108  ARMARX_INFO << "Stopped KinectV1 Grabber Interface";
109  }
110 
111 
112  void KinectV1PointCloudProvider::onExitCapturingPointCloudProvider()
113  {
114  ARMARX_TRACE;
115  ARMARX_VERBOSE << "onExitCapturingPointCloudProvider()";
116 
117  delete rgbImages_[0];
118  delete rgbImages_[1];
119 
120  delete[] rgbImages_;
121  }
122 
123 
124  void KinectV1PointCloudProvider::onInitImageProvider()
125  {
126  ARMARX_TRACE;
127  // init images
128  width_ = 640;
129  height_ = 480;
130  rgbImages_ = new CByteImage *[2];
131  size_ = width_ * height_;
132  rgbImages_[0] = new CByteImage(width_, height_, CByteImage::eRGB24);
133  rgbImages_[1] = new CByteImage(width_, height_, CByteImage::eRGB24);
134  ::ImageProcessor::Zero(rgbImages_[0]);
135  ::ImageProcessor::Zero(rgbImages_[1]);
136  Eigen::Vector2i dimensions = Eigen::Vector2i(width_, height_);
137  setImageFormat(visionx::ImageDimension(dimensions(0), dimensions(1)), visionx::eRgb);
138  setNumberImages(2);
139  }
140 
141 
142  bool KinectV1PointCloudProvider::doCapture()
143  {
144  static std::vector<uint8_t> rgb(size_ * 3);
145  static std::vector<uint16_t> depth(size_);
146  // float f = 595.f;
147  if (!device_->getRGB(rgb) || device_->getDepth(depth))
148  {
149  return false;
150  };
151  pcl::PointCloud<PointT>::Ptr outputCloud = pcl::PointCloud<PointT>::Ptr(new pcl::PointCloud<PointT>());
152  outputCloud->resize(size_);
153  for (int i = 0; i < size_; i++)
154  {
155  // color
156  int index = 3 * i;
157  for (int j = 0; j < 3; j++)
158  {
159  rgbImages_[0]->pixels[index + j] = rgb[index + j];
160  }
161  visionx::tools::depthValueToRGB((int)depth[i], rgbImages_[1]->pixels[index + 0], rgbImages_[1]->pixels[index + 1],
162  rgbImages_[1]->pixels[index + 2], false);
163  // X = (x - cx) * d / fx
164  // Y = (y - cy) * d / fy
165  // Z = d
166  // TODO: precalculate some of this stuff for efficiency
167  // outputCloud->points[i].x = (i % width_ - (width_ - 1) / 2.f) * depth[i] / f;
168  // outputCloud->points[i].y = (i / width_ - (height_ - 1) / 2.f) * depth[i] / f;
169  outputCloud->points[i].x = (i % width_ - cx_ + 0.5) * depth[i] / fx_;
170  outputCloud->points[i].y = (i / width_ - cy_ + 0.5) * depth[i] / fy_;
171  outputCloud->points[i].z = depth[i];
172  outputCloud->points[i].r = rgb[index];
173  outputCloud->points[i].g = rgb[index + 1];
174  outputCloud->points[i].b = rgb[index + 2];
175  }
176  IceUtil::Time ts = IceUtil::Time::now();
177  outputCloud->header.stamp = ts.toMicroSeconds();
178  providePointCloud<PointT>(outputCloud);
179  provideImages(rgbImages_, ts);
180  return true;
181  }
182 
183 
184  PropertyDefinitionsPtr KinectV1PointCloudProvider::createPropertyDefinitions()
185  {
187  }
188 
189  StereoCalibration KinectV1PointCloudProvider::getStereoCalibration(const Ice::Current& c)
190  {
191  return calibration;
192  }
193 
194 }
195 
196 
pcl
Definition: pcl_point_operators.cpp:4
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
index
uint8_t index
Definition: EtherCATFrame.h:59
KinectV1PointCloudProvider.h
trace.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
KinectV1Device
Definition: KinectV1Device.hpp:14
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
visionx::tools::depthValueToRGB
void depthValueToRGB(unsigned int depthInMM, unsigned char &r, unsigned char &g, unsigned char &b, bool noiseResistant=false)
Definition: ImageUtil.h:176
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
visionx::KinectV1PointCloudProviderPropertyDefinitions
Definition: KinectV1PointCloudProvider.h:56
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
IceUtil::Handle< class PropertyDefinitionContainer >
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
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28