CameraMobile.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 "CameraMobile.h"
29 #include "util.h"
33 #include "rtabmap/core/util2d.h"
34 #include <glm/gtx/transform.hpp>
35 
36 namespace rtabmap {
37 
38 #define nullptr 0
39 
41 // CameraMobile
44 const float CameraMobile::bilateralFilteringSigmaR = 0.075f;
45 
47  0.0f, 0.0f, 1.0f, 0.0f,
48  -1.0f, 0.0f, 0.0f, 0.0f,
49  0.0f, -1.0f, 0.0f, 0.0f);
51  0.0f, -1.0f, 0.0f, 0.0f,
52  0.0f, 0.0f, -1.0f, 0.0f,
53  1.0f, 0.0f, 0.0f, 0.0f);
54 
55 CameraMobile::CameraMobile(bool smoothing) :
56  Camera(10),
57  deviceTColorCamera_(Transform::getIdentity()),
58  textureId_(0),
59  uvs_initialized_(false),
60  stampEpochOffset_(0.0),
61  smoothing_(smoothing),
62  colorCameraToDisplayRotation_(ROTATION_0),
63  originUpdate_(false)
64 {
65 }
66 
68  // Disconnect camera service
69  close();
70 }
71 
72 bool CameraMobile::init(const std::string &, const std::string &)
73 {
75  return true;
76 }
77 
79 {
80  firstFrame_ = true;
81  lastKnownGPS_ = GPS();
82  lastEnvSensors_.clear();
84  originUpdate_ = false;
85  dataPose_ = Transform();
86  data_ = SensorData();
87 
88  if(textureId_ != 0)
89  {
90  glDeleteTextures(1, &textureId_);
91  textureId_ = 0;
92  }
93 }
94 
96 {
97  firstFrame_ = true;
98  lastKnownGPS_ = GPS();
99  lastEnvSensors_.clear();
100  dataPose_ = Transform();
101  data_ = SensorData();
102  originUpdate_ = true;
103 }
104 
105 bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
106 {
107  pose.setNull();
108 
109  int maxWaitTimeMs = maxWaitTime * 1000;
110 
111  // Interpolate pose
112  if(!poseBuffer_.empty())
113  {
114  poseMutex_.lock();
115  int waitTry = 0;
116  while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
117  {
118  poseMutex_.unlock();
119  ++waitTry;
120  uSleep(1);
121  poseMutex_.lock();
122  }
123  if(poseBuffer_.rbegin()->first < stamp)
124  {
125  if(maxWaitTimeMs > 0)
126  {
127  UWARN("Could not find poses to interpolate at time %f after waiting %d ms (latest is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first);
128  }
129  else
130  {
131  UWARN("Could not find poses to interpolate at time %f (latest is %f)...", stamp, poseBuffer_.rbegin()->first);
132  }
133  }
134  else
135  {
136  std::map<double, Transform>::const_iterator iterB = poseBuffer_.lower_bound(stamp);
137  std::map<double, Transform>::const_iterator iterA = iterB;
138  if(iterA != poseBuffer_.begin())
139  {
140  iterA = --iterA;
141  }
142  if(iterB == poseBuffer_.end())
143  {
144  iterB = --iterB;
145  }
146  if(iterA == iterB && stamp == iterA->first)
147  {
148  pose = iterA->second;
149  }
150  else if(stamp >= iterA->first && stamp <= iterB->first)
151  {
152  pose = iterA->second.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
153  }
154  else // stamp < iterA->first
155  {
156  UWARN("Could not find pose data to interpolate at time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
157  }
158  }
159  poseMutex_.unlock();
160  }
161  return !pose.isNull();
162 }
163 
164 void CameraMobile::poseReceived(const Transform & pose, double deviceStamp)
165 {
166  if(!pose.isNull())
167  {
168  Transform p = pose;
169  if(originUpdate_)
170  {
171  originOffset_ = p.translation().inverse();
172  originUpdate_ = false;
173  }
174 
175  if(stampEpochOffset_ == 0.0)
176  {
177  stampEpochOffset_ = UTimer::now() - deviceStamp;
178  }
179 
180  double epochStamp = stampEpochOffset_ + deviceStamp;
181 
182  if(!originOffset_.isNull())
183  {
184  p = originOffset_*p;
185  }
186 
187  {
188  UScopeMutex lock(poseMutex_);
189  poseBuffer_.insert(poseBuffer_.end(), std::make_pair(epochStamp, p));
190  if(poseBuffer_.size() > 1000)
191  {
192  poseBuffer_.erase(poseBuffer_.begin());
193  }
194  }
195 
196  // send pose of the camera (with optical rotation)
197  this->post(new PoseEvent(p * deviceTColorCamera_));
198  }
199 }
200 
202 {
203  return model_.isValidForProjection();
204 }
205 
206 void CameraMobile::setGPS(const GPS & gps)
207 {
208  lastKnownGPS_ = gps;
209 }
210 
211 void CameraMobile::addEnvSensor(int type, float value)
212 {
214 }
215 
216 void CameraMobile::update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord)
217 {
218  UScopeMutex lock(dataMutex_);
219 
220  bool notify = !data_.isValid();
221 
222  LOGD("CameraMobile::update pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
223  data_ = data;
224  dataPose_ = pose;
225 
226  viewMatrix_ = viewMatrix;
227  projectionMatrix_ = projectionMatrix;
228 
229  // adjust origin
230  if(!originOffset_.isNull())
231  {
234  }
235 
236  if(textureId_ == 0)
237  {
238  glGenTextures(1, &textureId_);
239  }
240 
241  if(texCoord)
242  {
243  memcpy(transformed_uvs_, texCoord, 8*sizeof(float));
244  uvs_initialized_ = true;
245  }
246 
247  LOGD("CameraMobile::update textureId_=%d", (int)textureId_);
248 
249  if(textureId_ != 0 && texCoord != 0)
250  {
251  cv::Mat rgbImage;
252  cv::cvtColor(data.imageRaw(), rgbImage, cv::COLOR_BGR2RGBA);
253 
254  glBindTexture(GL_TEXTURE_2D, textureId_);
255  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
256  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
257  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
258  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
259 
260  glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
261  //glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
262  //glPixelStorei(GL_UNPACK_SKIP_PIXELS, 0);
263  //glPixelStorei(GL_UNPACK_SKIP_ROWS, 0);
264  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, rgbImage.cols, rgbImage.rows, 0, GL_RGBA, GL_UNSIGNED_BYTE, rgbImage.data);
265 
266  GLint error = glGetError();
267  if(error != GL_NO_ERROR)
268  {
269  LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
270  textureId_ = 0;
271  return;
272  }
273  }
274 
275  postUpdate();
276 
277  if(notify)
278  {
280  }
281 }
282 
284 {
285  UScopeMutex lock(dataMutex_);
286  bool notify = !data_.isValid();
287 
289 
290  if(data_.isValid())
291  {
292  postUpdate();
293 
294  if(notify)
295  {
297  }
298  }
299 }
300 
302 {
303  LOGE("To use CameraMobile::updateOnRender(), CameraMobile::updateDataOnRender() "
304  "should be overridden by inherited classes. Returning empty data!\n");
305  return SensorData();
306 }
307 
309 {
310  if(data_.isValid())
311  {
312  if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
313  {
315  }
316  else if(lastKnownGPS_.stamp()>0.0)
317  {
318  LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
319  }
320 
321  if(lastEnvSensors_.size())
322  {
324  lastEnvSensors_.clear();
325  }
326 
327  if(smoothing_ && !data_.depthRaw().empty())
328  {
329  //UTimer t;
331  //LOGD("Bilateral filtering, time=%fs", t.ticks());
332  }
333 
334  // Rotate image depending on the camera orientation
336  {
337  UDEBUG("ROTATION_90");
338  cv::Mat rgb, depth;
339  cv::Mat rgbt(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
340  cv::flip(data_.imageRaw(),rgb,1);
341  cv::transpose(rgb,rgbt);
342  rgb = rgbt;
343  cv::Mat deptht(data_.depthRaw().cols, data_.depthRaw().rows, data_.depthRaw().type());
344  cv::flip(data_.depthRaw(),depth,1);
345  cv::transpose(depth,deptht);
346  depth = deptht;
348  cv::Size sizet(model.imageHeight(), model.imageWidth());
349  model = CameraModel(
350  model.fy(),
351  model.fx(),
352  model.cy(),
353  model.cx()>0?model.imageWidth()-model.cx():0,
354  model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
355  model.setImageSize(sizet);
356  data_.setRGBDImage(rgb, depth, model);
357 
358  std::vector<cv::KeyPoint> keypoints = data_.keypoints();
359  for(size_t i=0; i<keypoints.size(); ++i)
360  {
361  keypoints[i].pt.x = data_.keypoints()[i].pt.y;
362  keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.x;
363  }
364  data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
365  }
367  {
368  UDEBUG("ROTATION_180");
369  cv::Mat rgb, depth;
370  cv::flip(data_.imageRaw(),rgb,1);
371  cv::flip(rgb,rgb,0);
372  cv::flip(data_.depthOrRightRaw(),depth,1);
373  cv::flip(depth,depth,0);
375  cv::Size sizet(model.imageWidth(), model.imageHeight());
376  model = CameraModel(
377  model.fx(),
378  model.fy(),
379  model.cx()>0?model.imageWidth()-model.cx():0,
380  model.cy()>0?model.imageHeight()-model.cy():0,
381  model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
382  model.setImageSize(sizet);
383  data_.setRGBDImage(rgb, depth, model);
384 
385  std::vector<cv::KeyPoint> keypoints = data_.keypoints();
386  for(size_t i=0; i<keypoints.size(); ++i)
387  {
388  keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.x;
389  keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.y;
390  }
391  data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
392  }
394  {
395  UDEBUG("ROTATION_270");
396  cv::Mat rgb(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
397  cv::transpose(data_.imageRaw(),rgb);
398  cv::flip(rgb,rgb,1);
399  cv::Mat depth(data_.depthOrRightRaw().cols, data_.depthOrRightRaw().rows, data_.depthOrRightRaw().type());
400  cv::transpose(data_.depthOrRightRaw(),depth);
401  cv::flip(depth,depth,1);
403  cv::Size sizet(model.imageHeight(), model.imageWidth());
404  model = CameraModel(
405  model.fy(),
406  model.fx(),
407  model.cy()>0?model.imageHeight()-model.cy():0,
408  model.cx(),
409  model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
410  model.setImageSize(sizet);
411  data_.setRGBDImage(rgb, depth, model);
412 
413  std::vector<cv::KeyPoint> keypoints = data_.keypoints();
414  for(size_t i=0; i<keypoints.size(); ++i)
415  {
416  keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.y;
417  keypoints[i].pt.y = data_.keypoints()[i].pt.x;
418  }
419  data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
420  }
421  }
422 }
423 
425 {
427  if(dataReady_.acquire(1, 5000))
428  {
429  UScopeMutex lock(dataMutex_);
430  data = data_;
431  data_ = SensorData();
432  }
433  if(data.isValid())
434  {
435  data.setGroundTruth(Transform());
436  data.setStamp(stampEpochOffset_ + data.stamp());
437 
438  if(info)
439  {
440  // linear cov = 0.0001
441  info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame_?9999.0:0.0001);
442  if(!firstFrame_)
443  {
444  // angular cov = 0.000001
445  info->odomCovariance.at<double>(3,3) *= 0.01;
446  info->odomCovariance.at<double>(4,4) *= 0.01;
447  info->odomCovariance.at<double>(5,5) *= 0.01;
448  }
449  info->odomPose = dataPose_;
450  }
451 
452  firstFrame_ = false;
453  }
454  else
455  {
456  UWARN("CameraMobile::captureImage() invalid data!");
457  }
458  return data;
459 }
460 
462  const cv::Mat & pointCloudData,
463  int points,
464  const Transform & pose,
465  const CameraModel & model,
466  const cv::Mat & rgb,
467  std::vector<cv::KeyPoint> * kpts,
468  std::vector<cv::Point3f> * kpts3D,
469  int kptsSize)
470 {
471  if(!pointCloudData.empty())
472  {
473  cv::Mat scanData(1, pointCloudData.cols, CV_32FC4);
474  float * ptr = scanData.ptr<float>();
475  const float * inPtr = pointCloudData.ptr<float>();
476  int ic = pointCloudData.channels();
477  UASSERT(pointCloudData.depth() == CV_32F && ic >= 3);
478 
479  int oi = 0;
480  for(unsigned int i=0;i<points; ++i)
481  {
482  cv::Point3f pt(inPtr[i*ic], inPtr[i*ic + 1], inPtr[i*ic + 2]);
484  ptr[oi*4] = pt.x;
485  ptr[oi*4 + 1] = pt.y;
486  ptr[oi*4 + 2] = pt.z;
487 
488  //get color from rgb image
489  cv::Point3f org= pt;
491  if(pt.z > 0)
492  {
493  int u,v;
494  model.reproject(pt.x, pt.y, pt.z, u, v);
495  unsigned char r=255,g=255,b=255;
496  if(model.inFrame(u, v))
497  {
498  b=rgb.at<cv::Vec3b>(v,u).val[0];
499  g=rgb.at<cv::Vec3b>(v,u).val[1];
500  r=rgb.at<cv::Vec3b>(v,u).val[2];
501  if(kpts)
502  kpts->push_back(cv::KeyPoint(u,v,kptsSize));
503  if(kpts3D)
504  kpts3D->push_back(org);
505 
506  *(int*)&ptr[oi*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16);
507  ++oi;
508  }
509  }
510  //confidence
511  //*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16);
512 
513  }
514  return LaserScan::backwardCompatibility(scanData.colRange(0, oi), 0, 10, rtabmap::Transform::getIdentity());
515  }
516  return LaserScan();
517 }
518 
519 } /* namespace rtabmap */
rtabmap::SensorData
Definition: SensorData.h:51
int
int
UMutex::lock
int lock() const
Definition: UMutex.h:87
rtabmap::CameraMobile::scanFromPointCloudData
static LaserScan scanFromPointCloudData(const cv::Mat &pointCloudData, int points, const Transform &pose, const CameraModel &model, const cv::Mat &rgb, std::vector< cv::KeyPoint > *kpts=0, std::vector< cv::Point3f > *kpts3D=0, int kptsSize=3)
Definition: CameraMobile.cpp:461
rtabmap::CameraMobile::postUpdate
void postUpdate()
Definition: CameraMobile.cpp:308
USemaphore::acquire
bool acquire(int n=1, int ms=0)
Definition: USemaphore.h:101
rtabmap::CameraMobile::close
virtual void close()
Definition: CameraMobile.cpp:78
rtabmap::CameraMobile::poseMutex_
UMutex poseMutex_
Definition: CameraMobile.h:159
rtabmap::CameraMobile::captureImage
virtual SensorData captureImage(SensorCaptureInfo *info=0)
Definition: CameraMobile.cpp:424
rtabmap::CameraMobile::isCalibrated
virtual bool isCalibrated() const
Definition: CameraMobile.cpp:201
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
UTimer::now
static double now()
Definition: UTimer.cpp:80
rtabmap::CameraMobile::colorCameraToDisplayRotation_
ScreenRotation colorCameraToDisplayRotation_
Definition: CameraMobile.h:148
CameraMobile.h
rtabmap::CameraMobile::projectionMatrix_
glm::mat4 projectionMatrix_
Definition: CameraMobile.h:140
b
Array< int, 3, 1 > b
rtabmap::CameraMobile::setGPS
void setGPS(const GPS &gps)
Definition: CameraMobile.cpp:206
UMutex::unlock
int unlock() const
Definition: UMutex.h:113
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::CameraMobile::dataPose_
Transform dataPose_
Definition: CameraMobile.h:157
rtabmap::SensorData::isValid
bool isValid() const
Definition: SensorData.h:156
rtabmap::GPS
Definition: GPS.h:35
type
rtabmap::CameraMobile::dataMutex_
UMutex dataMutex_
Definition: CameraMobile.h:155
rtabmap::CameraMobile::transformed_uvs_
float transformed_uvs_[8]
Definition: CameraMobile.h:141
rtabmap::CameraMobile::uvs_initialized_
bool uvs_initialized_
Definition: CameraMobile.h:142
rtabmap::PoseEvent
Definition: CameraMobile.h:60
rtabmap::CameraMobile::addEnvSensor
void addEnvSensor(int type, float value)
Definition: CameraMobile.cpp:211
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:176
rtabmap::CameraMobile::viewMatrix_
glm::mat4 viewMatrix_
Definition: CameraMobile.h:139
rtabmap::Transform::setNull
void setNull()
Definition: Transform.cpp:152
rtabmap::LaserScan
Definition: LaserScan.h:37
rtabmap::ROTATION_270
@ ROTATION_270
270 degree rotation.
Definition: util.h:208
rtabmap::CameraMobile::deviceTColorCamera_
Transform deviceTColorCamera_
Definition: CameraMobile.h:136
glm::detail::tmat4x4
Definition: type_mat.hpp:47
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:240
LOGE
#define LOGE(...)
Definition: tango-gl/include/tango-gl/util.h:61
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
rtabmap::CameraMobile::smoothing_
bool smoothing_
Definition: CameraMobile.h:147
rtabmap::SensorData::setRGBDImage
void setRGBDImage(const cv::Mat &rgb, const cv::Mat &depth, const CameraModel &model, bool clearPreviousData=true)
Definition: SensorData.cpp:227
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::CameraMobile::lastKnownGPS_
GPS lastKnownGPS_
Definition: CameraMobile.h:149
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
data
int data[]
UScopeMutex
Definition: UMutex.h:157
util3d_transforms.h
g
float g
rtabmap::util2d::fastBilateralFiltering
cv::Mat RTABMAP_CORE_EXPORT fastBilateralFiltering(const cv::Mat &depth, float sigmaS=15.0f, float sigmaR=0.05f, bool earlyDivision=false)
Definition: util2d.cpp:1814
rtabmap::CameraMobile::textureId_
GLuint textureId_
Definition: CameraMobile.h:138
rtabmap::opengl_world_T_rtabmap_world
static const rtabmap::Transform opengl_world_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::CameraMobile::lastEnvSensors_
EnvSensors lastEnvSensors_
Definition: CameraMobile.h:150
first
constexpr int first(int i)
rtabmap::CameraMobile::update
void update(const SensorData &data, const Transform &pose, const glm::mat4 &viewMatrix, const glm::mat4 &projectionMatrix, const float *texCoord)
Definition: CameraMobile.cpp:216
rtabmap::Transform::interpolate
Transform interpolate(float t, const Transform &other) const
Definition: Transform.cpp:303
rtabmap::CameraModel::isValidForProjection
bool isValidForProjection() const
Definition: CameraModel.h:87
rtabmap::EnvSensor::Type
Type
Definition: EnvSensor.h:36
info
else if n * info
rtabmap::ROTATION_90
@ ROTATION_90
90 degree rotation.
Definition: util.h:202
rtabmap::CameraMobile::getPose
virtual bool getPose(double epochStamp, Transform &pose, cv::Mat &covariance, double maxWaitTime=0.06)
Definition: CameraMobile.cpp:105
UASSERT
#define UASSERT(condition)
error
void error(const char *str)
p
Point3_ p(2)
rtabmap::ROTATION_180
@ ROTATION_180
180 degree rotation.
Definition: util.h:205
rtabmap::GPS::stamp
const double & stamp() const
Definition: GPS.h:59
rtabmap::CameraMobile::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraMobile.cpp:72
rtabmap::CameraMobile::resetOrigin
void resetOrigin()
Definition: CameraMobile.cpp:95
rtabmap::CameraMobile::opticalRotation
static const rtabmap::Transform opticalRotation
Definition: CameraMobile.h:76
OdometryEvent.h
rtabmap::Camera
Definition: Camera.h:43
rtabmap::CameraMobile::updateOnRender
void updateOnRender()
Definition: CameraMobile.cpp:283
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::SensorData::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:801
UWARN
#define UWARN(...)
rtabmap::CameraMobile::dataReady_
USemaphore dataReady_
Definition: CameraMobile.h:154
rtabmap::CameraMobile::poseReceived
void poseReceived(const Transform &pose, double deviceStamp)
Definition: CameraMobile.cpp:164
rtabmap::CameraMobile::CameraMobile
CameraMobile(bool smoothing=false)
Definition: CameraMobile.cpp:55
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
ULogger.h
ULogger class and convenient macros.
transform.hpp
rtabmap::Transform
Definition: Transform.h:41
USemaphore::release
void release(int n=1)
Definition: USemaphore.h:168
util2d.h
rtabmap::CameraMobile::updateDataOnRender
virtual SensorData updateDataOnRender(Transform &pose)
Definition: CameraMobile.cpp:301
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:210
LOGD
#define LOGD(...)
Definition: tango-gl/include/tango-gl/util.h:52
rtabmap::CameraMobile::bilateralFilteringSigmaS
static const float bilateralFilteringSigmaS
Definition: CameraMobile.h:73
v
Array< int, Dynamic, 1 > v
UDEBUG
#define UDEBUG(...)
rtabmap::CameraMobile::data_
SensorData data_
Definition: CameraMobile.h:156
rtabmap::CameraMobile::bilateralFilteringSigmaR
static const float bilateralFilteringSigmaR
Definition: CameraMobile.h:74
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::CameraMobile::opticalRotationInv
static const rtabmap::Transform opticalRotationInv
Definition: CameraMobile.h:77
rtabmap::CameraMobile::originUpdate_
bool originUpdate_
Definition: CameraMobile.h:152
rtabmap::SensorData::keypoints
const std::vector< cv::KeyPoint > & keypoints() const
Definition: SensorData.h:274
false
#define false
Definition: ConvertUTF.c:56
rtabmap::ROTATION_0
@ ROTATION_0
0 degree rotation (natural orientation)
Definition: util.h:199
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:184
rtabmap::CameraMobile::model_
CameraModel model_
Definition: CameraMobile.h:135
rtabmap::glmFromTransform
glm::mat4 glmFromTransform(const rtabmap::Transform &transform)
Definition: util.h:124
rtabmap::SensorData::setGPS
void setGPS(const GPS &gps)
Definition: SensorData.h:290
rtabmap::CameraMobile::~CameraMobile
virtual ~CameraMobile()
Definition: CameraMobile.cpp:67
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::SensorData::keypoints3D
const std::vector< cv::Point3f > & keypoints3D() const
Definition: SensorData.h:275
rtabmap::EnvSensor
Definition: EnvSensor.h:33
util.h
trace.model
model
Definition: trace.py:4
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
value
value
i
int i
rtabmap::CameraMobile::firstFrame_
bool firstFrame_
Definition: CameraMobile.h:145
rtabmap::SensorData::setEnvSensors
void setEnvSensors(const EnvSensors &sensors)
Definition: SensorData.h:296
rtabmap::SensorData::setDepthOrRightRaw
RTABMAP_DEPRECATED void setDepthOrRightRaw(const cv::Mat &image)
Definition: SensorData.cpp:406
rtabmap::CameraMobile::originOffset_
Transform originOffset_
Definition: CameraMobile.h:151
rtabmap::CameraMobile::poseBuffer_
std::map< double, Transform > poseBuffer_
Definition: CameraMobile.h:160
rtabmap::CameraMobile::stampEpochOffset_
double stampEpochOffset_
Definition: CameraMobile.h:146
UEventsSender::post
void post(UEvent *event, bool async=true) const
Definition: UEventsSender.cpp:28


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