OpenNIImageProvider.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::Component
19  * @author David Gonzalez (david dot gonzalez at kit dot edu)
20  * @date 2014
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "OpenNIImageProvider.h"
26 
27 #include "Helpers/helpers.h"
28 
29 // boost
30 #include <boost/algorithm/string.hpp>
31 #include <boost/lexical_cast.hpp>
32 
33 // VisionXCore
34 #include <VisionX/core/exceptions/user/FrameRateNotSupportedException.h>
35 #include <VisionX/core/exceptions/user/StartingCaptureFailedException.h>
36 
37 // VisionXTools
38 #include <VisionX/tools/ImageUtil.h>
39 #include <VisionX/tools/TypeMapping.h>
40 
41 // ArmarXCore
42 #include <ArmarXCore/core/application/properties/Property.h>
43 
44 using namespace armarx;
45 using namespace visionx::tools;
46 
47 namespace visionx
48 {
49 
50 
51  bool OpenNIImageProvider::isBackEndInitialized = false;
52 
53  void
54  OpenNIImageProvider::onInitCapturingImageProvider()
55  {
56  //-------------------------------------------------------------------------
57 
58  coloredPoint3DBuffer = NULL;
59 
60  normalizedDepthCells = NULL;
61 
62  isModuleInitialized = false;
63 
64  //-------------------------------------------------------------------------
65 
66  uri = getProperty<std::string>("DeviceURI").getValue();
67 
68  videoDimension = getProperty<ImageDimension>("VideoMode").getValue();
69 
70  frameRate = getProperty<float>("FrameRate").getValue();
71 
72  imageType = getProperty<ImageType>("ImageType").getValue();
73 
74  ARMARX_INFO << "Information: URI : " << uri << flush;
75 
76  ARMARX_INFO << "Information: Video Dimension : (" << videoDimension.width << ","
77  << videoDimension.height << ")" << flush;
78 
79  ARMARX_INFO << "Information: Frame Rate : " << frameRate << flush;
80 
81  ARMARX_INFO << "Information: Image Type : " << imageType << flush;
82 
83  setNumberImages(1);
84 
85  setImageFormat(videoDimension, imageType, eBayerPatternBg);
86 
87  setImageSyncMode(eCaptureSynchronization);
88 
89  //-------------------------------------------------------------------------
90 
91  if (!isBackEndInitialized)
92  {
93  if (openni::OpenNI::initialize() == openni::STATUS_OK)
94  {
95  isBackEndInitialized = true;
96 
97  ARMARX_INFO << "Information: OpenNI Initialized successfully." << flush;
98  }
99  else
100  {
101  ARMARX_ERROR << "Error: While intializing OpenNI : "
102  << openni::OpenNI::getExtendedError() << flush;
103  }
104  }
105  }
106 
107  void
108  OpenNIImageProvider::onExitCapturingImageProvider()
109  {
110  StopDevice();
111 
112  DestroyDataStructure();
113 
114  ARMARX_INFO << "Information : Exit Capturing." << flush;
115  }
116 
117  void
118  OpenNIImageProvider::onStartCapture(float frameRate)
119  {
120  ARMARX_INFO << "Information: Starting capturing..." << flush;
121 
122  bool ERROR = false;
123 
124  if (isBackEndInitialized)
125  {
126  const bool UseAnyDevice = (!uri.length()) || (uri == std::string("ANY_DEVICE")) ||
127  (uri == std::string("Any")) || (uri == std::string("any")) ||
128  (uri == std::string("ANY"));
129 
130  const bool LookForDeviceTarget = !UseAnyDevice;
131 
132  bool DeviceTargetFound = false;
133 
134  openni::Array<openni::DeviceInfo> FoundDevices;
135 
136  openni::OpenNI::enumerateDevices(&FoundDevices);
137 
138  const int TotalDevices = FoundDevices.getSize();
139 
140  ARMARX_INFO << "Information: Total openni devices found : " << TotalDevices << flush;
141 
142  if (TotalDevices)
143  {
144  for (int i = 0; i < TotalDevices; ++i)
145  {
146  const std::string CurrentURI = std::string(FoundDevices[i].getUri());
147 
148  if (LookForDeviceTarget && (uri == CurrentURI))
149  {
150  ARMARX_INFO << "Target found: openni device[" << i
151  << "] Uri = " << CurrentURI << flush;
152 
153  DeviceTargetFound = true;
154  }
155  else
156  {
157  ARMARX_INFO << "Information: Available device OpenNI device[" << i
158  << "] Uri = " << CurrentURI << flush;
159  }
160  }
161 
162  const int Width = videoDimension.width;
163 
164  const int Height = videoDimension.height;
165 
166  const int Fps = EnsureFPS(int(round(frameRate)));
167 
168  ARMARX_INFO << "Information: Video Dimension : (" << Width << "," << Height << ")"
169  << flush;
170 
171  ARMARX_INFO << "Information: Frame Rate : " << Fps << flush;
172 
173  if (LookForDeviceTarget)
174  {
175  if (DeviceTargetFound)
176  {
177  if (StartDevice(uri.c_str(), Width, Height, Fps))
178  {
179  ARMARX_INFO << "openni device[" << uri << "] started succesfully"
180  << flush;
181 
182  if (CreateDataStructures())
183  {
184  ARMARX_ERROR << "Error: Data structures created succesffully"
185  << flush;
186 
187  ERROR = false;
188  }
189  else
190  {
191  ARMARX_ERROR << "Error: Data structures creation failure" << flush;
192 
193  ERROR = true;
194  }
195  }
196  else
197  {
198  ARMARX_ERROR << "Error: openni device[" << uri << "] started failure"
199  << flush;
200 
201  ERROR = true;
202  }
203  }
204  else
205  {
206  ARMARX_ERROR << "Error: openni device Uri = " << uri << " not found"
207  << flush;
208 
209  ERROR = true;
210  }
211  }
212  else
213  {
214  if (StartDevice(openni::ANY_DEVICE, Width, Height, Fps))
215  {
216  ARMARX_INFO << "Infromation: OpenNI device started succesfully." << flush;
217 
218  if (CreateDataStructures())
219  {
220  ARMARX_INFO << " Data structures created succesffully" << flush;
221 
222  ERROR = false;
223  }
224  else
225  {
226  ARMARX_ERROR << " Error: Data structures creation failure" << flush;
227 
228  ERROR = true;
229  }
230  }
231  else
232  {
233  ARMARX_ERROR << "Error: openni device started failure" << flush;
234 
235  ERROR = true;
236  }
237  }
238  }
239  else
240  {
241  ARMARX_ERROR << "Error: No openni devices are available" << flush;
242 
243  ERROR = true;
244  }
245  }
246  else
247  {
248  ARMARX_ERROR << "Error: Openni backend not initialized" << flush;
249 
250  ERROR = true;
251  }
252 
253  if (ERROR)
254  {
255  throw visionx::exceptions::user::StartingCaptureFailedException(
256  "Opening cameras failed!");
257  }
258  }
259 
260  void
261  OpenNIImageProvider::onStopCapture()
262  {
263  StopDevice();
264 
265  DestroyDataStructure();
266  }
267 
268  bool
269  OpenNIImageProvider::capture(void** ppImageBuffers)
270  {
271  if (!isModuleInitialized)
272  {
273  return false;
274  }
275 
276  bool succeeded = false;
277 
278  switch (getImageFormat().type)
279  {
280  /*case visionx::ePointsScan:
281  succeeded = true;
282  break;*/
283 
284  case visionx::eColoredPointsScan:
285  succeeded = CaptureColoredPoint();
286  break;
287 
288  /*
289  * Handle image types which are not supported by OpenNIImageProvider
290  */
291  default:
292  ARMARX_ERROR << "Error: Image type not supported by OpenNIImageProvider!" << flush;
293  return false;
294  }
295 
296  if (!succeeded)
297  {
298  ARMARX_ERROR << "Error: Capturing failed!" << flush;
299  return false;
300  }
301 
302  {
303  armarx::SharedMemoryScopedWriteLockPtr lock = getScopedWriteLock();
304 
305  ImageFormatInfo imageFormat = getImageFormat();
306 
307  const int BufferSize = imageFormat.dimension.width * imageFormat.dimension.height *
308  imageFormat.bytesPerPixel;
309 
310  memcpy(ppImageBuffers[0], coloredPoint3DBuffer, BufferSize);
311  }
312 
313  return true;
314  }
315 
316  /*
317  void OpenNIImageProvider::provideImages(CByteImage** images)
318  {
319  if(numberImages == 0)
320  return;
321 
322  const int Width = videoDimension.width;
323 
324  const int Height = videoDimension.height;
325 
326  CByteImage Result = Display(m_pColoredPoint3DBuffer,Width,Height,true);
327 
328  memcpy(images[0]->pixels, Result.pixels, Width * Height * Result.bytesPerPixel);
329  }
330 
331  void OpenNIImageProvider::provideImages(CFloatImage** images)
332  {
333  if(numberImages == 0)
334  return;
335 
336  const int Width = videoDimension.width;
337 
338  const int Height = videoDimension.height;
339 
340  const visionx::ColoredPoint3D* pColoredPoint3DBuffer = pColoredPoint3DBufferBase;
341 
342  const visionx::ColoredPoint3D* const pEndColoredPoint3DBuffer = pColoredPoint3DBufferBase + (Width * Height);
343 
344  float* pDepthPixel = images[0]->pixels;
345 
346  while(pColoredPoint3DBuffer<pEndColoredPoint3DBuffer)
347  {
348  *pDepthPixel++ = pColoredPoint3DBuffer->m_Point.z;
349 
350  ++pColoredPoint3DBuffer;
351  }
352  }
353  */
354 
355  bool
356  OpenNIImageProvider::StartDevice(const char* pDeviceURI,
357  const int Width,
358  const int Height,
359  const int Fps)
360  {
361  if (isModuleInitialized)
362  {
363  return false;
364  }
365 
366  //-------------------------------------------------------------------------------------------------------------------------------------------------------------
367 
368  if (deviceOpenNI.open(pDeviceURI) != openni::STATUS_OK)
369  {
370  ARMARX_ERROR << "Error: Cannot open device (" << pDeviceURI << ") => "
371  << openni::OpenNI::getExtendedError() << flush;
372 
373  return false;
374  }
375 
376  if (!deviceOpenNI.hasSensor(openni::SENSOR_DEPTH))
377  {
378  ARMARX_ERROR << "Error: Device (" << pDeviceURI << ") [ has not depth sensor ]"
379  << flush;
380 
381  return false;
382  }
383 
384  if (!deviceOpenNI.hasSensor(openni::SENSOR_COLOR))
385  {
386  ARMARX_ERROR << "Error: Device (" << pDeviceURI << ") [ has not color sensor ]"
387  << flush;
388 
389  return false;
390  }
391 
392  //-------------------------------------------------------------------------------------------------------------------------------------------------------------
393 
394  if (depthStreamOpenNI.create(deviceOpenNI, openni::SENSOR_DEPTH) != openni::STATUS_OK)
395  {
396  ARMARX_ERROR << "Error: Cannot create Depth stream on Device (" << pDeviceURI << ") => "
397  << openni::OpenNI::getExtendedError() << flush;
398 
399  return false;
400  }
401 
402  if (!depthStreamOpenNI.isValid())
403  {
404  ARMARX_ERROR << "Error: Cannot create Depth stream on Device (" << pDeviceURI << ") => "
405  << openni::OpenNI::getExtendedError() << flush;
406 
407  return false;
408  }
409 
410  openni::VideoMode CurrentDepthStreamMode = depthStreamOpenNI.getVideoMode();
411 
412  ARMARX_INFO << "Information: Depth Stream Default Video Dimension : ("
413  << CurrentDepthStreamMode.getResolutionX() << ","
414  << CurrentDepthStreamMode.getResolutionY() << ")" << flush;
415 
416  ARMARX_INFO << "Information: Depth Stream Default Frame Rate : "
417  << CurrentDepthStreamMode.getFps() << flush;
418 
419  ARMARX_INFO << "Information: Depth Stream Target Video Dimension : (" << Width << ","
420  << Height << ")" << flush;
421 
422  ARMARX_INFO << "Information: Depth Stream Target Frame Rate : " << Fps << flush;
423 
424  CurrentDepthStreamMode.setResolution(Width, Height);
425 
426  CurrentDepthStreamMode.setFps(Fps);
427 
428  if (depthStreamOpenNI.setVideoMode(CurrentDepthStreamMode) != openni::STATUS_OK)
429  {
430  ARMARX_ERROR << "Error: Cannot set Depth stream settings (" << Width << "," << Height
431  << "," << Fps << ") Device (" << pDeviceURI << ") => "
432  << openni::OpenNI::getExtendedError() << flush;
433 
434  return false;
435  }
436 
437  if (depthStreamOpenNI.start() != openni::STATUS_OK)
438  {
439  depthStreamOpenNI.destroy();
440 
441  ARMARX_ERROR << "Error: Cannot start Depth stream on Device (" << pDeviceURI << ") => "
442  << openni::OpenNI::getExtendedError() << flush;
443 
444  return false;
445  }
446 
447  CurrentDepthStreamMode = depthStreamOpenNI.getVideoMode();
448 
449  ARMARX_INFO << "Information: Depth Stream Active Video Dimension : ("
450  << CurrentDepthStreamMode.getResolutionX() << ","
451  << CurrentDepthStreamMode.getResolutionY() << ")" << flush;
452 
453  ARMARX_INFO << "Information: Depth Stream Active Frame Rate : "
454  << CurrentDepthStreamMode.getFps() << flush;
455 
456  if (!depthStreamOpenNI.isValid())
457  {
458  ARMARX_ERROR << "Error: Cannot create Depth stream on Device (" << pDeviceURI << ") => "
459  << openni::OpenNI::getExtendedError() << flush;
460 
461  return false;
462  }
463 
464  //-------------------------------------------------------------------------------------------------------------------------------------------------------------
465 
466  if (colorStreamOpenNI.create(deviceOpenNI, openni::SENSOR_COLOR) != openni::STATUS_OK)
467  {
468  ARMARX_ERROR << "Error: Cannot create Color stream on Device (" << pDeviceURI << ") => "
469  << openni::OpenNI::getExtendedError() << flush;
470 
471  return false;
472  }
473 
474  if (!colorStreamOpenNI.isValid())
475  {
476  ARMARX_ERROR << "Error: Cannot create Color stream on Device (" << pDeviceURI << ") => "
477  << openni::OpenNI::getExtendedError() << flush;
478 
479  return false;
480  }
481 
482  openni::VideoMode CurrentColorStreamMode = colorStreamOpenNI.getVideoMode();
483 
484  ARMARX_INFO << "Information: Color Stream Default Video Dimension : ("
485  << CurrentColorStreamMode.getResolutionX() << ","
486  << CurrentColorStreamMode.getResolutionY() << ")" << flush;
487 
488  ARMARX_INFO << "Information: Color Stream Default Frame Rate : "
489  << CurrentColorStreamMode.getFps() << flush;
490 
491  ARMARX_INFO << "Information: Color Stream Target Video Dimension : (" << Width << ","
492  << Height << ")" << flush;
493 
494  ARMARX_INFO << "Information: Color Stream Target Frame Rate : " << Fps << flush;
495 
496  CurrentColorStreamMode.setResolution(Width, Height);
497 
498  CurrentColorStreamMode.setFps(Fps);
499 
500 
501  if (colorStreamOpenNI.setVideoMode(CurrentColorStreamMode) != openni::STATUS_OK)
502  {
503  ARMARX_ERROR << "Error: Cannot set Color stream settings (" << Width << "," << Height
504  << "," << Fps << ") Device (" << pDeviceURI << ") => "
505  << openni::OpenNI::getExtendedError() << flush;
506 
507  return false;
508  }
509 
510  if (colorStreamOpenNI.start() != openni::STATUS_OK)
511  {
512  depthStreamOpenNI.destroy();
513 
514  ARMARX_ERROR << "Error: Cannot start Color stream on Device (" << pDeviceURI << ") => "
515  << openni::OpenNI::getExtendedError() << flush;
516 
517  return false;
518  }
519 
520  CurrentColorStreamMode = colorStreamOpenNI.getVideoMode();
521 
522  ARMARX_INFO << "Information: Color Stream Active Video Dimension : ("
523  << CurrentColorStreamMode.getResolutionX() << ","
524  << CurrentColorStreamMode.getResolutionY() << ")" << flush;
525 
526  ARMARX_INFO << "Information: Color Stream Active Frame Rate : "
527  << CurrentColorStreamMode.getFps() << flush;
528 
529  if (!colorStreamOpenNI.isValid())
530  {
531  ARMARX_ERROR << "Error: Cannot create Color stream on Device (" << pDeviceURI << ") => "
532  << openni::OpenNI::getExtendedError() << flush;
533 
534  return false;
535  }
536 
537  //-------------------------------------------------------------------------------------------------------------------------------------------------------------
538 
539  if (deviceOpenNI.setDepthColorSyncEnabled(true) != openni::STATUS_OK)
540  {
541  ARMARX_WARNING << "Warning: Cannot synchronize Depth and Color on Device ("
542  << pDeviceURI << ") => " << openni::OpenNI::getExtendedError() << flush;
543  }
544 
545  if (deviceOpenNI.isImageRegistrationModeSupported(
546  openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
547  {
548  if (deviceOpenNI.setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR) !=
549  openni::STATUS_OK)
550  {
551  ARMARX_WARNING << "Warning: Device supports depth-to-color registration but could "
552  "not be enabled ("
553  << pDeviceURI << ") =>" << openni::OpenNI::getExtendedError()
554  << flush;
555  }
556  }
557 
558  //-------------------------------------------------------------------------------------------------------------------------------------------------------------
559 
560  isModuleInitialized = true;
561 
562  return true;
563  }
564 
565  bool
566  OpenNIImageProvider::CreateDataStructures()
567  {
568  DestroyDataStructure();
569 
570  const int Width = videoDimension.width;
571 
572  const int Height = videoDimension.height;
573 
574  const int Area = Width * Height;
575 
576  coloredPoint3DBuffer = new visionx::ColoredPoint3D[Area];
577 
578  memset(coloredPoint3DBuffer, 0, sizeof(visionx::ColoredPoint3D) * Area);
579 
580  normalizedDepthCells = new NormalizedDepthCell[Area];
581 
582  memset(normalizedDepthCells, 0, sizeof(NormalizedDepthCell) * Area);
583 
584  InitializeNormalizedDepthCells();
585 
586  return true;
587  }
588 
589  bool
590  OpenNIImageProvider::InitializeNormalizedDepthCells()
591  {
592  if (!normalizedDepthCells)
593  {
594  return false;
595  }
596 
597  const int Width = videoDimension.width;
598 
599  const int Height = videoDimension.height;
600 
601  const float XZFactor = std::tan(depthStreamOpenNI.getHorizontalFieldOfView() / 2.0f) * 2.0f;
602 
603  const float YZFactor = std::tan(depthStreamOpenNI.getVerticalFieldOfView() / 2.0f) * 2.0f;
604 
605  NormalizedDepthCell* pNormalizedDepthCell = normalizedDepthCells;
606 
607  for (int Y = 0; Y < Height; ++Y)
608  {
609  const float NY = (0.5f - (float(Y) / float(Height))) * YZFactor;
610 
611  for (int X = 0; X < Width; ++X, ++pNormalizedDepthCell)
612  {
613  pNormalizedDepthCell->nx = ((float(X) / float(Width)) - 0.5f) * XZFactor;
614 
615  pNormalizedDepthCell->ny = NY;
616  }
617  }
618 
619  return true;
620  }
621 
622  bool
623  OpenNIImageProvider::StopDevice()
624  {
625  if (isModuleInitialized)
626  {
627  depthStreamOpenNI.stop();
628 
629  depthStreamOpenNI.destroy();
630 
631  colorStreamOpenNI.stop();
632 
633  colorStreamOpenNI.destroy();
634 
635  deviceOpenNI.close();
636 
637  isModuleInitialized = false;
638  }
639 
640  //TODO: Verifiy this out value
641  return true;
642  }
643 
644  bool
645  OpenNIImageProvider::DestroyDataStructure()
646  {
647  if (coloredPoint3DBuffer)
648  {
649  delete[] coloredPoint3DBuffer;
650 
651  coloredPoint3DBuffer = NULL;
652  }
653 
654  if (normalizedDepthCells)
655  {
656  delete[] normalizedDepthCells;
657 
658  normalizedDepthCells = NULL;
659  }
660 
661  return true;
662  }
663 
664  bool
665  OpenNIImageProvider::CaptureColoredPoint()
666  {
667  if (!isModuleInitialized)
668  {
669  return false;
670  }
671 
672  if (!coloredPoint3DBuffer)
673  {
674  return false;
675  }
676 
677  if (!normalizedDepthCells)
678  {
679  return false;
680  }
681 
682  openni::VideoStream* ppStreams[2] = {&depthStreamOpenNI, &colorStreamOpenNI};
683 
684  bool HasBeenCaptured[2] = {false, false};
685 
686  while (!(HasBeenCaptured[0] && HasBeenCaptured[1]))
687  {
688  int StreamIndex = -1;
689 
690  bool CaptureSuccesfull = false;
691 
692  for (int i = 0; i < 3; ++i)
693  {
694  if (openni::OpenNI::waitForAnyStream(ppStreams, 2, &StreamIndex, 5000) ==
695  openni::STATUS_OK)
696  {
697 
698  CaptureSuccesfull = true;
699 
700  break;
701  }
702  else
703  {
704  ARMARX_WARNING << "Warning: Capturing Try not succesfull ->"
705  << openni::OpenNI::getExtendedError() << flush;
706  }
707  }
708 
709  if (!CaptureSuccesfull)
710  {
711  return false;
712  }
713 
714  if (!(StreamIndex >= 0) && (StreamIndex <= 1))
715  {
716  return false;
717  }
718 
719  if (ppStreams[StreamIndex]->readFrame(&frameOpenNI) != openni::STATUS_OK)
720  {
721  return false;
722  }
723 
724  switch (StreamIndex)
725  {
726  case 0:
727 
728  if (!DispatchDepthFrame())
729  {
730  return false;
731  }
732 
733  break;
734 
735  case 1:
736 
737  if (!DispatchColorFrame())
738  {
739  return false;
740  }
741 
742  break;
743 
744  default:
745  return false;
746  }
747 
748  HasBeenCaptured[StreamIndex] = true;
749  }
750 
751 
752  /*
753  //Only for Testing and Debug
754  const int Width = videoDimension.width;
755 
756  const int Height = videoDimension.height;
757 
758  CByteImage* pColorImage = Display(m_pColoredPoint3DBuffer,Width,Height,true);
759 
760  CByteImage* pDepthImage = Display(m_pColoredPoint3DBuffer,Width,Height,false);
761 
762  if(pColorImage)
763  {
764  pColorImage->SaveToFile("/home/gonzalez/ColorImage.bmp");
765  delete pColorImage;
766  pColorImage = NULL;
767  }
768 
769  if(pDepthImage)
770  {
771  pDepthImage->SaveToFile("/home/gonzalez/DepthImage.bmp");
772  delete pDepthImage;
773  pDepthImage = NULL;
774  }
775  */
776 
777 
778  return true;
779  }
780 
781  bool
782  OpenNIImageProvider::DispatchDepthFrame()
783  {
784  const NormalizedDepthCell* pNormalizedDepthCell = normalizedDepthCells;
785 
786  visionx::ColoredPoint3D* pColoredPoint3D = coloredPoint3DBuffer;
787 
788  const visionx::ColoredPoint3D* const pEndColoredPoint3D =
789  coloredPoint3DBuffer + (videoDimension.width * videoDimension.height);
790 
791  uint16_t* pDepthPixel = (uint16_t*)(frameOpenNI.getData());
792 
793  while (pColoredPoint3D < pEndColoredPoint3D)
794  {
795  pColoredPoint3D->point.z = float(*pDepthPixel++);
796 
797  if (pColoredPoint3D->point.z)
798  {
799  pColoredPoint3D->point.x = pNormalizedDepthCell->nx * pColoredPoint3D->point.z;
800 
801  pColoredPoint3D->point.y = pNormalizedDepthCell->ny * pColoredPoint3D->point.z;
802  }
803  else
804  {
805  pColoredPoint3D->point.x = 0.0f;
806 
807  pColoredPoint3D->point.y = 0.0f;
808  }
809 
810  ++pColoredPoint3D;
811 
812  ++pNormalizedDepthCell;
813  }
814 
815  return true;
816  }
817 
818  bool
819  OpenNIImageProvider::DispatchColorFrame()
820  {
821  visionx::ColoredPoint3D* pColoredPoint3D = coloredPoint3DBuffer;
822 
823  const visionx::ColoredPoint3D* const pEndColoredPoint3D =
824  coloredPoint3DBuffer + (videoDimension.width * videoDimension.height);
825 
826  unsigned char* pColorPixel = (unsigned char*)frameOpenNI.getData();
827 
828  while (pColoredPoint3D < pEndColoredPoint3D)
829  {
830  pColoredPoint3D->color.r = *pColorPixel++;
831 
832  pColoredPoint3D->color.g = *pColorPixel++;
833 
834  pColoredPoint3D->color.b = *pColorPixel++;
835 
836  pColoredPoint3D->color.a = 0;
837 
838  ++pColoredPoint3D;
839  }
840 
841  return true;
842  }
843 
844  int
845  OpenNIImageProvider::EnsureFPS(const int FPS)
846  {
847  if (FPS == 30)
848  {
849  return FPS;
850  }
851 
852  else if (FPS == 15)
853  {
854  return FPS;
855  }
856 
857  const int DeviationTo30 = std::abs(FPS - 30);
858 
859  const int DeviationTo15 = std::abs(FPS - 15);
860 
861  ARMARX_WARNING << "Warning: Selected FPS = " << FPS << "not supported by device" << flush;
862 
863  if (DeviationTo30 < DeviationTo15)
864  {
865  ARMARX_ERROR << "Warning: Closest Fps = [30] supported will be used" << flush;
866 
867  return 30;
868  }
869  else
870  {
871  ARMARX_ERROR << "Warning: Closest Fps = [15] supported will be used" << flush;
872 
873  return 15;
874  }
875  }
876 } // namespace visionx
StartingCaptureFailedException.h
Area
ldouble Area(const point2d &a, const point2d &b, const point2d &c)
Definition: gdiam.cpp:1523
armarx::MessageTypeT::ERROR
@ ERROR
visionx::tools
Definition: PCLUtilities.cpp:3
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
OpenNIImageProvider.h
visionx::OpenNIImageProvider::NormalizedDepthCell::nx
float nx
Definition: OpenNIImageProvider.h:268
armarx::SharedMemoryScopedWriteLockPtr
std::shared_ptr< SharedMemoryScopedWriteLock > SharedMemoryScopedWriteLockPtr
Definition: SharedMemoryProvider.h:47
visionx::OpenNIImageProvider::NormalizedDepthCell
OpenNI Capture.
Definition: OpenNIImageProvider.h:266
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:281
Property.h
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
visionx::exceptions::user::StartingCaptureFailedException
Definition: StartingCaptureFailedException.h:31
FrameRateNotSupportedException.h
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
float
#define float
Definition: 16_Level.h:22
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
visionx::OpenNIImageProvider::NormalizedDepthCell::ny
float ny
Definition: OpenNIImageProvider.h:269
ImageUtil.h
TypeMapping.h
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27