CameraTango.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 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "CameraTango.h"
29 #include "util.h"
30 #include "rtabmap/utilite/ULogger.h"
31 #include "rtabmap/core/util3d_transforms.h"
32 #include "rtabmap/core/OdometryEvent.h"
33 #include "rtabmap/core/util2d.h"
34 #include <tango_client_api.h>
35 #include <tango_support_api.h>
36 #include "tango-gl/camera.h"
37 
38 namespace rtabmap {
39 
40 #define nullptr 0
41 const int kVersionStringLength = 128;
42 const int holeSize = 5;
43 const float maxDepthError = 0.10;
44 const int scanDownsampling = 1;
45 
46 //android phone
47 //11 10 01 00 // portrait
48 //01 11 00 10 // left
49 //10 00 11 01 // right
50 //00 01 10 11 // down
51 
52 const float kTextureCoords0[] = {1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0};
53 const float kTextureCoords90[] = {0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 0.0};
54 const float kTextureCoords180[] = {0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 1.0};
55 const float kTextureCoords270[] = {1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0};
56 
57 // Callbacks
58 void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_cloud)
59 {
60  CameraTango* app = static_cast<CameraTango*>(context);
61  if(point_cloud->num_points>0)
62  {
63  app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
64  }
65 }
66 
67 void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuffer* color)
68 {
69  CameraTango* app = static_cast<CameraTango*>(context);
70 
71  cv::Mat tangoImage;
72  if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
73  {
74  tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
75  }
76  else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
77  {
78  tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
79  }
80  else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
81  {
82  tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
83  }
84  else if(color->format == 35)
85  {
86  tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
87  }
88  else
89  {
90  LOGE("Not supported color format : %d.", color->format);
91  }
92 
93  if(!tangoImage.empty())
94  {
95  app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
96  }
97 }
98 
99 void onPoseAvailableRouter(void* context, const TangoPoseData* pose)
100 {
101  if(pose->status_code == TANGO_POSE_VALID)
102  {
103  CameraTango* app = static_cast<CameraTango*>(context);
104  app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world, pose->timestamp);
105  }
106 }
107 
108 void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
109 {
110  CameraTango* app = static_cast<CameraTango*>(context);
111  app->tangoEventReceived(event->type, event->event_key, event->event_value);
112 }
113 
115 // CameraTango
117 CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing) :
118  CameraMobile(smoothing),
119  tango_config_(0),
120  colorCamera_(colorCamera),
121  decimation_(decimation),
122  rawScanPublished_(publishRawScan),
123  tangoColorType_(0),
124  tangoColorStamp_(0)
125 {
126  UASSERT(decimation >= 1);
127 }
128 
129 CameraTango::~CameraTango() {
130  // Disconnect Tango service
131  close();
132 }
133 
134 // Compute fisheye distorted coordinates from undistorted coordinates.
135 // The distortion model used by the Tango fisheye camera is called FOV and is
136 // described in 'Straight lines have to be straight' by Frederic Devernay and
137 // Olivier Faugeras. See https://hal.inria.fr/inria-00267247/document.
138 // Tango ROS Streamer: https://github.com/Intermodalics/tango_ros/blob/master/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp
139 void applyFovModel(
140  double xu, double yu, double w, double w_inverse, double two_tan_w_div_two,
141  double* xd, double* yd) {
142  double ru = sqrt(xu * xu + yu * yu);
143  constexpr double epsilon = 1e-7;
144  if (w < epsilon || ru < epsilon) {
145  *xd = xu;
146  *yd = yu ;
147  } else {
148  double rd_div_ru = std::atan(ru * two_tan_w_div_two) * w_inverse / ru;
149  *xd = xu * rd_div_ru;
150  *yd = yu * rd_div_ru;
151  }
152 }
153 // Compute the warp maps to undistort the Tango fisheye image using the FOV
154 // model. See OpenCV documentation for more information on warp maps:
155 // http://docs.opencv.org/2.4/modules/imgproc/doc/geometric_transformations.html
156 // Tango ROS Streamer: https://github.com/Intermodalics/tango_ros/blob/master/tango_ros_common/tango_ros_native/src/tango_ros_node.cpp
157 // @param fisheyeModel the fisheye camera intrinsics.
158 // @param mapX the output map for the x direction.
159 // @param mapY the output map for the y direction.
160 void initFisheyeRectificationMap(
161  const CameraModel& fisheyeModel,
162  cv::Mat & mapX, cv::Mat & mapY) {
163  const double & fx = fisheyeModel.K().at<double>(0,0);
164  const double & fy = fisheyeModel.K().at<double>(1,1);
165  const double & cx = fisheyeModel.K().at<double>(0,2);
166  const double & cy = fisheyeModel.K().at<double>(1,2);
167  const double & w = fisheyeModel.D().at<double>(0,0);
168  mapX.create(fisheyeModel.imageSize(), CV_32FC1);
169  mapY.create(fisheyeModel.imageSize(), CV_32FC1);
170  LOGD("initFisheyeRectificationMap: fx=%f fy=%f, cx=%f, cy=%f, w=%f", fx, fy, cx, cy, w);
171  // Pre-computed variables for more efficiency.
172  const double fy_inverse = 1.0 / fy;
173  const double fx_inverse = 1.0 / fx;
174  const double w_inverse = 1 / w;
175  const double two_tan_w_div_two = 2.0 * std::tan(w * 0.5);
176  // Compute warp maps in x and y directions.
177  // OpenCV expects maps from dest to src, i.e. from undistorted to distorted
178  // pixel coordinates.
179  for(int iu = 0; iu < fisheyeModel.imageHeight(); ++iu) {
180  for (int ju = 0; ju < fisheyeModel.imageWidth(); ++ju) {
181  double xu = (ju - cx) * fx_inverse;
182  double yu = (iu - cy) * fy_inverse;
183  double xd, yd;
184  applyFovModel(xu, yu, w, w_inverse, two_tan_w_div_two, &xd, &yd);
185  double jd = cx + xd * fx;
186  double id = cy + yd * fy;
187  mapX.at<float>(iu, ju) = jd;
188  mapY.at<float>(iu, ju) = id;
189  }
190  }
191 }
192 
193 bool CameraTango::init(const std::string & calibrationFolder, const std::string & cameraName)
194 {
195  close();
196 
197  CameraMobile::init(calibrationFolder, cameraName);
198 
199  TangoSupport_initialize(TangoService_getPoseAtTime, TangoService_getCameraIntrinsics);
200 
201  // Connect to Tango
202  LOGI("NativeRTABMap: Setup tango config");
203  tango_config_ = TangoService_getConfig(TANGO_CONFIG_DEFAULT);
204  if (tango_config_ == nullptr)
205  {
206  LOGE("NativeRTABMap: Failed to get default config form");
207  return false;
208  }
209 
210  // Set auto-recovery for motion tracking as requested by the user.
211  bool is_atuo_recovery = true;
212  int ret = TangoConfig_setBool(tango_config_, "config_enable_auto_recovery", is_atuo_recovery);
213  if (ret != TANGO_SUCCESS)
214  {
215  LOGE("NativeRTABMap: config_enable_auto_recovery() failed with error code: %d", ret);
216  return false;
217  }
218 
219  if(colorCamera_)
220  {
221  // Enable color.
222  ret = TangoConfig_setBool(tango_config_, "config_enable_color_camera", true);
223  if (ret != TANGO_SUCCESS)
224  {
225  LOGE("NativeRTABMap: config_enable_color_camera() failed with error code: %d", ret);
226  return false;
227  }
228  }
229 
230  // Enable depth.
231  ret = TangoConfig_setBool(tango_config_, "config_enable_depth", true);
232  if (ret != TANGO_SUCCESS)
233  {
234  LOGE("NativeRTABMap: config_enable_depth() failed with error code: %d", ret);
235  return false;
236  }
237 
238  // Need to specify the depth_mode as XYZC.
239  ret = TangoConfig_setInt32(tango_config_, "config_depth_mode", TANGO_POINTCLOUD_XYZC);
240  if (ret != TANGO_SUCCESS)
241  {
242  LOGE("Failed to set 'depth_mode' configuration flag with error code: %d", ret);
243  return false;
244  }
245 
246  // Note that it's super important for AR applications that we enable low
247  // latency imu integration so that we have pose information available as
248  // quickly as possible. Without setting this flag, you'll often receive
249  // invalid poses when calling GetPoseAtTime for an image.
250  ret = TangoConfig_setBool(tango_config_, "config_enable_low_latency_imu_integration", true);
251  if (ret != TANGO_SUCCESS)
252  {
253  LOGE("NativeRTABMap: Failed to enable low latency imu integration.");
254  return false;
255  }
256 
257  // Drift correction allows motion tracking to recover after it loses tracking.
258  //
259  // The drift corrected pose is is available through the frame pair with
260  // base frame AREA_DESCRIPTION and target frame DEVICE.
261  /*ret = TangoConfig_setBool(tango_config_, "config_enable_drift_correction", true);
262  if (ret != TANGO_SUCCESS) {
263  LOGE(
264  "NativeRTABMap: enabling config_enable_drift_correction "
265  "failed with error code: %d",
266  ret);
267  return false;
268  }*/
269 
270  // Get TangoCore version string from service.
271  char tango_core_version[kVersionStringLength];
272  ret = TangoConfig_getString(tango_config_, "tango_service_library_version", tango_core_version, kVersionStringLength);
273  if (ret != TANGO_SUCCESS)
274  {
275  LOGE("NativeRTABMap: get tango core version failed with error code: %d", ret);
276  return false;
277  }
278  LOGI("NativeRTABMap: Tango version : %s", tango_core_version);
279 
280 
281  // Callbacks
282  LOGI("NativeRTABMap: Setup callbacks");
283  // Attach the OnXYZijAvailable callback.
284  // The callback will be called after the service is connected.
285  ret = TangoService_connectOnPointCloudAvailable(onPointCloudAvailableRouter);
286  if (ret != TANGO_SUCCESS)
287  {
288  LOGE("NativeRTABMap: Failed to connect to point cloud callback with error code: %d", ret);
289  return false;
290  }
291 
292  ret = TangoService_connectOnFrameAvailable(colorCamera_?TANGO_CAMERA_COLOR:TANGO_CAMERA_FISHEYE, this, onFrameAvailableRouter);
293  if (ret != TANGO_SUCCESS)
294  {
295  LOGE("NativeRTABMap: Failed to connect to color callback with error code: %d", ret);
296  return false;
297  }
298 
299  // Attach the onPoseAvailable callback.
300  // The callback will be called after the service is connected.
301  TangoCoordinateFramePair pair;
302  //pair.base = TANGO_COORDINATE_FRAME_AREA_DESCRIPTION; // drift correction is enabled
303  pair.base = TANGO_COORDINATE_FRAME_START_OF_SERVICE;
304  pair.target = TANGO_COORDINATE_FRAME_DEVICE;
305  ret = TangoService_connectOnPoseAvailable(1, &pair, onPoseAvailableRouter);
306  if (ret != TANGO_SUCCESS)
307  {
308  LOGE("NativeRTABMap: Failed to connect to pose callback with error code: %d", ret);
309  return false;
310  }
311 
312  // Attach the onEventAvailable callback.
313  // The callback will be called after the service is connected.
314  ret = TangoService_connectOnTangoEvent(onTangoEventAvailableRouter);
315  if (ret != TANGO_SUCCESS)
316  {
317  LOGE("PointCloudApp: Failed to connect to event callback with error code: %d", ret);
318  return false;
319  }
320 
321  // Now connect service so the callbacks above will be called
322  LOGI("NativeRTABMap: Connect to tango service");
323  ret = TangoService_connect(this, tango_config_);
324  if (ret != TANGO_SUCCESS)
325  {
326  LOGE("NativeRTABMap: Failed to connect to the Tango service with error code: %d", ret);
327  return false;
328  }
329 
330  // update extrinsics
331  LOGI("NativeRTABMap: Update extrinsics");
332  TangoPoseData pose_data;
333  TangoCoordinateFramePair frame_pair;
334 
335  // TangoService_getPoseAtTime function is used for query device extrinsics
336  // as well. We use timestamp 0.0 and the target frame pair to get the
337  // extrinsics from the sensors.
338  //
339  // Get color camera with respect to device transformation matrix.
340  frame_pair.base = TANGO_COORDINATE_FRAME_DEVICE;
341  frame_pair.target = colorCamera_?TANGO_COORDINATE_FRAME_CAMERA_COLOR:TANGO_COORDINATE_FRAME_CAMERA_FISHEYE;
342  ret = TangoService_getPoseAtTime(0.0, frame_pair, &pose_data);
343  if (ret != TANGO_SUCCESS)
344  {
345  LOGE("NativeRTABMap: Failed to get transform between the color camera frame and device frames");
346  return false;
347  }
348  deviceTColorCamera_ = rtabmap::Transform(
349  pose_data.translation[0],
350  pose_data.translation[1],
351  pose_data.translation[2],
352  pose_data.orientation[0],
353  pose_data.orientation[1],
354  pose_data.orientation[2],
355  pose_data.orientation[3]);
356  deviceTColorCamera_ = rtabmap_world_T_opengl_world * deviceTColorCamera_;
357 
358  // camera intrinsic
359  TangoCameraIntrinsics color_camera_intrinsics;
360  ret = TangoService_getCameraIntrinsics(colorCamera_?TANGO_CAMERA_COLOR:TANGO_CAMERA_FISHEYE, &color_camera_intrinsics);
361  if (ret != TANGO_SUCCESS)
362  {
363  LOGE("NativeRTABMap: Failed to get the intrinsics for the color camera with error code: %d.", ret);
364  return false;
365  }
366 
367  LOGD("Calibration: fx=%f fy=%f cx=%f cy=%f width=%d height=%d",
368  color_camera_intrinsics.fx,
369  color_camera_intrinsics.fy,
370  color_camera_intrinsics.cx,
371  color_camera_intrinsics.cy,
372  color_camera_intrinsics.width,
373  color_camera_intrinsics.height);
374 
375  cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
376  K.at<double>(0,0) = color_camera_intrinsics.fx;
377  K.at<double>(1,1) = color_camera_intrinsics.fy;
378  K.at<double>(0,2) = color_camera_intrinsics.cx;
379  K.at<double>(1,2) = color_camera_intrinsics.cy;
380  cv::Mat D = cv::Mat::zeros(1, 5, CV_64FC1);
381  LOGD("Calibration type = %d", color_camera_intrinsics.calibration_type);
382  if(color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_POLYNOMIAL_5_PARAMETERS ||
383  color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_EQUIDISTANT)
384  {
385  D.at<double>(0,0) = color_camera_intrinsics.distortion[0];
386  D.at<double>(0,1) = color_camera_intrinsics.distortion[1];
387  D.at<double>(0,2) = color_camera_intrinsics.distortion[2];
388  D.at<double>(0,3) = color_camera_intrinsics.distortion[3];
389  D.at<double>(0,4) = color_camera_intrinsics.distortion[4];
390  }
391  else if(color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_POLYNOMIAL_3_PARAMETERS)
392  {
393  D.at<double>(0,0) = color_camera_intrinsics.distortion[0];
394  D.at<double>(0,1) = color_camera_intrinsics.distortion[1];
395  D.at<double>(0,2) = 0.;
396  D.at<double>(0,3) = 0.;
397  D.at<double>(0,4) = color_camera_intrinsics.distortion[2];
398  }
399  else if(color_camera_intrinsics.calibration_type == TANGO_CALIBRATION_POLYNOMIAL_2_PARAMETERS)
400  {
401  D.at<double>(0,0) = color_camera_intrinsics.distortion[0];
402  D.at<double>(0,1) = color_camera_intrinsics.distortion[1];
403  D.at<double>(0,2) = 0.;
404  D.at<double>(0,3) = 0.;
405  D.at<double>(0,4) = 0.;
406  }
407 
408  cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
409  cv::Mat P;
410 
411  LOGD("Distortion params: %f, %f, %f, %f, %f", D.at<double>(0,0), D.at<double>(0,1), D.at<double>(0,2), D.at<double>(0,3), D.at<double>(0,4));
412  model_ = CameraModel(colorCamera_?"color":"fisheye",
413  cv::Size(color_camera_intrinsics.width, color_camera_intrinsics.height),
414  K, D, R, P,
415  deviceTColorCamera_);
416 
417  if(!colorCamera_)
418  {
419  initFisheyeRectificationMap(model_, fisheyeRectifyMapX_, fisheyeRectifyMapY_);
420  }
421 
422  LOGI("deviceTColorCameraRtabmap =%s", deviceTColorCamera_.prettyPrint().c_str());
423  return true;
424 }
425 
426 void CameraTango::close()
427 {
428  if(tango_config_)
429  {
430  TangoConfig_free(tango_config_);
431  tango_config_ = nullptr;
432  LOGI("TangoService_disconnect()");
433  TangoService_disconnect();
434  LOGI("TangoService_disconnect() done.");
435  }
436  fisheyeRectifyMapX_ = cv::Mat();
437  fisheyeRectifyMapY_ = cv::Mat();
438 
439  CameraMobile::close();
440 }
441 
442 void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
443 {
444  if(!cloud.empty())
445  {
446  //LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols);
447 
448  UASSERT(cloud.type() == CV_32FC4);
449  boost::mutex::scoped_lock lock(tangoDataMutex_);
450 
451  // From post: http://stackoverflow.com/questions/29236110/timing-issues-with-tango-image-frames
452  // "In the current version of Project Tango Tablet RGB IR camera
453  // is used for both depth and color images and it can only do one
454  // or the other for each frame. So in the stream we get 4 RGB frames
455  // followed by 1 Depth frame resulting in the pattern you observed. This
456  // is more of a hardware limitation."
457  //
458  // So, synchronize with the last RGB frame before the Depth is acquired
459  if(!tangoColor_.empty())
460  {
461  UTimer timer;
462  double dt = fabs(timestamp - tangoColorStamp_);
463 
464  //LOGD("Depth: %f vs %f = %f", tangoColorStamp_, timestamp, dt);
465 
466  if(dt >= 0.0 && dt < 0.5)
467  {
468  bool notify = !tangoData_.isValid();
469 
470  cv::Mat tangoImage = tangoColor_;
471  cv::Mat rgb;
472  double cloudStamp = timestamp;
473  double rgbStamp = tangoColorStamp_;
474  int tangoColorType = tangoColorType_;
475 
476  tangoColor_ = cv::Mat();
477  tangoColorStamp_ = 0.0;
478  tangoColorType_ = 0;
479 
480  LOGD("tangoColorType=%d", tangoColorType);
481  if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
482  {
483  cv::cvtColor(tangoImage, rgb, cv::COLOR_RGBA2BGR);
484  }
485  else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YV12)
486  {
487  cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV2BGR_YV12);
488  }
489  else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
490  {
491  cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV2BGR_NV21);
492  }
493  else if(tangoColorType == 35)
494  {
495  cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV420sp2GRAY);
496  }
497  else
498  {
499  LOGE("Not supported color format : %d.", tangoColorType);
500  tangoData_ = SensorData();
501  return;
502  }
503 
504  //for(int i=0; i<rgb.cols; ++i)
505  //{
506  // UERROR("%d,%d,%d", (int)rgb.at<cv::Vec3b>(i)[0], (int)rgb.at<cv::Vec3b>(i)[1], (int)rgb.at<cv::Vec3b>(i)[2]);
507  //}
508 
509  CameraModel model = model_;
510 
511  if(colorCamera_)
512  {
513  if(decimation_ > 1)
514  {
515  rgb = util2d::decimate(rgb, decimation_);
516  model = model.scaled(1.0/double(decimation_));
517  }
518  }
519  else
520  {
521  //UTimer t;
522  cv::Mat rgbRect;
523  cv::remap(rgb, rgbRect, fisheyeRectifyMapX_, fisheyeRectifyMapY_, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0);
524  rgb = rgbRect;
525  //LOGD("Rectification time=%fs", t.ticks());
526  }
527 
528  // Querying the depth image's frame transformation based on the depth image's
529  // timestamp.
530  cv::Mat depth;
531 
532  // Calculate the relative pose from color camera frame at timestamp
533  // color_timestamp t1 and depth
534  // camera frame at depth_timestamp t0.
535  Transform colorToDepth;
536  TangoPoseData pose_color_image_t1_T_depth_image_t0;
537  if (TangoSupport_calculateRelativePose(
538  rgbStamp, colorCamera_?TANGO_COORDINATE_FRAME_CAMERA_COLOR:TANGO_COORDINATE_FRAME_CAMERA_FISHEYE, cloudStamp,
539  TANGO_COORDINATE_FRAME_CAMERA_DEPTH,
540  &pose_color_image_t1_T_depth_image_t0) == TANGO_SUCCESS)
541  {
542  colorToDepth = tangoPoseToTransform(&pose_color_image_t1_T_depth_image_t0);
543  }
544  else
545  {
546  LOGE(
547  "SynchronizationApplication: Could not find a valid relative pose at "
548  "time for color and "
549  " depth cameras.");
550  }
551 
552  if(colorToDepth.getNormSquared() > 100000)
553  {
554  LOGE("Very large color to depth error detected (%s)! Ignoring this frame!", colorToDepth.prettyPrint().c_str());
555  colorToDepth.setNull();
556  }
557  cv::Mat scan;
558  if(!colorToDepth.isNull())
559  {
560  // The Color Camera frame at timestamp t0 with respect to Depth
561  // Camera frame at timestamp t1.
562  //LOGD("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
563  LOGD("rgb=%dx%d cloud size=%d", rgb.cols, rgb.rows, (int)cloud.total());
564 
565  int pixelsSet = 0;
566  int depthSizeDec = colorCamera_?8:1;
567  depth = cv::Mat::zeros(model_.imageHeight()/depthSizeDec, model_.imageWidth()/depthSizeDec, CV_16UC1); // mm
568  CameraModel depthModel = model_.scaled(1.0f/float(depthSizeDec));
569  std::vector<cv::Point3f> scanData(rawScanPublished_?cloud.total():0);
570  int oi=0;
571  int closePoints = 0;
572  float closeROI[4];
573  closeROI[0] = depth.cols/4;
574  closeROI[1] = 3*(depth.cols/4);
575  closeROI[2] = depth.rows/4;
576  closeROI[3] = 3*(depth.rows/4);
577  unsigned short minDepthValue=10000;
578  for(unsigned int i=0; i<cloud.total(); ++i)
579  {
580  const float * p = cloud.ptr<float>(0,i);
581  cv::Point3f pt = util3d::transformPoint(cv::Point3f(p[0], p[1], p[2]), colorToDepth);
582 
583  if(pt.z > 0.0f && i%scanDownsampling == 0 && rawScanPublished_)
584  {
585  scanData.at(oi++) = pt;
586  }
587 
588  int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
589  // get the coordinate on image plane.
590  pixel_x_l = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
591  pixel_y_l = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
592  pixel_x_h = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx() + 0.5f);
593  pixel_y_h = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy() + 0.5f);
594  unsigned short depth_value(pt.z * 1000.0f);
595 
596  if(pixel_x_l>=closeROI[0] && pixel_x_l<closeROI[1] &&
597  pixel_y_l>closeROI[2] && pixel_y_l<closeROI[3] &&
598  depth_value < 600)
599  {
600  ++closePoints;
601  if(depth_value < minDepthValue)
602  {
603  minDepthValue = depth_value;
604  }
605  }
606 
607  bool pixelSet = false;
608  if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
609  pixel_y_l>0 && pixel_y_l<depth.rows && // ignore first line
610  depth_value)
611  {
612  unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_l, pixel_x_l);
613  if(depthPixel == 0 || depthPixel > depth_value)
614  {
615  depthPixel = depth_value;
616  pixelSet = true;
617  }
618  }
619  if(pixel_x_h>=0 && pixel_x_h<depth.cols &&
620  pixel_y_h>0 && pixel_y_h<depth.rows && // ignore first line
621  depth_value)
622  {
623  unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_h, pixel_x_h);
624  if(depthPixel == 0 || depthPixel > depth_value)
625  {
626  depthPixel = depth_value;
627  pixelSet = true;
628  }
629  }
630  if(pixelSet)
631  {
632  pixelsSet += 1;
633  }
634  }
635 
636  if(closePoints > 100)
637  {
638  this->post(new CameraInfoEvent(0, "TooClose", ""));
639  }
640 
641  if(oi)
642  {
643  scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
644  }
645  //LOGD("pixels depth set= %d", pixelsSet);
646  }
647  else
648  {
649  LOGE("color to depth pose is null?!? (rgb stamp=%f) (depth stamp=%f)", rgbStamp, cloudStamp);
650  }
651 
652  if(!rgb.empty() && !depth.empty())
653  {
654  depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);
655 
656  Transform odom = getPoseAtTimestamp(rgbStamp);
657 
658  //LOGD("Local = %s", model.localTransform().prettyPrint().c_str());
659  //LOGD("tango = %s", poseDevice.prettyPrint().c_str());
660  //LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());
661 
662  // occlusion depth
663  if(!depth.empty())
664  {
665  rtabmap::CameraModel depthModel = model.scaled(float(depth.cols) / float(model.imageWidth()));
666  depthModel.setLocalTransform(odom*model.localTransform());
667  this->setOcclusionImage(depth, depthModel);
668  }
669 
670  //LOGD("rtabmap = %s", odom.prettyPrint().c_str());
671  //LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str());
672 
673  Transform scanLocalTransform = model.localTransform();
674 
675  if(rawScanPublished_)
676  {
677  tangoData_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
678  }
679  else
680  {
681  tangoData_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
682  }
683  tangoData_.setGroundTruth(odom);
684  }
685  else
686  {
687  LOGE("Could not get depth and rgb images!?!");
688  tangoData_ = SensorData();
689  return;
690  }
691 
692  if(notify)
693  {
694  tangoDataReady_.release();
695  }
696  LOGD("process cloud received %fs", timer.ticks());
697  }
698  }
699  }
700 }
701 
702 void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double timestamp)
703 {
704  if(!tangoImage.empty())
705  {
706  //LOGD("RGB received! %fs", timestamp);
707 
708  boost::mutex::scoped_lock lock(tangoDataMutex_);
709 
710  tangoColor_ = tangoImage.clone();
711  tangoColorStamp_ = timestamp;
712  tangoColorType_ = type;
713  }
714 }
715 
716 void CameraTango::tangoEventReceived(int type, const char * key, const char * value)
717 {
718  this->post(new CameraInfoEvent(type, key, value));
719 }
720 
721 std::string CameraTango::getSerial() const
722 {
723  return "Tango";
724 }
725 
726 rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const
727 {
728  UASSERT(tangoPose);
729  rtabmap::Transform pose;
730 
731  pose = rtabmap::Transform(
732  tangoPose->translation[0],
733  tangoPose->translation[1],
734  tangoPose->translation[2],
735  tangoPose->orientation[0],
736  tangoPose->orientation[1],
737  tangoPose->orientation[2],
738  tangoPose->orientation[3]);
739 
740  return pose;
741 }
742 
743 rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
744 {
745  rtabmap::Transform pose;
746 
747  TangoPoseData pose_start_service_T_device;
748  TangoCoordinateFramePair frame_pair;
749  frame_pair.base = TANGO_COORDINATE_FRAME_START_OF_SERVICE;
750  frame_pair.target = TANGO_COORDINATE_FRAME_DEVICE;
751  TangoErrorType status = TangoService_getPoseAtTime(timestamp, frame_pair, &pose_start_service_T_device);
752  if (status != TANGO_SUCCESS)
753  {
754  LOGE(
755  "PoseData: Failed to get transform between the Start of service and "
756  "device frames at timestamp %lf",
757  timestamp);
758  }
759  if (pose_start_service_T_device.status_code != TANGO_POSE_VALID)
760  {
761  LOGW(
762  "PoseData: Failed to get transform between the Start of service and "
763  "device frames at timestamp %lf",
764  timestamp);
765  }
766  else
767  {
768  pose = rtabmap_world_T_tango_world * tangoPoseToTransform(&pose_start_service_T_device) * tango_device_T_rtabmap_world;
769  }
770 
771  return pose;
772 }
773 
774 SensorData CameraTango::updateDataOnRender(Transform & pose)
775 {
776  //LOGI("Capturing image...");
777 
778  pose.setNull();
779  if(textureId_ == 0)
780  {
781  glGenTextures(1, &textureId_);
782  glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
783  glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
784  glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
785  glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
786  glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
787  }
788 
789  // Update Texture (optional, just for first-view rendering)
790  if(colorCamera_ && textureId_)
791  {
792  double video_overlay_timestamp;
793  TangoErrorType status = TangoService_updateTextureExternalOes(TANGO_CAMERA_COLOR, textureId_, &video_overlay_timestamp);
794 
795  if (status == TANGO_SUCCESS)
796  {
797  pose = getPoseAtTimestamp(video_overlay_timestamp);
798 
799  int rotation = static_cast<int>(getScreenRotation()) + 1; // remove 90deg camera rotation
800  if (rotation > 3) {
801  rotation -= 4;
802  }
803 
804  TangoDoubleMatrixTransformData matrix_transform;
805  status = TangoSupport_getDoubleMatrixTransformAtTime(
806  video_overlay_timestamp,
807  TANGO_COORDINATE_FRAME_CAMERA_COLOR,
808  TANGO_COORDINATE_FRAME_START_OF_SERVICE,
809  TANGO_SUPPORT_ENGINE_OPENGL,
810  TANGO_SUPPORT_ENGINE_OPENGL,
811  static_cast<TangoSupportRotation>(rotation),
812  &matrix_transform);
813  if (matrix_transform.status_code == TANGO_POSE_VALID)
814  {
815  // Get projection matrix
816  TangoCameraIntrinsics color_camera_intrinsics;
817  int ret = TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation(
818  TANGO_CAMERA_COLOR,
819  static_cast<TangoSupportRotation>(rotation),
820  &color_camera_intrinsics);
821 
822  if (ret == TANGO_SUCCESS) {
823  float image_width = static_cast<float>(color_camera_intrinsics.width);
824  float image_height = static_cast<float>(color_camera_intrinsics.height);
825  float fx = static_cast<float>(color_camera_intrinsics.fx);
826  float fy = static_cast<float>(color_camera_intrinsics.fy);
827  float cx = static_cast<float>(color_camera_intrinsics.cx);
828  float cy = static_cast<float>(color_camera_intrinsics.cy);
829 
830  viewMatrix_ = glm::make_mat4(matrix_transform.matrix);
831 
832  projectionMatrix_ = tango_gl::Camera::ProjectionMatrixForCameraIntrinsics(
833  image_width, image_height, fx, fy, cx, cy, 0.3, 50);
834 
835  switch(rotation)
836  {
837  case ROTATION_90:
838  memcpy(transformed_uvs_, kTextureCoords90, 8*sizeof(float));
839  break;
840  case ROTATION_180:
841  memcpy(transformed_uvs_, kTextureCoords180, 8*sizeof(float));
842  break;
843  case ROTATION_270:
844  memcpy(transformed_uvs_, kTextureCoords270, 8*sizeof(float));
845  break;
846  case ROTATION_0:
847  default:
848  memcpy(transformed_uvs_, kTextureCoords0, 8*sizeof(float));
849  }
850  uvs_initialized_ = true;
851  }
852  else
853  {
854  UERROR("TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation failed!");
855  }
856  }
857  else
858  {
859  UERROR("TangoSupport_getDoubleMatrixTransformAtTime failed!");
860  }
861  }
862  else
863  {
864  UERROR("TangoService_updateTextureExternalOes failed!");
865  }
866  }
867 
868  SensorData data;
869  if(tangoDataReady_.acquireTry(1))
870  {
871  boost::mutex::scoped_lock lock(tangoDataMutex_);
872  data = tangoData_;
873  tangoData_ = SensorData();
874  pose = data.groundTruth();
875  data.setGroundTruth(Transform());
876  }
877  return data;
878 
879 }
880 
881 } /* namespace rtabmap */
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::CameraTango::CameraTango
CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing)
Definition: CameraTango.cpp:117
w
RowVector3d w
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
rtabmap::CameraModel::imageWidth
int imageWidth() const
Definition: CameraModel.h:120
glm::tan
GLM_FUNC_DECL genType tan(genType const &angle)
rtabmap::CameraTango::fisheyeRectifyMapX_
cv::Mat fisheyeRectifyMapX_
Definition: CameraTango.h:79
rtabmap::CameraMobile::close
virtual void close()
Definition: CameraMobile.cpp:86
cy
const double cy
D
MatrixXcd D
rtabmap::applyFovModel
void applyFovModel(double xu, double yu, double w, double w_inverse, double two_tan_w_div_two, double *xd, double *yd)
Definition: CameraTango.cpp:139
rtabmap::CameraTango::rawScanPublished_
bool rawScanPublished_
Definition: CameraTango.h:72
rtabmap::LaserScan::backwardCompatibility
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:133
K
K
timer
rtabmap::CameraTango::getSerial
virtual std::string getSerial() const
Definition: CameraTango.cpp:721
rtabmap::CameraMobile::projectionMatrix_
glm::mat4 projectionMatrix_
Definition: CameraMobile.h:140
ret
int ret
rtabmap::CameraTango::tango_config_
void * tango_config_
Definition: CameraTango.h:69
USemaphore::acquireTry
int acquireTry(int n)
Definition: USemaphore.h:144
fx
const double fx
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:105
this
this
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::scanDownsampling
const int scanDownsampling
Definition: CameraTango.cpp:44
rtabmap::rtabmap_world_T_tango_world
static const rtabmap::Transform rtabmap_world_T_tango_world(0.0f, 1.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f)
dt
const double dt
rtabmap::SensorData::isValid
bool isValid() const
Definition: SensorData.h:156
type
rtabmap::CameraMobile::transformed_uvs_
float transformed_uvs_[8]
Definition: CameraMobile.h:141
rtabmap::CameraMobile::uvs_initialized_
bool uvs_initialized_
Definition: CameraMobile.h:142
rtabmap::onTangoEventAvailableRouter
void onTangoEventAvailableRouter(void *context, const TangoEvent *event)
Definition: CameraTango.cpp:108
rtabmap::CameraMobile::viewMatrix_
glm::mat4 viewMatrix_
Definition: CameraMobile.h:139
rtabmap::CameraTango::tangoDataMutex_
boost::mutex tangoDataMutex_
Definition: CameraTango.h:77
rtabmap::CameraModel::K
cv::Mat K() const
Definition: CameraModel.h:110
rtabmap::CameraTango::colorCamera_
bool colorCamera_
Definition: CameraTango.h:70
rtabmap::CameraModel::D
cv::Mat D() const
Definition: CameraModel.h:111
rtabmap::Transform::setNull
void setNull()
Definition: Transform.cpp:152
rtabmap::CameraTango::fisheyeRectifyMapY_
cv::Mat fisheyeRectifyMapY_
Definition: CameraTango.h:80
rtabmap::ROTATION_270
@ ROTATION_270
270 degree rotation.
Definition: util.h:208
rtabmap::CameraMobile::deviceTColorCamera_
Transform deviceTColorCamera_
Definition: CameraMobile.h:136
rtabmap::maxDepthError
const float maxDepthError
Definition: CameraTango.cpp:43
rtabmap::util2d::decimate
cv::Mat RTABMAP_CORE_EXPORT decimate(const cv::Mat &image, int d)
Definition: util2d.cpp:1201
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
LOGE
#define LOGE(...)
Definition: tango-gl/include/tango-gl/util.h:61
fabs
Real fabs(const Real &a)
rtabmap::tango_device_T_rtabmap_world
static const rtabmap::Transform tango_device_T_rtabmap_world(0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f)
rtabmap::kVersionStringLength
const int kVersionStringLength
Definition: CameraTango.cpp:41
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::CameraModel::setLocalTransform
void setLocalTransform(const Transform &transform)
Definition: CameraModel.h:115
glm::make_mat4
GLM_FUNC_DECL detail::tmat4x4< T, defaultp > make_mat4(T const *const ptr)
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::CameraTango::tangoEventReceived
void tangoEventReceived(int type, const char *key, const char *value)
Definition: CameraTango.cpp:716
rtabmap::Transform::getNormSquared
float getNormSquared() const
Definition: Transform.cpp:288
rtabmap::holeSize
const int holeSize
Definition: CameraTango.cpp:42
rtabmap::CameraTango::tangoColorType_
int tangoColorType_
Definition: CameraTango.h:75
rtabmap::rtabmap_world_T_opengl_world
static const rtabmap::Transform rtabmap_world_T_opengl_world(0.0f, 0.0f,-1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f)
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
app
QApplication * app
Definition: tools/DataRecorder/main.cpp:59
data
int data[]
rtabmap::CameraTango::cloudReceived
void cloudReceived(const cv::Mat &cloud, double timestamp)
Definition: CameraTango.cpp:442
util3d_transforms.h
rtabmap::CameraMobile::textureId_
GLuint textureId_
Definition: CameraMobile.h:138
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::CameraTango::close
virtual void close()
Definition: CameraTango.cpp:426
rtabmap::kTextureCoords270
const float kTextureCoords270[]
Definition: CameraTango.cpp:55
rtabmap::CameraMobile::getScreenRotation
ScreenRotation getScreenRotation() const
Definition: CameraMobile.h:122
rtabmap::CameraTango::tangoDataReady_
USemaphore tangoDataReady_
Definition: CameraTango.h:78
rtabmap::CameraTango
Definition: CameraTango.h:46
rtabmap::ROTATION_90
@ ROTATION_90
90 degree rotation.
Definition: util.h:202
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:103
UASSERT
#define UASSERT(condition)
rtabmap::SensorData::setGroundTruth
void setGroundTruth(const Transform &pose)
Definition: SensorData.h:283
p
Point3_ p(2)
rtabmap::ROTATION_180
@ ROTATION_180
180 degree rotation.
Definition: util.h:205
rtabmap::CameraMobile::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraMobile.cpp:76
OdometryEvent.h
rtabmap::kTextureCoords90
const float kTextureCoords90[]
Definition: CameraTango.cpp:53
rtabmap::kTextureCoords180
const float kTextureCoords180[]
Definition: CameraTango.cpp:54
rtabmap::CameraTango::getPoseAtTimestamp
rtabmap::Transform getPoseAtTimestamp(double timestamp)
Definition: CameraTango.cpp:743
key
const gtsam::Symbol key( 'X', 0)
LOGI
#define LOGI(...)
Definition: tango-gl/include/tango-gl/util.h:53
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
camera.h
rtabmap::CameraTango::tangoColor_
cv::Mat tangoColor_
Definition: CameraTango.h:74
rtabmap::CameraTango::rgbReceived
void rgbReceived(const cv::Mat &tangoImage, int type, double timestamp)
Definition: CameraTango.cpp:702
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
rtabmap::CameraTango::tangoPoseToTransform
rtabmap::Transform tangoPoseToTransform(const TangoPoseData *tangoPose) const
Definition: CameraTango.cpp:726
USemaphore::release
void release(int n=1)
Definition: USemaphore.h:168
util2d.h
rtabmap::onPoseAvailableRouter
void onPoseAvailableRouter(void *context, const TangoPoseData *pose)
Definition: CameraTango.cpp:99
LOGW
#define LOGW(...)
Definition: tango-gl/include/tango-gl/util.h:54
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
rtabmap::CameraTango::tangoData_
SensorData tangoData_
Definition: CameraTango.h:73
rtabmap::CameraModel::scaled
CameraModel scaled(double scale) const
Definition: CameraModel.cpp:609
PointMatcherSupport::Parametrizable
LOGD
#define LOGD(...)
Definition: tango-gl/include/tango-gl/util.h:52
rtabmap::CameraTango::updateDataOnRender
virtual SensorData updateDataOnRender(Transform &pose)
Definition: CameraTango.cpp:774
rtabmap::onFrameAvailableRouter
void onFrameAvailableRouter(void *context, TangoCameraId id, const TangoImageBuffer *color)
Definition: CameraTango.cpp:67
rtabmap::CameraMobile
Definition: CameraMobile.h:71
rtabmap::CameraInfoEvent
Definition: CameraMobile.h:44
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:119
UTimer
Definition: UTimer.h:46
rtabmap::CameraModel::imageHeight
int imageHeight() const
Definition: CameraModel.h:121
epsilon
double epsilon
rtabmap::CameraTango::~CameraTango
virtual ~CameraTango()
Definition: CameraTango.cpp:129
rtabmap::ROTATION_0
@ ROTATION_0
0 degree rotation (natural orientation)
Definition: util.h:199
rtabmap::CameraTango::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraTango.cpp:193
rtabmap::CameraTango::decimation_
int decimation_
Definition: CameraTango.h:71
cx
const double cx
rtabmap::CameraMobile::model_
CameraModel model_
Definition: CameraMobile.h:135
rtabmap::CameraMobile::setOcclusionImage
void setOcclusionImage(const cv::Mat &image, const CameraModel &model)
Definition: CameraMobile.h:124
glm::atan
GLM_FUNC_DECL genType atan(genType const &y, genType const &x)
rtabmap::CameraTango::tangoColorStamp_
double tangoColorStamp_
Definition: CameraTango.h:76
tango_gl::Camera::ProjectionMatrixForCameraIntrinsics
static glm::mat4 ProjectionMatrixForCameraIntrinsics(float width, float height, float fx, float fy, float cx, float cy, float near, float far)
Definition: camera.cpp:65
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
util.h
rtabmap::kTextureCoords0
const float kTextureCoords0[]
Definition: CameraTango.cpp:52
trace.model
model
Definition: trace.py:4
CameraTango.h
value
value
i
int i
rtabmap::SensorCapture::getNextSeqID
int getNextSeqID()
Definition: SensorCapture.h:83
rtabmap::onPointCloudAvailableRouter
void onPointCloudAvailableRouter(void *context, const TangoPointCloud *point_cloud)
Definition: CameraTango.cpp:58
fy
const double fy
rtabmap::initFisheyeRectificationMap
void initFisheyeRectificationMap(const CameraModel &fisheyeModel, cv::Mat &mapX, cv::Mat &mapY)
Definition: CameraTango.cpp:160
UEventsSender::post
void post(UEvent *event, bool async=true) const
Definition: UEventsSender.cpp:28
R
R

rtabmap
Author(s): Mathieu Labbe
autogenerated on Sun Dec 1 2024 03:42:42