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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58