CameraK4W2.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
19 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 */
26 
27 #include <rtabmap/core/camera/CameraK4W2.h>
28 #include <rtabmap/utilite/UTimer.h>
29 #include <rtabmap/utilite/UThreadC.h>
30 #include <rtabmap/core/util2d.h>
31 #include <opencv2/imgproc/types_c.h>
32 
33 #ifdef RTABMAP_K4W2
34 #include <Kinect.h>
35 #endif
36 
37 namespace rtabmap
38 {
39 
40 #ifdef RTABMAP_K4W2
41 // Safe release for interfaces
42 template<class Interface>
43 inline void SafeRelease(Interface *& pInterfaceToRelease)
44 {
45  if (pInterfaceToRelease != NULL)
46  {
47  pInterfaceToRelease->Release();
48  pInterfaceToRelease = NULL;
49  }
50 }
51 #endif
52 
53 bool CameraK4W2::available()
54 {
55 #ifdef RTABMAP_K4W2
56  return true;
57 #else
58  return false;
59 #endif
60 }
61 
62 CameraK4W2::CameraK4W2(
63  int deviceId,
64  Type type,
65  float imageRate,
66  const Transform & localTransform) :
67  Camera(imageRate, localTransform)
68 #ifdef RTABMAP_K4W2
69  ,
70  type_(type),
71  pKinectSensor_(NULL),
72  pCoordinateMapper_(NULL),
73  pDepthCoordinates_(new DepthSpacePoint[cColorWidth * cColorHeight]),
74  pColorCoordinates_(new ColorSpacePoint[cDepthWidth * cDepthHeight]),
75  pMultiSourceFrameReader_(NULL),
76  pColorRGBX_(new RGBQUAD[cColorWidth * cColorHeight]),
77  hMSEvent(NULL)
78 #endif
79 {
80 }
81 
82 CameraK4W2::~CameraK4W2()
83 {
84 #ifdef RTABMAP_K4W2
85  if (pDepthCoordinates_)
86  {
87  delete[] pDepthCoordinates_;
88  pDepthCoordinates_ = NULL;
89  }
90 
91  if (pColorCoordinates_)
92  {
93  delete[] pColorCoordinates_;
94  pColorCoordinates_ = NULL;
95  }
96 
97  if (pColorRGBX_)
98  {
99  delete[] pColorRGBX_;
100  pColorRGBX_ = NULL;
101  }
102 
103  close();
104 #endif
105 }
106 
107 void CameraK4W2::close()
108 {
109 #ifdef RTABMAP_K4W2
110  if (pMultiSourceFrameReader_)
111  {
112  pMultiSourceFrameReader_->UnsubscribeMultiSourceFrameArrived(hMSEvent);
113  CloseHandle((HANDLE)hMSEvent);
114  hMSEvent = NULL;
115  }
116 
117  // done with frame reader
118  SafeRelease(pMultiSourceFrameReader_);
119 
120  // done with coordinate mapper
121  SafeRelease(pCoordinateMapper_);
122 
123  // close the Kinect Sensor
124  if (pKinectSensor_)
125  {
126  pKinectSensor_->Close();
127  }
128 
129  SafeRelease(pKinectSensor_);
130 
131  colorCameraModel_ = CameraModel();
132 #endif
133 }
134 
135 bool CameraK4W2::init(const std::string & calibrationFolder, const std::string & cameraName)
136 {
137 #ifdef RTABMAP_K4W2
138  HRESULT hr;
139 
140  close();
141 
142  hr = GetDefaultKinectSensor(&pKinectSensor_);
143  if (FAILED(hr))
144  {
145  return false;
146  }
147 
148  if (pKinectSensor_)
149  {
150  // Initialize the Kinect and get coordinate mapper and the frame reader
151 
152  hr = pKinectSensor_->Open();
153 
154  if (SUCCEEDED(hr))
155  {
156  hr = pKinectSensor_->get_CoordinateMapper(&pCoordinateMapper_);
157 
158  if (SUCCEEDED(hr))
159  {
160  hr = pKinectSensor_->OpenMultiSourceFrameReader(
161  FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color,
162  &pMultiSourceFrameReader_);
163 
164  if (SUCCEEDED(hr))
165  {
166  hr = pMultiSourceFrameReader_->SubscribeMultiSourceFrameArrived(&hMSEvent);
167  }
168  }
169  }
170  }
171 
172  if (!pKinectSensor_ || FAILED(hr))
173  {
174  UERROR("No ready Kinect found!");
175  close();
176  return false;
177  }
178 
179  // to query camera parameters, we should wait a little
180  uSleep(3000);
181 
182  // initialize color calibration if not set yet
183  CameraIntrinsics intrinsics;
184  hr = pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
185  if (SUCCEEDED(hr) && intrinsics.FocalLengthX > 0.0f)
186  {
187  // guess color intrinsics by comparing two reprojections
188  CameraModel depthModel(
189  intrinsics.FocalLengthX,
190  intrinsics.FocalLengthY,
191  intrinsics.PrincipalPointX,
192  intrinsics.PrincipalPointY);
193 
194  cv::Mat fakeDepth = cv::Mat::ones(cDepthHeight, cDepthWidth, CV_16UC1) * 1000;
195  hr = pCoordinateMapper_->MapDepthFrameToColorSpace(cDepthWidth * cDepthHeight, (UINT16*)fakeDepth.data, cDepthWidth * cDepthHeight, pColorCoordinates_);
196  if (SUCCEEDED(hr))
197  {
198  int firstIndex = -1;
199  int lastIndex = -1;
200  for (int depthIndex = 0; depthIndex < (cDepthWidth*cDepthHeight); ++depthIndex)
201  {
202  ColorSpacePoint p = pColorCoordinates_[depthIndex];
203  // Values that are negative infinity means it is an invalid color to depth mapping so we
204  // skip processing for this pixel
205  if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
206  {
207  if (firstIndex == -1)
208  {
209  firstIndex = depthIndex;
210  }
211  lastIndex = depthIndex;
212  }
213  }
214 
215  UASSERT(firstIndex >= 0 && lastIndex >= 0);
216  float fx, fy, cx, cy;
217  float x1, y1, z1, x2, y2, z2;
218  depthModel.project(firstIndex - (firstIndex / cDepthWidth)*cDepthWidth, firstIndex / cDepthWidth, 1.0f, x1, y1, z1);
219  depthModel.project(lastIndex - (lastIndex / cDepthWidth)*cDepthWidth, lastIndex / cDepthWidth, 1.0f, x2, y2, z2);
220  ColorSpacePoint uv1 = pColorCoordinates_[firstIndex];
221  ColorSpacePoint uv2 = pColorCoordinates_[lastIndex];
222  fx = ((uv1.X - uv2.X)*z1*z2) / (x1*z2 - x2*z1);
223  cx = uv1.X - (x1 / z1) * fx;
224  fy = ((uv1.Y - uv2.Y)*z1*z2) / (y1*z2 - y2*z1);
225  cy = uv1.Y - (y1 / z1) * fy;
226 
227  colorCameraModel_ = CameraModel(
228  fx,
229  fy,
230  float(cColorWidth) - cx,
231  cy,
232  this->getLocalTransform(),
233  0,
234  cv::Size(cColorWidth, cColorHeight));
235  }
236  }
237 
238  if (!colorCameraModel_.isValidForProjection())
239  {
240  UERROR("Failed to get camera parameters! Is the camera connected? Try restarting the camera again or use kTypeColor2DepthSD.");
241  close();
242  return false;
243  }
244 
245  std::string serial = getSerial();
246  if (!serial.empty())
247  {
248  UINFO("Running kinect device \"%s\"", serial.c_str());
249  }
250 
251  return true;
252 #else
253  UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!");
254  return false;
255 #endif
256 }
257 
258 bool CameraK4W2::isCalibrated() const
259 {
260  return true;
261 }
262 
263 std::string CameraK4W2::getSerial() const
264 {
265 #ifdef RTABMAP_K4W2
266  if (pKinectSensor_)
267  {
268  wchar_t uid[255] = { 0 };
269  // It seems to fail every time!?
270  HRESULT hr = pKinectSensor_->get_UniqueKinectId(255, uid);
271  if (SUCCEEDED(hr))
272  {
273  std::wstring ws(uid);
274  return std::string(ws.begin(), ws.end());
275  }
276  }
277 #endif
278  return "";
279 }
280 
281 SensorData CameraK4W2::captureImage(SensorCaptureInfo * info)
282 {
283  SensorData data;
284 
285 #ifdef RTABMAP_K4W2
286 
287  if (!pMultiSourceFrameReader_)
288  {
289  return data;
290  }
291 
292  HRESULT hr;
293 
294  //now check for frame events
295  HANDLE handles[] = { reinterpret_cast<HANDLE>(hMSEvent) };
296 
297  double t = UTimer::now();
298  while((UTimer::now()-t < 5.0) && WaitForMultipleObjects(_countof(handles), handles, false, 5000) == WAIT_OBJECT_0)
299  {
300  IMultiSourceFrameArrivedEventArgs* pArgs = NULL;
301 
302  hr = pMultiSourceFrameReader_->GetMultiSourceFrameArrivedEventData(hMSEvent, &pArgs);
303  if (SUCCEEDED(hr))
304  {
305  IMultiSourceFrameReference * pFrameRef = NULL;
306  hr = pArgs->get_FrameReference(&pFrameRef);
307  if (SUCCEEDED(hr))
308  {
309  IMultiSourceFrame* pMultiSourceFrame = NULL;
310  IDepthFrame* pDepthFrame = NULL;
311  IColorFrame* pColorFrame = NULL;
312 
313  hr = pFrameRef->AcquireFrame(&pMultiSourceFrame);
314  if (FAILED(hr))
315  {
316  UERROR("Failed getting latest frame.");
317  }
318 
319  IDepthFrameReference* pDepthFrameReference = NULL;
320  hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference);
321  if (SUCCEEDED(hr))
322  {
323  hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
324  }
325  SafeRelease(pDepthFrameReference);
326 
327  IColorFrameReference* pColorFrameReference = NULL;
328  hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference);
329  if (SUCCEEDED(hr))
330  {
331  hr = pColorFrameReference->AcquireFrame(&pColorFrame);
332  }
333  SafeRelease(pColorFrameReference);
334 
335  if (pDepthFrame && pColorFrame)
336  {
337  IFrameDescription* pDepthFrameDescription = NULL;
338  int nDepthWidth = 0;
339  int nDepthHeight = 0;
340  UINT nDepthBufferSize = 0;
341  UINT16 *pDepthBuffer = NULL;
342 
343  IFrameDescription* pColorFrameDescription = NULL;
344  int nColorWidth = 0;
345  int nColorHeight = 0;
346  ColorImageFormat imageFormat = ColorImageFormat_None;
347  UINT nColorBufferSize = 0;
348  RGBQUAD *pColorBuffer = NULL;
349 
350  // get depth frame data
351  if (SUCCEEDED(hr))
352  hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription);
353  if (SUCCEEDED(hr))
354  hr = pDepthFrameDescription->get_Width(&nDepthWidth);
355  if (SUCCEEDED(hr))
356  hr = pDepthFrameDescription->get_Height(&nDepthHeight);
357  if (SUCCEEDED(hr))
358  hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
359 
360  // get color frame data
361  if (SUCCEEDED(hr))
362  hr = pColorFrame->get_FrameDescription(&pColorFrameDescription);
363  if (SUCCEEDED(hr))
364  hr = pColorFrameDescription->get_Width(&nColorWidth);
365  if (SUCCEEDED(hr))
366  hr = pColorFrameDescription->get_Height(&nColorHeight);
367  if (SUCCEEDED(hr))
368  hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
369  if (SUCCEEDED(hr))
370  {
371  if (imageFormat == ColorImageFormat_Bgra)
372  {
373  hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
374  }
375  else if (pColorRGBX_)
376  {
377  pColorBuffer = pColorRGBX_;
378  nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD);
379  hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra);
380  }
381  else
382  {
383  hr = E_FAIL;
384  }
385  }
386 
387  if(SUCCEEDED(hr))
388  {
389  //ProcessFrame(nDepthTime, pDepthBuffer, nDepthWidth, nDepthHeight,
390  // pColorBuffer, nColorWidth, nColorHeight,
391  // pBodyIndexBuffer, nBodyIndexWidth, nBodyIndexHeight);
392 
393  // Make sure we've received valid data
394  if (pCoordinateMapper_ &&
395  pDepthBuffer && (nDepthWidth == cDepthWidth) && (nDepthHeight == cDepthHeight) &&
396  pColorBuffer && (nColorWidth == cColorWidth) && (nColorHeight == cColorHeight))
397  {
398  if (type_ == kTypeColor2DepthSD)
399  {
400  HRESULT hr = pCoordinateMapper_->MapColorFrameToDepthSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nColorWidth * nColorHeight, pDepthCoordinates_);
401  if (SUCCEEDED(hr))
402  {
403  cv::Mat depth = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_16UC1);
404  cv::Mat imageColorRegistered = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_8UC3);
405  // loop over output pixels
406  for (int colorIndex = 0; colorIndex < (nColorWidth*nColorHeight); ++colorIndex)
407  {
408  DepthSpacePoint p = pDepthCoordinates_[colorIndex];
409  // Values that are negative infinity means it is an invalid color to depth mapping so we
410  // skip processing for this pixel
411  if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
412  {
413  // To avoid black lines caused by rounding pixel values, we should set 4 pixels
414  // At the same do mirror
415  int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
416  pixel_x_l = nDepthWidth - static_cast<int>(p.X);
417  pixel_y_l = static_cast<int>(p.Y);
418  pixel_x_h = pixel_x_l - 1;
419  pixel_y_h = pixel_y_l + 1;
420 
421  const RGBQUAD* pSrc = pColorBuffer + colorIndex;
422  if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
423  {
424  unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_l);
425  ptr[0] = pSrc->rgbBlue;
426  ptr[1] = pSrc->rgbGreen;
427  ptr[2] = pSrc->rgbRed;
428  depth.at<unsigned short>(pixel_y_l, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_l*nDepthWidth);
429  }
430  if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
431  {
432  unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_l);
433  ptr[0] = pSrc->rgbBlue;
434  ptr[1] = pSrc->rgbGreen;
435  ptr[2] = pSrc->rgbRed;
436  depth.at<unsigned short>(pixel_y_h, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_h*nDepthWidth);
437  }
438  if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
439  {
440  unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_h);
441  ptr[0] = pSrc->rgbBlue;
442  ptr[1] = pSrc->rgbGreen;
443  ptr[2] = pSrc->rgbRed;
444  depth.at<unsigned short>(pixel_y_l, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_l*nDepthWidth);
445  }
446  if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
447  {
448  unsigned char * ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_h);
449  ptr[0] = pSrc->rgbBlue;
450  ptr[1] = pSrc->rgbGreen;
451  ptr[2] = pSrc->rgbRed;
452  depth.at<unsigned short>(pixel_y_h, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_h*nDepthWidth);
453  }
454  }
455  }
456 
457  CameraIntrinsics intrinsics;
458  pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
459  CameraModel model(
460  intrinsics.FocalLengthX,
461  intrinsics.FocalLengthY,
462  intrinsics.PrincipalPointX,
463  intrinsics.PrincipalPointY,
464  this->getLocalTransform(),
465  0,
466  depth.size());
467  data = SensorData(imageColorRegistered, depth, model, this->getNextSeqID(), UTimer::now());
468  }
469  else
470  {
471  UERROR("Failed color to depth registration!");
472  }
473  }
474  else //depthToColor
475  {
476  HRESULT hr = pCoordinateMapper_->MapDepthFrameToColorSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nDepthWidth * nDepthHeight, pColorCoordinates_);
477  if (SUCCEEDED(hr))
478  {
479  cv::Mat depthSource(nDepthHeight, nDepthWidth, CV_16UC1, pDepthBuffer);
480  cv::Mat depthRegistered = cv::Mat::zeros(
481  type_ == kTypeDepth2ColorSD ? nColorHeight/2 : nColorHeight,
482  type_ == kTypeDepth2ColorSD ? nColorWidth/2 : nColorWidth,
483  CV_16UC1);
484  cv::Mat imageColor;
485  if(type_ == kTypeDepth2ColorSD)
486  {
487  cv::Mat tmp;
488  cv::resize(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
489  cv::cvtColor(tmp, imageColor, CV_BGRA2BGR);
490  }
491  else
492  {
493  cv::cvtColor(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), imageColor, CV_BGRA2BGR);
494  }
495  // loop over output pixels
496  for (int depthIndex = 0; depthIndex < (nDepthWidth*nDepthHeight); ++depthIndex)
497  {
498  ColorSpacePoint p = pColorCoordinates_[depthIndex];
499  // Values that are negative infinity means it is an invalid color to depth mapping so we
500  // skip processing for this pixel
501  if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
502  {
503  if (type_ == kTypeDepth2ColorSD)
504  {
505  p.X /= 2.0f;
506  p.Y /= 2.0f;
507  }
508  const unsigned short & depth_value = depthSource.at<unsigned short>(0, depthIndex);
509  int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
510  // get the coordinate on image plane.
511  pixel_x_l = depthRegistered.cols - p.X; // flip depth
512  pixel_y_l = p.Y;
513  pixel_x_h = pixel_x_l - 1;
514  pixel_y_h = pixel_y_l + 1;
515 
516  if (pixel_x_l >= 0 && pixel_x_l < depthRegistered.cols &&
517  pixel_y_l>0 && pixel_y_l < depthRegistered.rows && // ignore first line
518  depth_value)
519  {
520  unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_l, pixel_x_l);
521  if (depthPixel == 0 || depthPixel > depth_value)
522  {
523  depthPixel = depth_value;
524  }
525  }
526  if (pixel_x_h >= 0 && pixel_x_h < depthRegistered.cols &&
527  pixel_y_h>0 && pixel_y_h < depthRegistered.rows && // ignore first line
528  depth_value)
529  {
530  unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_h, pixel_x_h);
531  if (depthPixel == 0 || depthPixel > depth_value)
532  {
533  depthPixel = depth_value;
534  }
535  }
536  }
537  }
538 
539  CameraModel model = colorCameraModel_;
540  if (type_ == kTypeDepth2ColorSD)
541  {
542  model = model.scaled(0.5);
543  }
544  util2d::fillRegisteredDepthHoles(depthRegistered, true, true, type_ == kTypeDepth2ColorHD);
545  depthRegistered = rtabmap::util2d::fillDepthHoles(depthRegistered, 1);
546  cv::flip(imageColor, imageColor, 1);
547  data = SensorData(imageColor, depthRegistered, model, this->getNextSeqID(), UTimer::now());
548  }
549  else
550  {
551  UERROR("Failed depth to color registration!");
552  }
553  }
554  }
555  }
556 
557  SafeRelease(pDepthFrameDescription);
558  SafeRelease(pColorFrameDescription);
559  }
560 
561  pFrameRef->Release();
562 
563  SafeRelease(pDepthFrame);
564  SafeRelease(pColorFrame);
565  SafeRelease(pMultiSourceFrame);
566  }
567  pArgs->Release();
568  }
569  if (!data.imageRaw().empty())
570  {
571  break;
572  }
573  }
574 #else
575  UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!");
576 #endif
577  return data;
578 }
579 
580 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
UINFO
#define UINFO(...)
rtabmap::CameraK4W2::isCalibrated
virtual bool isCalibrated() const
Definition: CameraK4W2.cpp:258
cy
const double cy
rtabmap::util2d::fillRegisteredDepthHoles
void RTABMAP_CORE_EXPORT fillRegisteredDepthHoles(cv::Mat &depthRegistered, bool vertical, bool horizontal, bool fillDoubleHoles=false)
Definition: util2d.cpp:1598
x1
x1
UTimer::now
static double now()
Definition: UTimer.cpp:80
rtabmap::CameraK4W2::close
void close()
Definition: CameraK4W2.cpp:107
rtabmap::CameraK4W2::available
static bool available()
Definition: CameraK4W2.cpp:53
fx
const double fx
z1
Point2 z1
CameraK4W2.h
this
this
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::CameraK4W2::cDepthHeight
static const int cDepthHeight
Definition: CameraK4W2.h:58
rtabmap::CameraK4W2::getSerial
virtual std::string getSerial() const
Definition: CameraK4W2.cpp:263
type
DepthSpacePoint
struct _DepthSpacePoint DepthSpacePoint
Definition: CameraK4W2.h:36
rtabmap::SensorCapture::getLocalTransform
const Transform & getLocalTransform() const
Definition: SensorCapture.h:62
rtabmap::CameraK4W2::captureImage
virtual SensorData captureImage(SensorCaptureInfo *info=0)
Definition: CameraK4W2.cpp:281
ColorSpacePoint
struct _ColorSpacePoint ColorSpacePoint
Definition: CameraK4W2.h:37
UTimer.h
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
rtabmap::util2d::fillDepthHoles
cv::Mat RTABMAP_CORE_EXPORT fillDepthHoles(const cv::Mat &depth, int maximumHoleSize=1, float errorRatio=0.02f)
Definition: util2d.cpp:1439
rtabmap::CameraK4W2::kTypeDepth2ColorSD
@ kTypeDepth2ColorSD
Definition: CameraK4W2.h:52
Type
data
int data[]
rtabmap::CameraModel::project
void project(float u, float v, float depth, float &x, float &y, float &z) const
Definition: CameraModel.cpp:766
rtabmap::CameraK4W2::cColorHeight
static const int cColorHeight
Definition: CameraK4W2.h:60
BYTE
unsigned char BYTE
Definition: lz4.c:123
RGBQUAD
struct tagRGBQUAD RGBQUAD
Definition: CameraK4W2.h:38
x2
x2
rtabmap::CameraK4W2::~CameraK4W2
virtual ~CameraK4W2()
Definition: CameraK4W2.cpp:82
rtabmap::CameraK4W2::cColorWidth
static const int cColorWidth
Definition: CameraK4W2.h:59
UASSERT
#define UASSERT(condition)
p
Point3_ p(2)
rtabmap::Camera
Definition: Camera.h:43
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
y1
CEPHES_EXTERN_EXPORT double y1(double x)
rtabmap::Transform
Definition: Transform.h:41
util2d.h
rtabmap::CameraK4W2::CameraK4W2
CameraK4W2(int deviceId=0, Type type=kTypeDepth2ColorSD, float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity())
Definition: CameraK4W2.cpp:62
rtabmap::CameraK4W2::cDepthWidth
static const int cDepthWidth
Definition: CameraK4W2.h:57
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::CameraK4W2::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraK4W2.cpp:135
NULL
#define NULL
UThreadC.h
cx
const double cx
t
Point2 t(10, 10)
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
rtabmap::CameraK4W2::kTypeDepth2ColorHD
@ kTypeDepth2ColorHD
Definition: CameraK4W2.h:53
rtabmap::CameraK4W2::kTypeColor2DepthSD
@ kTypeColor2DepthSD
Definition: CameraK4W2.h:51
rtabmap::SensorCapture::getNextSeqID
int getNextSeqID()
Definition: SensorCapture.h:83
fy
const double fy

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:07