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 
35 namespace rtabmap {
36 
37 #define nullptr 0
38 
40 // CameraMobile
43 const float CameraMobile::bilateralFilteringSigmaR = 0.075f;
44 
46  0.0f, 0.0f, 1.0f, 0.0f,
47  -1.0f, 0.0f, 0.0f, 0.0f,
48  0.0f, -1.0f, 0.0f, 0.0f);
50  0.0f, -1.0f, 0.0f, 0.0f,
51  0.0f, 0.0f, -1.0f, 0.0f,
52  1.0f, 0.0f, 0.0f, 0.0f);
53 
54 CameraMobile::CameraMobile(bool smoothing) :
55  Camera(10),
56  deviceTColorCamera_(Transform::getIdentity()),
57  spinOncePreviousStamp_(0.0),
58  previousStamp_(0.0),
59  stampEpochOffset_(0.0),
60  smoothing_(smoothing),
61  colorCameraToDisplayRotation_(ROTATION_0),
62  originUpdate_(false)
63 {
64 }
65 
67  // Disconnect camera service
68  close();
69 }
70 
71 bool CameraMobile::init(const std::string &, const std::string &)
72 {
74  return true;
75 }
76 
78 {
80  previousStamp_ = 0.0;
81  lastKnownGPS_ = GPS();
82  lastEnvSensors_.clear();
84  originUpdate_ = false;
85  pose_ = Transform();
86  data_ = SensorData();
87 }
88 
90 {
91  originUpdate_ = true;
92 }
93 
95 {
96  if(!pose.isNull())
97  {
98  // send pose of the camera (without optical rotation)
100  if(originUpdate_)
101  {
103  originUpdate_ = false;
104  }
105 
106  if(!originOffset_.isNull())
107  {
108  this->post(new PoseEvent(originOffset_*p));
109  }
110  else
111  {
112  this->post(new PoseEvent(p));
113  }
114  }
115 }
116 
118 {
119  return model_.isValidForProjection();
120 }
121 
122 void CameraMobile::setGPS(const GPS & gps)
123 {
124  lastKnownGPS_ = gps;
125 }
126 
127 void CameraMobile::setData(const SensorData & data, const Transform & pose)
128 {
129  LOGD("CameraMobile::setData pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
130  data_ = data;
131  pose_ = pose;
132 }
133 
134 void CameraMobile::addEnvSensor(int type, float value)
135 {
136  lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
137 }
138 
140 {
141  if(!this->isRunning())
142  {
143  bool ignoreFrame = false;
144  float rate = 10.0f; // maximum 10 FPS for image data
145  double now = UTimer::now();
146  if(rate>0.0f)
147  {
148  if((spinOncePreviousStamp_>=0.0 && now>spinOncePreviousStamp_ && now - spinOncePreviousStamp_ < 1.0f/rate) ||
150  {
151  ignoreFrame = true;
152  }
153  }
154 
155  if(!ignoreFrame)
156  {
159  mainLoop();
160  }
161  else
162  {
163  // just send pose
164  capturePoseOnly();
165  }
166  }
167 }
168 
170 {
171  double t = cameraStartedTime_.elapsed();
172  if(t < 5.0)
173  {
174  uSleep((5.0-t)*1000); // just to make sure that the camera is started
175  }
176 }
177 
179 {
180  CameraInfo info;
181  SensorData data = this->captureImage(&info);
182 
183  if(data.isValid() && !info.odomPose.isNull())
184  {
185  if(lastKnownGPS_.stamp() > 0.0 && data.stamp()-lastKnownGPS_.stamp()<1.0)
186  {
187  data.setGPS(lastKnownGPS_);
188  }
189  else if(lastKnownGPS_.stamp()>0.0)
190  {
191  LOGD("GPS too old (current time=%f, gps time = %f)", data.stamp(), lastKnownGPS_.stamp());
192  }
193 
194  if(lastEnvSensors_.size())
195  {
197  lastEnvSensors_.clear();
198  }
199 
200  if(smoothing_ && !data.depthRaw().empty())
201  {
202  //UTimer t;
204  //LOGD("Bilateral filtering, time=%fs", t.ticks());
205  }
206 
207  // Rotate image depending on the camera orientation
209  {
210  UDEBUG("ROTATION_90");
211  cv::Mat rgb, depth;
212  cv::Mat rgbt(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
213  cv::flip(data.imageRaw(),rgb,1);
214  cv::transpose(rgb,rgbt);
215  rgb = rgbt;
216  cv::Mat deptht(data.depthRaw().cols, data.depthRaw().rows, data.depthRaw().type());
217  cv::flip(data.depthRaw(),depth,1);
218  cv::transpose(depth,deptht);
219  depth = deptht;
220  CameraModel model = data.cameraModels()[0];
221  cv::Size sizet(model.imageHeight(), model.imageWidth());
222  model = CameraModel(
223  model.fy(),
224  model.fx(),
225  model.cy(),
226  model.cx()>0?model.imageWidth()-model.cx():0,
227  model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
228  model.setImageSize(sizet);
229  data.setRGBDImage(rgb, depth, model);
230 
231  std::vector<cv::KeyPoint> keypoints = data.keypoints();
232  for(size_t i=0; i<keypoints.size(); ++i)
233  {
234  keypoints[i].pt.x = data.keypoints()[i].pt.y;
235  keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.x;
236  }
237  data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
238  }
240  {
241  UDEBUG("ROTATION_180");
242  cv::Mat rgb, depth;
243  cv::flip(data.imageRaw(),rgb,1);
244  cv::flip(rgb,rgb,0);
245  cv::flip(data.depthOrRightRaw(),depth,1);
246  cv::flip(depth,depth,0);
247  CameraModel model = data.cameraModels()[0];
248  cv::Size sizet(model.imageWidth(), model.imageHeight());
249  model = CameraModel(
250  model.fx(),
251  model.fy(),
252  model.cx()>0?model.imageWidth()-model.cx():0,
253  model.cy()>0?model.imageHeight()-model.cy():0,
254  model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
255  model.setImageSize(sizet);
256  data.setRGBDImage(rgb, depth, model);
257 
258  std::vector<cv::KeyPoint> keypoints = data.keypoints();
259  for(size_t i=0; i<keypoints.size(); ++i)
260  {
261  keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.x;
262  keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.y;
263  }
264  data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
265  }
267  {
268  UDEBUG("ROTATION_270");
269  cv::Mat rgb(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
270  cv::transpose(data.imageRaw(),rgb);
271  cv::flip(rgb,rgb,1);
272  cv::Mat depth(data.depthOrRightRaw().cols, data.depthOrRightRaw().rows, data.depthOrRightRaw().type());
273  cv::transpose(data.depthOrRightRaw(),depth);
274  cv::flip(depth,depth,1);
275  CameraModel model = data.cameraModels()[0];
276  cv::Size sizet(model.imageHeight(), model.imageWidth());
277  model = CameraModel(
278  model.fy(),
279  model.fx(),
280  model.cy()>0?model.imageHeight()-model.cy():0,
281  model.cx(),
282  model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
283  model.setImageSize(sizet);
284  data.setRGBDImage(rgb, depth, model);
285 
286  std::vector<cv::KeyPoint> keypoints = data.keypoints();
287  for(size_t i=0; i<keypoints.size(); ++i)
288  {
289  keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.y;
290  keypoints[i].pt.y = data.keypoints()[i].pt.x;
291  }
292  data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
293  }
294 
295  rtabmap::Transform pose = info.odomPose;
296  data.setGroundTruth(Transform());
297 
298  // convert stamp to epoch
299  bool firstFrame = previousPose_.isNull();
300  if(firstFrame)
301  {
303  }
304  data.setStamp(stampEpochOffset_ + data.stamp());
305  OdometryInfo info;
306  if(!firstFrame)
307  {
308  info.interval = data.stamp()-previousStamp_;
309  info.transform = previousPose_.inverse() * pose;
310  }
311  // linear cov = 0.0001
312  info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
313  if(!firstFrame)
314  {
315  // angular cov = 0.000001
316  info.reg.covariance.at<double>(3,3) *= 0.01;
317  info.reg.covariance.at<double>(4,4) *= 0.01;
318  info.reg.covariance.at<double>(5,5) *= 0.01;
319  }
320  LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.0001);
321  this->post(new OdometryEvent(data, pose, info));
322  previousPose_ = pose;
323  previousStamp_ = data.stamp();
324  }
325  else if(!this->isKilled())
326  {
327  LOGW("Odometry lost");
328  this->post(new OdometryEvent());
329  }
330 }
331 
333 {
334  if(info)
335  {
336  info->odomPose = pose_;
337  }
338  return data_;
339 }
340 
341 } /* namespace rtabmap */
int imageWidth() const
Definition: CameraModel.h:120
void setImageSize(const cv::Size &size)
0 degree rotation (natural orientation)
Definition: util.h:197
std::string prettyPrint() const
Definition: Transform.cpp:295
#define LOGW(...)
void addEnvSensor(int type, float value)
virtual SensorData captureImage(CameraInfo *info=0)
double elapsed()
Definition: UTimer.h:75
f
ScreenRotation colorCameraToDisplayRotation_
Definition: CameraMobile.h:123
virtual void mainLoop()
Transform translation() const
Definition: Transform.cpp:203
bool isRunning() const
Definition: UThread.cpp:245
void setRGBDImage(const cv::Mat &rgb, const cv::Mat &depth, const CameraModel &model, bool clearPreviousData=true)
Definition: SensorData.cpp:193
cv::Mat RTABMAP_EXP fastBilateralFiltering(const cv::Mat &depth, float sigmaS=15.0f, float sigmaR=0.05f, bool earlyDivision=false)
Definition: util2d.cpp:1814
const std::vector< cv::KeyPoint > & keypoints() const
Definition: SensorData.h:249
void setGPS(const GPS &gps)
Definition: SensorData.h:265
void poseReceived(const Transform &pose)
const cv::Mat & imageRaw() const
Definition: SensorData.h:164
void setData(const SensorData &data, const Transform &pose)
double getElapsedTime()
Definition: UTimer.cpp:97
void post(UEvent *event, bool async=true) const
bool isValid() const
Definition: SensorData.h:137
#define LOGD(...)
virtual void mainLoopBegin()
bool isNull() const
Definition: Transform.cpp:107
Transform deviceTColorCamera_
Definition: CameraMobile.h:113
CameraMobile(bool smoothing=false)
bool isKilled() const
Definition: UThread.cpp:255
EnvSensors lastEnvSensors_
Definition: CameraMobile.h:125
270 degree rotation.
Definition: util.h:206
static const float bilateralFilteringSigmaS
Definition: CameraMobile.h:73
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:165
void start()
Definition: UTimer.cpp:87
virtual void capturePoseOnly()
Definition: CameraMobile.h:106
180 degree rotation.
Definition: util.h:203
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
void setEnvSensors(const EnvSensors &sensors)
Definition: SensorData.h:271
double stamp() const
Definition: SensorData.h:157
#define LOGI(...)
#define false
Definition: ConvertUTF.c:56
void uSleep(unsigned int ms)
bool isValidForProjection() const
Definition: CameraModel.h:87
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:215
#define UDEBUG(...)
static const rtabmap::Transform opticalRotation
Definition: CameraMobile.h:76
static const float bilateralFilteringSigmaR
Definition: CameraMobile.h:74
90 degree rotation.
Definition: util.h:200
virtual bool isCalibrated() const
ULogger class and convenient macros.
static double now()
Definition: UTimer.cpp:80
void setStamp(double stamp)
Definition: SensorData.h:158
RegistrationInfo reg
Definition: OdometryInfo.h:96
void setGroundTruth(const Transform &pose)
Definition: SensorData.h:258
Transform inverse() const
Definition: Transform.cpp:178
static const rtabmap::Transform opticalRotationInv
Definition: CameraMobile.h:77
virtual void close()
Transform odomPose
Definition: CameraInfo.h:69
int imageHeight() const
Definition: CameraModel.h:121
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:757
const std::vector< cv::Point3f > & keypoints3D() const
Definition: SensorData.h:250
void setGPS(const GPS &gps)
cv::Mat depthRaw() const
Definition: SensorData.h:189
const double & stamp() const
Definition: GPS.h:59


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