RCPointCloudProvider.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::RCPointCloudProvider
17  * @author Markus Grotz ( markus dot grotz at kit dot edu )
18  * @date 2017
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "RCPointCloudProvider.h"
24 
25 #include <pcl/common/transforms.h>
26 
27 #include <rc_genicam_api/system.h>
28 #include <rc_genicam_api/interface.h>
29 #include <rc_genicam_api/buffer.h>
30 #include <rc_genicam_api/image.h>
31 #include <rc_genicam_api/config.h>
32 #include <rc_genicam_api/pixel_formats.h>
33 
34 #include <VirtualRobot/MathTools.h>
35 
39 
40 
41 #include <Calibration/Calibration.h>
42 #include <Calibration/StereoCalibration.h>
43 #include <Image/ImageProcessor.h>
44 #include <stdlib.h>
45 
47 
48 
49 using namespace visionx;
50 namespace visionx
51 {
52 
53  using Point = pcl::PointXYZRGBA;
54  using PointCloud = pcl::PointCloud<Point>;
55 
56  // Our installed version is missing the getColor method
57  // From: https://github.com/roboception/rc_genicam_api/blob/f7f1621fc3ac5d638292e3d5d1ea9393959ecd9d/rc_genicam_api/image.cc
58  static void rcg_getColor(uint8_t rgb[3], const std::shared_ptr<const rcg::Image>& img,
59  uint32_t ds, uint32_t i, uint32_t k)
60  {
61  i *= ds;
62  k *= ds;
63 
64  if (img->getPixelFormat() == Mono8) // convert from monochrome
65  {
66  size_t lstep = img->getWidth() + img->getXPadding();
67  const uint8_t* p = img->getPixels() + k * lstep + i;
68 
69  uint32_t g = 0, n = 0;
70 
71  for (uint32_t kk = 0; kk < ds; kk++)
72  {
73  for (uint32_t ii = 0; ii < ds; ii++)
74  {
75  g += p[ii];
76  n++;
77  }
78 
79  p += lstep;
80  }
81 
82  rgb[2] = rgb[1] = rgb[0] = static_cast<uint8_t>(g / n);
83  }
84  else if (img->getPixelFormat() == YCbCr411_8) // convert from YUV
85  {
86  size_t lstep = (img->getWidth() >> 2) * 6 + img->getXPadding();
87  const uint8_t* p = img->getPixels() + k * lstep;
88 
89  uint32_t r = 0;
90  uint32_t g = 0;
91  uint32_t b = 0;
92  uint32_t n = 0;
93 
94  for (uint32_t kk = 0; kk < ds; kk++)
95  {
96  for (uint32_t ii = 0; ii < ds; ii++)
97  {
98  uint8_t v[3];
99  rcg::convYCbCr411toRGB(v, p, static_cast<int>(i + ii));
100 
101  r += v[0];
102  g += v[1];
103  b += v[2];
104  n++;
105  }
106 
107  p += lstep;
108  }
109 
110  rgb[0] = static_cast<uint8_t>(r / n);
111  rgb[1] = static_cast<uint8_t>(g / n);
112  rgb[2] = static_cast<uint8_t>(b / n);
113  }
114  }
115 
116  // Adapted from: https://github.com/roboception/rc_genicam_api/blob/master/tools/gc_pointcloud.cc
117  static void storePointCloud(double f, double t, double scale,
118  std::shared_ptr<const rcg::Image> intensityCombined,
119  std::shared_ptr<const rcg::Image> disp,
120  PointCloud* outPoints)
121  {
122  // intensityCombined: Top half is the left image, bottom half the right image
123  // get size and scale factor between left image and disparity image
124 
125  size_t width = disp->getWidth();
126  size_t height = disp->getHeight();
127  bool bigendian = disp->isBigEndian();
128  size_t leftWidth = intensityCombined->getWidth();
129  size_t ds = (leftWidth + disp->getWidth() - 1) / disp->getWidth();
130 
131  // convert focal length factor into focal length in (disparity) pixels
132 
133  f *= width;
134 
135  // get pointer to disparity data and size of row in bytes
136 
137  const uint8_t* dps = disp->getPixels();
138  size_t dstep = disp->getWidth() * sizeof(uint16_t) + disp->getXPadding();
139 
140  // count number of valid disparities
141 
142  int n = 0;
143  for (size_t k = 0; k < height; k++)
144  {
145  int j = 0;
146  for (size_t i = 0; i < width; i++)
147  {
148  if ((dps[j] | dps[j + 1]) != 0)
149  {
150  n++;
151  }
152  j += 2;
153  }
154 
155  dps += dstep;
156  }
157 
158  dps = disp->getPixels();
159 
160  outPoints->clear();
161  outPoints->reserve(height * width);
162 
163  // create point cloud
164 
165  for (size_t k = 0; k < height; k++)
166  {
167  for (size_t i = 0; i < width; i++)
168  {
169  // convert disparity from fixed comma 16 bit integer into float value
170 
171  double d;
172  if (bigendian)
173  {
174  size_t j = i << 1;
175  d = scale * ((dps[j] << 8) | dps[j + 1]);
176  }
177  else
178  {
179  size_t j = i << 1;
180  d = scale * ((dps[j + 1] << 8) | dps[j]);
181  }
182 
183  // if disparity is valid and color can be obtained
184 
185  if (d > 0)
186  {
187  // reconstruct 3D point from disparity value
188 
189  double x = (i + 0.5 - 0.5 * width) * t / d;
190  double y = (k + 0.5 - 0.5 * height) * t / d;
191  double z = f * t / d;
192 
193  // compute size of reconstructed point
194 
195  // double x2=(i-0.5*width)*t/d;
196  // double size=2*1.4*std::abs(x2-x); // unused
197 
198  // get corresponding color value
199 
200  uint8_t rgb[3] = {0, 0, 0 };
201  // FIXME: Replace with rcg::getColor once available
202  ::rcg_getColor(rgb, intensityCombined, static_cast<uint32_t>(ds), static_cast<uint32_t>(i),
203  static_cast<uint32_t>(k));
204 
205  // store colored point
206  Point point;
207  const double meterToMillimeter = 1000.0f;
208  point.x = x * meterToMillimeter;
209  point.y = y * meterToMillimeter;
210  point.z = z * meterToMillimeter;
211  point.r = rgb[0];
212  point.g = rgb[1];
213  point.b = rgb[2];
214  point.a = 255;
215 
216  outPoints->push_back(point);
217  }
218  }
219 
220  dps += dstep;
221  }
222  }
223 
225  : intensityBuffer(50)
226  , disparityBuffer(25)
227  {
228 
229  }
230 
232  {
235 
236  updateFinalCloudTransform(
237  getProperty<float>("PointCloud_Scale_X").getValue(),
238  getProperty<float>("PointCloud_Scale_Y").getValue(),
239  getProperty<float>("PointCloud_Scale_Z").getValue(),
240  getProperty<float>("PointCloud_Rotate_Roll").getValue(),
241  getProperty<float>("PointCloud_Rotate_Pitch").getValue(),
242  getProperty<float>("PointCloud_Rotate_Yaw").getValue(),
243  getProperty<float>("PointCloud_Translate_X").getValue(),
244  getProperty<float>("PointCloud_Translate_Y").getValue(),
245  getProperty<float>("PointCloud_Translate_Z").getValue());
246  }
247 
249  {
253  {
254  processGui(prx);
255  });
256  }
257 
259  {
262  }
263 
265  {
268  }
269 
271  {
272  using namespace visionx::roboception;
273 
274  if (!initDevice(getProperty<std::string>("DeviceId")))
275  {
276  getArmarXManager()->asyncShutdown();
277  return;
278  }
279  enableIntensity(false);
281  enableDisparity(true);
282  enableConfidence(false);
283  enableError(false);
284 
285  rcg::setString(nodemap, GC_COMPONENT_SELECTOR, "IntensityCombined", true);
286 
287  // Color format
288  if (rcg::setEnum(nodemap, "PixelFormat", "YCbCr411_8", false))
289  {
290  ARMARX_INFO << "Enabling color mode";
291  }
292  else
293  {
294  ARMARX_INFO << "Falling back to monochrome mode";
295  }
296 
298  getProperty<float>("ScaleFactor"),
299  "/tmp/armar6_rc_point_cloud_calibration.txt"
300  );
301 
302  std::string depthQuality = getProperty<std::string>("DepthImageResolution").getValue();
303  try
304  {
305  rcg::setEnum(nodemap, GC_DEPTH_QUALITY, depthQuality.c_str(), true);
306  }
307  catch (std::invalid_argument& e)
308  {
309  ARMARX_INFO << e.what();
310  ARMARX_WARNING << "Selected DepthImageResolution '" << depthQuality.c_str() << "' is currently not available for this device. "
311  << "Falling back to '" << GC_DEPTH_QUALITY_HIGH << "' (640x480@3FPS).";
312  rcg::setEnum(nodemap, GC_DEPTH_QUALITY, GC_DEPTH_QUALITY_HIGH, true);
313  }
314  ARMARX_INFO << "Setting depth quality to " << rcg::getEnum(nodemap, GC_DEPTH_QUALITY) << " (out of Low, Medium, High, Full)";
316 
317  frameRate = std::min(frameRate, getProperty<float>("framerate").getValue());
318  rcg::setFloat(nodemap, "AcquisitionFrameRate", frameRate);
320 
321  scan3dCoordinateScale = rcg::getFloat(nodemap, "Scan3dCoordinateScale", nullptr, nullptr, true);
322 
324  setImageFormat(dimensions, visionx::eRgb, visionx::eBayerPatternRg);
325  setPointCloudSyncMode(visionx::eCaptureSynchronization);
326  }
327 
329  {
330  cleanupRC();
331  }
332 
333  void RCPointCloudProvider::onStartCapture(float framesPerSecond)
334  {
335  startRC();
336  }
337 
339  {
340  stopRC();
341  }
342 
344  {
345  const rcg::Buffer* buffer = stream->grab(10000);
346  if (!buffer)
347  {
348  ARMARX_WARNING << deactivateSpam(10) << "Unable to get image";
349  return false;
350  }
351 
352  // check for a complete image in the buffer
353 
354  if (buffer->getIsIncomplete() || !buffer->getImagePresent(0))
355  {
356  ARMARX_INFO << deactivateSpam(1) << "Waiting for image";
357  return false;
358  }
359 
360  // store image in the corresponding list
361 
362  uint64_t pixelformat = buffer->getPixelFormat(0);
363  if (pixelformat == Mono8 || pixelformat == YCbCr411_8)
364  {
365  intensityBuffer.add(buffer, 0);
366  }
367  else if (pixelformat == Coord3D_C16)
368  {
369  disparityBuffer.add(buffer, 0);
370  }
371  else
372  {
373  ARMARX_WARNING << "Unexpected pixel format: " << pixelformat;
374  return false;
375  }
376  // check if both lists contain an image with the current timestamp
377 
378  uint64_t timestamp = buffer->getTimestampNS();
379 
380  std::shared_ptr<const rcg::Image> intensityCombined = intensityBuffer.find(timestamp);
381  std::shared_ptr<const rcg::Image> disparity = disparityBuffer.find(timestamp);
382 
383  if (!intensityCombined || !disparity)
384  {
385  // Waiting for second synchronized image
386  return false;
387  }
388  updateTimestamp(buffer->getTimestamp());
389 
390  // compute and store point cloud from synchronized image pair
391 
392  PointCloud::Ptr pointCloud(new PointCloud());
393  storePointCloud(focalLengthFactor, baseline, scan3dCoordinateScale, intensityCombined, disparity, pointCloud.get());
394 
395 
396  // Provide images
397  {
398  visionx::ImageFormatInfo imageFormat = getImageFormat();
399 
400  // ARMARX_LOG << "grabbing image " << imageFormat.dimension.width << "x" << imageFormat.dimension.height << " vs " << buffer->getWidth() << "x" << buffer->getHeight();
401 
402  int width = imageFormat.dimension.width;
403  int height = imageFormat.dimension.height;
404  if (scaleFactor > 1.0)
405  {
406  width *= scaleFactor;
407  height *= scaleFactor;
408  }
409 
410  //const int imageSize = imageFormat.dimension.width * imageFormat.dimension.height * imageFormat.bytesPerPixel;
411  const uint8_t* p = intensityCombined->getPixels();
412 
413  size_t px = intensityCombined->getXPadding();
414  uint64_t format = intensityCombined->getPixelFormat();
415 
416  for (size_t n = 0; n < numImages; n++)
417  {
418  //fillCameraImageRGB(cameraImages[n]->pixels, n, *intensityCombined);
419  fillCameraImageRGB(cameraImages[n]->pixels, n, p, width, height, format, px);
420  }
421 
422 
423  if (scaleFactor > 1.0)
424  {
425  ::ImageProcessor::Resize(cameraImages[0], smallImages[0]);
426  ::ImageProcessor::Resize(cameraImages[1], smallImages[1]);
428 
429  // armarx::SharedMemoryScopedWriteLockPtr lock(this->ImageProvider::sharedMemoryProvider->getScopedWriteLock());
430 
431  // for (size_t i = 0; i < numImages; i++)
432  // {
433  // memcpy(ppImageBuffers[i], smallImages[i]->pixels, imageSize);
434  // }
435  }
436  else
437  {
439  // for (size_t i = 0; i < numImages; i++)
440  // {
441  // memcpy(ppImageBuffers[i], cameraImages[i]->pixels, imageSize);
442  // }
443  }
444  }
445 
446  PointCloud::Ptr transformedPointCloud(new PointCloud());
447  const auto& tr = getFinalCloudTransform();
448  ARMARX_DEBUG << deactivateSpam(0.5) << "final transform =\n" << tr;
449  pcl::transformPointCloud(*pointCloud, *transformedPointCloud, tr);
450 
451  providePointCloud(transformedPointCloud);
452 
453  // remove all images from the buffer with the current or an older time stamp
454  intensityBuffer.removeOld(timestamp);
455  disparityBuffer.removeOld(timestamp);
456 
457  return true;
458  }
459 
460 
462  {
465  }
466 
467 
468 
470  {
471  throw std::runtime_error("Not yet implemented");
472  }
473 
474 
476  {
477  }
478 
480  {
481  }
482 
484  armarx::ComponentPropertyDefinitions(prefix)
485  {
486  using namespace visionx::roboception;
487 
488  defineOptionalProperty<std::string>("DeviceId", "00_14_2d_2c_6e_ed", "");
489  defineOptionalProperty<float>("ScaleFactor", 2.0, "Image down scale factor");
490  defineOptionalProperty<float>("framerate", 25.0f, "Frames per second, actual framerate is equal to min(this framerate, framerate provided by the device)")
491  .setMatchRegex("\\d+(.\\d*)?")
492  .setMin(0.0f)
493  .setMax(25.0f);
494  defineOptionalProperty<bool>("isEnabled", true, "enable the capturing process immediately");
495  defineOptionalProperty<std::string>("DepthImageResolution", "640x480@3Fps", "Resolution of the depth image. "
496  "The higher the resolution, the lower the framerate provided by the device")
497  .setCaseInsensitive(true)
498  .map("214x160@25Fps", GC_DEPTH_QUALITY_LOW)
499  .map("320x240@15Fps", GC_DEPTH_QUALITY_MEDIUM)
500  .map("640x480@3Fps", GC_DEPTH_QUALITY_HIGH)
501  .map("1280x960@0.8Fps", GC_DEPTH_QUALITY_FULL);
502 
503  defineOptionalProperty<std::string>("ReferenceFrameName", "Roboception_LeftCamera", "Optional reference frame name.");
504 
505  defineOptionalProperty<float>("PointCloud_Scale_X", 1.0, "");
506  defineOptionalProperty<float>("PointCloud_Scale_Y", 1.0, "");
507  defineOptionalProperty<float>("PointCloud_Scale_Z", 1.0, "");
508  defineOptionalProperty<float>("PointCloud_Translate_X", 0, "");
509  defineOptionalProperty<float>("PointCloud_Translate_Y", 0, "");
510  defineOptionalProperty<float>("PointCloud_Translate_Z", 0, "");
511  defineOptionalProperty<float>("PointCloud_Rotate_Roll", 0, "");
512  defineOptionalProperty<float>("PointCloud_Rotate_Pitch", 0, "");
513  defineOptionalProperty<float>("PointCloud_Rotate_Yaw", 0, "");
514  }
515 
516  armarx::RemoteGui::WidgetPtr RCPointCloudProvider::buildGui()
517  {
518  const auto& srt = finalCloudTransform.getWriteBuffer();;
520  .addTextLabel("Scale x/y/z")
521  .addChild(armarx::RemoteGui::makeFloatSpinBox("sx").decimals(4).min(0).max(1000).value(srt(0)))
522  .addChild(armarx::RemoteGui::makeFloatSpinBox("sy").decimals(4).min(0).max(1000).value(srt(1)))
523  .addChild(armarx::RemoteGui::makeFloatSpinBox("sz").decimals(4).min(0).max(1000).value(srt(2)))
524  .addChild(new armarx::RemoteGui::HSpacer)
525 
526  .addTextLabel("Roll / Pitch / Yaw")
530  .addChild(new armarx::RemoteGui::HSpacer)
531 
532  .addTextLabel("Translate x/y/z")
533  .addChild(armarx::RemoteGui::makeFloatSpinBox("tx").value(srt(6)).min(-1000).max(1000))
534  .addChild(armarx::RemoteGui::makeFloatSpinBox("ty").value(srt(7)).min(-1000).max(1000))
535  .addChild(armarx::RemoteGui::makeFloatSpinBox("tz").value(srt(8)).min(-1000).max(1000))
536  .addChild(new armarx::RemoteGui::HSpacer);
537  }
538  void RCPointCloudProvider::processGui(armarx::RemoteGui::TabProxy& prx)
539  {
540  prx.receiveUpdates();
541  updateFinalCloudTransform(
542  prx.getValue<float>("sx").get(),
543  prx.getValue<float>("sy").get(),
544  prx.getValue<float>("sz").get(),
545  prx.getValue<float>("rx").get(),
546  prx.getValue<float>("ry").get(),
547  prx.getValue<float>("rz").get(),
548  prx.getValue<float>("tx").get(),
549  prx.getValue<float>("ty").get(),
550  prx.getValue<float>("tz").get());
551  }
552 
553  void RCPointCloudProvider::updateFinalCloudTransform(
554  float sx, float sy, float sz,
555  float rx, float ry, float rz,
556  float tx, float ty, float tz)
557  {
558  finalCloudTransform.getWriteBuffer()(0) = sx;
559  finalCloudTransform.getWriteBuffer()(1) = sy;
560  finalCloudTransform.getWriteBuffer()(2) = sz;
561 
562  finalCloudTransform.getWriteBuffer()(3) = rx;
563  finalCloudTransform.getWriteBuffer()(4) = ry;
564  finalCloudTransform.getWriteBuffer()(5) = rz;
565 
566  finalCloudTransform.getWriteBuffer()(6) = tx;
567  finalCloudTransform.getWriteBuffer()(7) = ty;
568  finalCloudTransform.getWriteBuffer()(8) = tz;
569 
570  finalCloudTransform.commitWrite();
571  }
572 
573  Eigen::Matrix4f RCPointCloudProvider::getFinalCloudTransform()
574  {
575  const auto& srt = finalCloudTransform.getUpToDateReadBuffer();
576  return VirtualRobot::MathTools::posrpy2eigen4f(srt(6), srt(7), srt(8), srt(4), srt(5), srt(6)) *
577  Eigen::Vector4f{srt(0), srt(1), srt(2), 1.f}.asDiagonal();
578  }
579 }
visionx::RoboceptionUser::stream
rcg::StreamPtr stream
Definition: RoboceptionUser.h:89
visionx::roboception::GC_DEPTH_QUALITY_HIGH
const char *const GC_DEPTH_QUALITY_HIGH
Definition: roboception_constants.h:18
visionx::RCPointCloudProvider::doCapture
bool doCapture() override
Main capturing function.
Definition: RCPointCloudProvider.cpp:343
visionx::PointCloudProvider::onConnectComponent
void onConnectComponent() override
Definition: PointCloudProvider.cpp:77
visionx::RoboceptionUser::cleanupRC
void cleanupRC()
Definition: RoboceptionUser.cpp:235
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
MiscLib::Vector::push_back
void push_back(const T &v)
Definition: Vector.h:346
visionx::RoboceptionUser::dimensions
visionx::ImageDimension dimensions
Definition: RoboceptionUser.h:100
visionx::roboception::GC_DEPTH_QUALITY
const char *const GC_DEPTH_QUALITY
Definition: roboception_constants.h:15
visionx::roboception::GC_DEPTH_QUALITY_MEDIUM
const char *const GC_DEPTH_QUALITY_MEDIUM
Definition: roboception_constants.h:17
visionx::CapturingPointCloudProvider::setPointCloudSyncMode
void setPointCloudSyncMode(ImageSyncMode pointCloudSyncMode)
Sets the point cloud synchronization mode.
Definition: CapturingPointCloudProvider.cpp:190
visionx::RoboceptionUser::nodemap
std::shared_ptr< GenApi::CNodeMapRef > nodemap
Definition: RoboceptionUser.h:90
WidgetBuilder.h
armarx::ManagedIceObject::getArmarXManager
ArmarXManagerPtr getArmarXManager() const
Returns the ArmarX manager used to add and remove components.
Definition: ManagedIceObject.cpp:348
visionx::ImageProvider::onExitComponent
void onExitComponent() override
Definition: ImageProvider.cpp:152
armarx::RemoteGui::detail::SimpleGridLayoutBuilder::addChild
SimpleGridLayoutBuilder & addChild(WidgetPtr const &child, int colspan)
Definition: LayoutWidgets.h:121
visionx::RCPointCloudProvider::onExitCapturingPointCloudProvider
void onExitCapturingPointCloudProvider() override
This is called when the Component::onExitComponent() setup is called.
Definition: RCPointCloudProvider.cpp:328
visionx::RoboceptionUser::enableError
void enableError(bool enabled)
Definition: RoboceptionUser.cpp:133
visionx::ImageProvider::setImageFormat
void setImageFormat(ImageDimension imageDimension, ImageType imageType, BayerPatternType bayerPatternType=visionx::eBayerPatternRg)
Sets the image basic format data.
Definition: ImageProvider.cpp:275
visionx::roboception
Definition: roboception_constants.h:7
visionx::roboception::GC_DEPTH_QUALITY_LOW
const char *const GC_DEPTH_QUALITY_LOW
Definition: roboception_constants.h:16
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::roboception::GC_DEPTH_QUALITY_FULL
const char *const GC_DEPTH_QUALITY_FULL
Definition: roboception_constants.h:19
armarx::RemoteGui::makeSimpleGridLayout
detail::SimpleGridLayoutBuilder makeSimpleGridLayout(std::string const &name="")
Definition: LayoutWidgets.h:235
visionx::ImageProvider::updateTimestamp
void updateTimestamp(Ice::Long timestamp, bool threadSafe=true)
Updates the timestamp of the currently captured image.
Definition: ImageProvider.cpp:165
visionx::PointCloudProvider::onDisconnectComponent
void onDisconnectComponent() override
Hook for subclass.
Definition: PointCloudProvider.cpp:95
visionx::PointCloudProvider::onExitComponent
void onExitComponent() override
Definition: PointCloudProvider.cpp:106
visionx::ImageProvider::onDisconnectComponent
void onDisconnectComponent() override
Hook for subclass.
Definition: ImageProvider.cpp:141
visionx::RCPointCloudProvider::onStartCapture
void onStartCapture(float frameRate) override
This is called when the point cloud provider capturing has been started.
Definition: RCPointCloudProvider.cpp:333
armarx::RemoteGui::TabProxy
Definition: WidgetProxy.h:17
visionx::RoboceptionUser::initDevice
bool initDevice(const std::string &dname)
Definition: RoboceptionUser.cpp:57
visionx::RCPointCloudProvider::onExitImageProvider
void onExitImageProvider() override
This is called when the Component::onExitComponent() setup is called.
Definition: RCPointCloudProvider.cpp:479
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
roboception_constants.h
visionx::RCPointCloudProvider::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: RCPointCloudProvider.cpp:231
armarx::RemoteGui::TabProxy::receiveUpdates
void receiveUpdates()
Definition: WidgetProxy.h:131
visionx::CapturingPointCloudProvider::frameRate
float frameRate
Required frame rate.
Definition: CapturingPointCloudProvider.h:214
M_PI
#define M_PI
Definition: MathTools.h:17
visionx::RCPointCloudProvider::startCaptureForNumFrames
void startCaptureForNumFrames(Ice::Int, const Ice::Current &) override
Definition: RCPointCloudProvider.cpp:469
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
visionx::RCPointCloudProvider::onExitComponent
void onExitComponent() override
Hook for subclass.
Definition: RCPointCloudProvider.cpp:264
Point
Definition: PointCloud.h:21
visionx::RCPointCloudProvider::onInitImageProvider
void onInitImageProvider() override
This is called when the Component::onInitComponent() is called.
Definition: RCPointCloudProvider.cpp:475
visionx::RoboceptionUser::enableDisparity
void enableDisparity(bool enabled)
Definition: RoboceptionUser.cpp:121
visionx::PointCloudProvider::onInitComponent
void onInitComponent() override
Definition: PointCloudProvider.cpp:59
visionx::RoboceptionUser::cameraImages
CByteImage ** cameraImages
Definition: RoboceptionUser.h:97
visionx::roboception::DepthQualityToFPS
const std::map< std::string, float > DepthQualityToFPS
Definition: roboception_constants.cpp:6
visionx::RoboceptionUser::baseline
double baseline
Definition: RoboceptionUser.h:95
visionx::RoboceptionUser::numImages
size_t numImages
Definition: RoboceptionUser.h:92
max
T max(T t1, T t2)
Definition: gdiam.h:48
visionx::RCPointCloudProvider::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: RCPointCloudProvider.cpp:461
visionx::roboception::GC_COMPONENT_SELECTOR
const char *const GC_COMPONENT_SELECTOR
Definition: roboception_constants.h:10
armarx::RemoteGui::detail::SimpleGridLayoutBuilder::addTextLabel
SimpleGridLayoutBuilder & addTextLabel(std::string const &text, int colspan)
Definition: LayoutWidgets.h:130
visionx::ImageProvider::provideImages
void provideImages(void **inputBuffers, const IceUtil::Time &imageTimestamp=IceUtil::Time())
send images raw.
Definition: ImageProvider.cpp:319
visionx::RCPointCloudProvider::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: RCPointCloudProvider.cpp:248
visionx::RoboceptionUser::startRC
void startRC()
Definition: RoboceptionUser.cpp:220
armarx::WidgetDescription::WidgetPtr
::IceInternal::Handle<::armarx::WidgetDescription::Widget > WidgetPtr
Definition: NJointControllerBase.h:66
visionx::PointCloud
pcl::PointCloud< Point > PointCloud
Definition: RCPointCloudProvider.cpp:54
visionx::ImageProvider::setNumberImages
void setNumberImages(int numberImages)
Sets the number of images on each capture.
Definition: ImageProvider.cpp:313
armarx::RemoteGui::TabProxy::getValue
ValueProxy< T > getValue(std::string const &name)
Definition: WidgetProxy.h:161
PointCloud
Definition: PointCloud.h:69
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
MiscLib::Vector::reserve
void reserve(size_type s)
Definition: Vector.h:187
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
visionx::PointCloudProvider::providePointCloud
void providePointCloud(PointCloudPtrT pointCloudPtr)
offer the new point cloud.
Definition: PointCloudProvider.h:122
visionx::ImageProvider::onConnectComponent
void onConnectComponent() override
Definition: ImageProvider.cpp:104
RCPointCloudProvider.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
visionx::RCPointCloudProvider::onDisconnectComponent
void onDisconnectComponent() override
Hook for subclass.
Definition: RCPointCloudProvider.cpp:258
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
visionx::RoboceptionUser::enableIntensity
void enableIntensity(bool enabled)
Definition: RoboceptionUser.cpp:109
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::RemoteGui::detail::SimpleGridLayoutBuilder::cols
SimpleGridLayoutBuilder & cols(int n)
Definition: LayoutWidgets.h:108
visionx::ImageProvider::onInitComponent
void onInitComponent() override
Definition: ImageProvider.cpp:88
visionx::RoboceptionUser::enableConfidence
void enableConfidence(bool enabled)
Definition: RoboceptionUser.cpp:127
armarx::VariantType::Int
const VariantTypeId Int
Definition: Variant.h:916
visionx::RoboceptionUser::enableIntensityCombined
void enableIntensityCombined(bool enabled)
Definition: RoboceptionUser.cpp:115
visionx::RCPointCloudProvider::RCPointCloudProvider
RCPointCloudProvider()
Definition: RCPointCloudProvider.cpp:224
visionx::RoboceptionUser::smallImages
CByteImage ** smallImages
Definition: RoboceptionUser.h:98
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:92
visionx::RoboceptionUser::setupStreamAndCalibration
void setupStreamAndCalibration(float scalefactor, const std::string &calibSavePath)
Definition: RoboceptionUser.cpp:139
TypeMapping.h
armarx::RemoteGuiComponentPluginUser::createOrUpdateRemoteGuiTab
void createOrUpdateRemoteGuiTab(Ts &&...ts)
Definition: RemoteGuiComponentPlugin.h:255
visionx::RoboceptionUser::focalLengthFactor
double focalLengthFactor
Definition: RoboceptionUser.h:94
visionx::RCPointCloudProviderPropertyDefinitions
Definition: RCPointCloudProvider.h:59
min
T min(T t1, T t2)
Definition: gdiam.h:42
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
visionx::RCPointCloudProvider::onStopCapture
void onStopCapture() override
This is called when the point cloud provider capturing has been stopped.
Definition: RCPointCloudProvider.cpp:338
ArmarXDataPath.h
visionx::RoboceptionUser::fillCameraImageRGB
static void fillCameraImageRGB(unsigned char *outputPixels, size_t nImage, const uint8_t *inputPixels, size_t width, size_t height, uint64_t pixelFormat, size_t xpadding)
Definition: RoboceptionUser.cpp:275
armarx::RemoteGui::makeFloatSpinBox
detail::FloatSpinBoxBuilder makeFloatSpinBox(std::string const &name)
Definition: FloatWidgets.h:52
visionx::RCPointCloudProviderPropertyDefinitions::RCPointCloudProviderPropertyDefinitions
RCPointCloudProviderPropertyDefinitions(std::string prefix)
Definition: RCPointCloudProvider.cpp:483
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
MiscLib::Vector::clear
void clear()
Definition: Vector.h:174
visionx::RoboceptionUser::stopRC
void stopRC()
Definition: RoboceptionUser.cpp:228
visionx::RoboceptionUser::scaleFactor
float scaleFactor
Definition: RoboceptionUser.h:93
visionx::RCPointCloudProvider::onInitCapturingPointCloudProvider
void onInitCapturingPointCloudProvider() override
This is called when the Component::onInitComponent() is called.
Definition: RCPointCloudProvider.cpp:270