AzureKinectIRImageProvider.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package VisionX::ArmarXObjects::AzureKinectIRImageProvider
17  * @author Mirko W├Ąchter
18  * @author Christian R. G. Dreher <c.dreher@kit.edu>
19  * @date 2019
20  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
21  * GNU General Public License
22  */
23 
24 
26 
27 
28 // IVT
29 #include <Image/ImageProcessor.h>
30 #include <Calibration/Calibration.h>
31 
32 // ArmarX
37 
38 // VisionX
42 
43 namespace
44 {
45  /**
46  * @brief Converts a k4a image to an IVT image.
47  * @param color_image
48  * @param result
49  */
50  void
51  k4aToIvtImage(const k4a::image& color_image, ::CByteImage& result)
52  {
53  auto cw = static_cast<unsigned int>(color_image.get_width_pixels());
54  auto ch = static_cast<unsigned int>(color_image.get_height_pixels());
55  ARMARX_CHECK_EQUAL(static_cast<unsigned int>(result.width), cw);
56  ARMARX_CHECK_EQUAL(static_cast<unsigned int>(result.height), ch);
57  auto color_buffer = reinterpret_cast<const unsigned char*>(color_image.get_buffer());
58  auto rgb_buffer_ivt = result.pixels;
59 
60  int index_ivt = 0;
61  int index_k4a = 0;
62  for (unsigned int y = 0; y < ch; ++y)
63  {
64  for (unsigned int x = 0; x < cw; ++x)
65  {
66  rgb_buffer_ivt[index_ivt] = float(color_buffer[index_k4a] + color_buffer[index_k4a + 1]) / 2.f;
67  index_ivt += 1;
68  index_k4a += 2;
69  }
70  }
71  }
72 }
73 
74 
75 namespace visionx
76 {
77 
80  {
81  }
82 
83 
85  {
87  return defs;
88  }
89 
90  void
91  AzureKinectIRImageProvider::onStartCapture(float /* framesPerSecond */)
92  {
93  // Check for devices.
94  const uint32_t DEVICE_COUNT = k4a::device::get_installed_count();
95  if (DEVICE_COUNT == 0)
96  {
97  getArmarXManager()->asyncShutdown();
98  throw armarx::LocalException("No Azure Kinect devices detected!");
99  }
100 
101  device = k4a::device::open(K4A_DEVICE_DEFAULT);
102  k4aCalibration = device.get_calibration(config.depth_mode, config.color_resolution);
103 
104  transformation = k4a::transformation(k4aCalibration);
105 
106  device.start_cameras(&config);
107 
108  setMetaInfo("serialNumber", new armarx::Variant(device.get_serialnum()));
109  setMetaInfo("rgbVersion", new armarx::Variant(VersionToString(device.get_version().rgb)));
110  setMetaInfo("depthVersion", new armarx::Variant(VersionToString(device.get_version().depth)));
111  setMetaInfo("depthSensorVersion", new armarx::Variant(VersionToString(device.get_version().depth_sensor)));
112  setMetaInfo("audioVersion", new armarx::Variant(VersionToString(device.get_version().audio)));
113  }
114 
115 
116  bool
117  AzureKinectIRImageProvider::capture(void** pp_image_buffers)
118  {
120 
121  k4a::capture capture;
122  const std::chrono::milliseconds TIMEOUT{1000};
123 
124  bool status;
125  try
126  {
127  status = device.get_capture(&capture, TIMEOUT);
128  }
129  catch (const std::exception&)
130  {
131  ARMARX_WARNING << "Failed to get capture from device. Restarting camera.";
132  StopWatch sw;
133  device.stop_cameras();
134  device.start_cameras(&config);
135  ARMARX_INFO << "Restarting took " << sw.stop() << ".";
136  return false;
137  }
138 
139  if (status)
140  {
141  setMetaInfo("temperature", new armarx::Variant(capture.get_temperature_c()));
142 
143  const k4a::image irImage = capture.get_ir_image();
144  CByteImage buffer(depthDim.first, depthDim.second, CByteImage::eGrayScale);
145  k4aToIvtImage(irImage, buffer);
146 
147  std::memcpy(pp_image_buffers[0], buffer.pixels, 1024 * 1024);
148  return true;
149  }
150  return false;
151  }
152 
153 
154 
155 
156  std::string
158  {
159  return "AzureKinectIRImageProvider";
160  }
161 
162 
164  {
165  config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
166  auto fps = getProperty<float>("framerate").getValue();
167  if (fps == 5.0f)
168  {
169  config.camera_fps = K4A_FRAMES_PER_SECOND_5;
170  }
171  else if (fps == 15.0f)
172  {
173  config.camera_fps = K4A_FRAMES_PER_SECOND_15;
174  }
175  else if (fps == 30.0f)
176  {
177  config.camera_fps = K4A_FRAMES_PER_SECOND_30;
178  }
179  else
180  {
181  throw armarx::LocalException("Invalid framerate: ") << fps << " - Only framerates 5, 15 and 30 are "
182  << "supported by Azure Kinect.";
183  }
184  frameRate = fps;
185  config.depth_mode = K4A_DEPTH_MODE_PASSIVE_IR;
186  config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
187  config.color_resolution = K4A_COLOR_RESOLUTION_1080P;
188 
189  // This means that we'll only get captures that have both color and depth images, so we don't
190  // need to check if the capture contains a particular type of image.
191  config.synchronized_images_only = true;
192 
193  setNumberImages(1);
194  depthDim = GetDepthDimensions(config.depth_mode);
195  auto color_dim = GetColorDimensions(config.color_resolution);
196  ARMARX_INFO << "Depth image size: " << depthDim.first << "x" << depthDim.second;
197  ARMARX_INFO << "Color image size: " << color_dim.first << "x" << color_dim.second;
198 
200  visionx::ImageDimension(depthDim.first, depthDim.second),
201  visionx::eGrayScale,
202  visionx::eBayerPatternGr);
203  resultIRImage.reset(visionx::tools::createByteImage(getImageFormat(), visionx::eRgb));
204  }
205 
207  {
208 
209  }
210 
212  {
213 
214  }
215 
216  std::string AzureKinectIRImageProvider::getReferenceFrame(const Ice::Current& current)
217  {
218  return std::string();
219  }
220 
221 
222  bool AzureKinectIRImageProvider::getImagesAreUndistorted(const Ice::Current& current)
223  {
224  return false;
225  }
226 
227  StereoCalibration
229  {
230  using namespace Eigen;
231  using Matrix3fRowMajor = Matrix<float, 3, 3, StorageOptions::RowMajor>;
233 
234  const auto convert_calibration = [](const k4a_calibration_camera_t& k4a_calib,
235  float scale = 1.)
236  {
237  MonocularCalibration calibration;
238 
239  const k4a_calibration_intrinsic_parameters_t& params = k4a_calib.intrinsics.parameters;
240  calibration.cameraParam.principalPoint = {params.param.cx * scale,
241  params.param.cy* scale
242  };
243  calibration.cameraParam.focalLength = {params.param.fx * scale,
244  params.param.fy* scale
245  };
246  // TODO: Figure out convertions. IVT (Calibration.h) expects 4 parameters:
247  // - The first radial lens distortion parameter.
248  // - The second radial lens distortion parameter.
249  // - The first tangential lens distortion parameter.
250  // - The second tangential lens distortion parameter.
251  // However, the Kinect offers k1-k6 radial distortion coefficients and 2 p1-p2
252  // tangential distortion parameters, which means that k3-k6 are unused.
253  // It is even unclear whether this is correct now, as previously it was a vector of all
254  // 6 k-params, which resulted in failed assertions in TypeMapping.cpp in the function
255  // CStereoCalibration* visionx::tools::convert(const visionx::StereoCalibration& stereoCalibration)
256  // lin 314 at time of this commit.
257  // See: https://microsoft.github.io/Azure-Kinect-Sensor-SDK/master/structk4a__calibration__intrinsic__parameters__t_1_1__param.html
258  calibration.cameraParam.distortion = { params.param.k1, params.param.k2,
259  params.param.p1, params.param.p2,
260  };
261  calibration.cameraParam.width = k4a_calib.resolution_width;
262  calibration.cameraParam.height = k4a_calib.resolution_height;
263  const Matrix3fRowMajor ROTATION =
264  Map<const Matrix3fRowMajor> {k4a_calib.extrinsics.rotation};
265  calibration.cameraParam.rotation = convertEigenMatToVisionX(ROTATION);
266  calibration.cameraParam.translation = { k4a_calib.extrinsics.translation,
267  k4a_calib.extrinsics.translation + 3
268  };
269 
270  return calibration;
271  };
272 
273  StereoCalibration calibration;
274 
275  calibration.calibrationLeft = calibration.calibrationRight = convert_calibration(k4aCalibration.color_camera_calibration);
276 
277  calibration.rectificationHomographyLeft = calibration.rectificationHomographyRight = convertEigenMatToVisionX(Matrix3f::Identity());
278 
279 
280  return calibration;
281  }
282 
283 
284 
285 }
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
Eigen
Definition: Elements.h:36
armarx::ManagedIceObject::setMetaInfo
void setMetaInfo(const std::string &id, const VariantBasePtr &value)
Allows to set meta information that can be queried live via Ice interface on the ArmarXManager.
Definition: ManagedIceObject.cpp:749
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
visionx::CapturingImageProvider::frameRate
float frameRate
Required frame rate.
Definition: CapturingImageProvider.h:198
StopWatch.h
visionx::AzureKinectIRImageProvider::getReferenceFrame
std::string getReferenceFrame(const Ice::Current &current) override
Definition: AzureKinectIRImageProvider.cpp:216
visionx::AzureKinectIRImageProvider::onStopCapture
void onStopCapture() override
This is called when the image provider capturing has been stopped.
Definition: AzureKinectIRImageProvider.cpp:211
armarx::ManagedIceObject::getArmarXManager
ArmarXManagerPtr getArmarXManager() const
Returns the ArmarX manager used to add and remove components.
Definition: ManagedIceObject.cpp:348
trace.h
AzureKinectIRImageProvider.h
visionx::AzureKinectIRImageProviderPropertyDefinitions::AzureKinectIRImageProviderPropertyDefinitions
AzureKinectIRImageProviderPropertyDefinitions(std::string prefix)
Definition: AzureKinectIRImageProvider.cpp:78
visionx::AzureKinectIRImageProvider::getDefaultName
virtual std::string getDefaultName() const override
Definition: AzureKinectIRImageProvider.cpp:157
visionx::AzureKinectIRImageProvider::GetColorDimensions
static std::pair< int, int > GetColorDimensions(const k4a_color_resolution_t resolution)
Definition: AzureKinectIRImageProvider.h:114
visionx::ImageProvider::setImageFormat
void setImageFormat(ImageDimension imageDimension, ImageType imageType, BayerPatternType bayerPatternType=visionx::eBayerPatternRg)
Sets the image basic format data.
Definition: ImageProvider.cpp:275
visionx::ImageProvider::getImageFormat
ImageFormatInfo getImageFormat(const Ice::Current &c=Ice::emptyCurrent) override
Returns the entire image format info struct via Ice.
Definition: ImageProvider.cpp:75
visionx::tools::createByteImage
CByteImage * createByteImage(const ImageFormatInfo &imageFormat, const ImageType imageType)
Creates a ByteImage for the destination type specified in the given imageProviderInfo.
visionx::AzureKinectIRImageProvider::getImagesAreUndistorted
bool getImagesAreUndistorted(const Ice::Current &current) override
Definition: AzureKinectIRImageProvider.cpp:222
armarx::status
status
Definition: FiniteStateMachine.h:259
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
visionx::AzureKinectIRImageProvider::getStereoCalibration
StereoCalibration getStereoCalibration(const Ice::Current &current) override
Definition: AzureKinectIRImageProvider.cpp:228
visionx::AzureKinectIRImageProvider::onExitCapturingImageProvider
void onExitCapturingImageProvider() override
This is called when the Component::onExitComponent() setup is called.
Definition: AzureKinectIRImageProvider.cpp:206
visionx::AzureKinectIRImageProvider::createPropertyDefinitions
virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: AzureKinectIRImageProvider.cpp:84
visionx::CapturingPointCloudProviderPropertyDefinitions
Definition: CapturingPointCloudProvider.h:38
visionx::CapturingImageProvider::capture
virtual void capture()
Definition: CapturingImageProvider.cpp:106
visionx::AzureKinectIRImageProvider::VersionToString
static std::string VersionToString(const k4a_version_t &version)
Definition: AzureKinectIRImageProvider.h:158
visionx::AzureKinectIRImageProvider::onInitCapturingImageProvider
void onInitCapturingImageProvider() override
This is called when the Component::onInitComponent() is called.
Definition: AzureKinectIRImageProvider.cpp:163
visionx::ImageProvider::setNumberImages
void setNumberImages(int numberImages)
Sets the number of images on each capture.
Definition: ImageProvider.cpp:313
visionx::AzureKinectIRImageProviderPropertyDefinitions
Definition: AzureKinectIRImageProvider.h:54
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
TimeUtil.h
float
#define float
Definition: 16_Level.h:22
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
visionx::AzureKinectIRImageProvider::onStartCapture
void onStartCapture(float frames_per_second) override
This is called when the image provider capturing has been started.
Definition: AzureKinectIRImageProvider.cpp:91
IceUtil::Handle< class PropertyDefinitionContainer >
visionx::tools::convertEigenMatToVisionX
visionx::types::Mat convertEigenMatToVisionX(Eigen::MatrixXf m)
Definition: TypeMapping.cpp:667
ImageUtil.h
Eigen::Matrix
Definition: EigenForwardDeclarations.h:27
armarx::core::time::StopWatch
Measures the passed time between the construction or calling reset() and stop().
Definition: StopWatch.h:42
TypeMapping.h
visionx::AzureKinectIRImageProvider::GetDepthDimensions
static std::pair< int, int > GetDepthDimensions(const k4a_depth_mode_t depth_mode)
Definition: AzureKinectIRImageProvider.h:138
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
helper.h
Variant.h
scene3D::ROTATION
@ ROTATION
Definition: ManipulatorMode.h:36