OdometryThread.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 
30 #include "rtabmap/core/Odometry.h"
35 
36 namespace rtabmap {
37 
38 OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize) :
39  _odometry(odometry),
40  _dataBufferMaxSize(dataBufferMaxSize),
41  _resetOdometry(false),
42  _resetPose(Transform::getIdentity()),
43  _lastImuStamp(0.0),
44  _imuEstimatedDelay(0.0)
45 {
46  UASSERT(_odometry != 0);
47 }
48 
50 {
52  this->join(true);
53  delete _odometry;
54  UDEBUG("");
55 }
56 
58 {
59  if(this->isRunning())
60  {
61  if(event->getClassName().compare("SensorEvent") == 0)
62  {
63  SensorEvent * sensorEvent = (SensorEvent*)event;
64  if(sensorEvent->getCode() == SensorEvent::kCodeData)
65  {
66  this->addData(sensorEvent->data());
67  }
68  }
69  else if(event->getClassName().compare("IMUEvent") == 0)
70  {
71  IMUEvent * imuEvent = (IMUEvent*)event;
72  if(!imuEvent->getData().empty())
73  {
74  this->addData(SensorData(imuEvent->getData(), 0, imuEvent->getStamp()));
75  }
76  }
77  }
78  if(event->getClassName().compare("OdometryResetEvent") == 0)
79  {
80  OdometryResetEvent * odomEvent = (OdometryResetEvent*)event;
82  if(!odomEvent->getPose().isNull())
83  {
84  _resetPose = odomEvent->getPose();
85  }
86  _resetOdometry = true;
87  }
88  return false;
89 }
90 
92 {
94 }
95 
97 {
99 }
100 
101 //============================================================
102 // MAIN LOOP
103 //============================================================
105 {
106  if(_resetOdometry)
107  {
109  _resetOdometry = false;
110  UScopeMutex lock(_dataMutex);
111  _dataBuffer.clear();
112  _imuBuffer.clear();
113  _lastImuStamp = 0.0f;
114  }
115 
117  if(getData(data))
118  {
120  UDEBUG("Processing data...");
121  Transform pose = _odometry->process(data, &info);
122  if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (pose.isNull() && data.imu().empty()))
123  {
124  UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
125  // a null pose notify that odometry could not be computed
126  this->post(new OdometryEvent(data, pose, info));
127  }
128  }
129 }
130 
132 {
133  if(data.imu().empty())
134  {
135  if(dynamic_cast<OdometryMono*>(_odometry) == 0)
136  {
137  if((data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty())) &&
138  data.laserScanRaw().empty())
139  {
140  ULOGGER_ERROR("Missing some information (images/scans empty or missing calibration)!?");
141  return;
142  }
143  }
144  else
145  {
146  // Mono can accept RGB only
147  if(data.imageRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty()))
148  {
149  ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
150  return;
151  }
152  }
153  }
154 
155  bool notify = true;
156  _dataMutex.lock();
157  {
158  if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty() || data.imu().empty())
159  {
160  _dataBuffer.push_back(data);
161  while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
162  {
163  UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
164  _dataBuffer.erase(_dataBuffer.begin());
165  notify = false;
166  }
167  }
168  else
169  {
170  _imuBuffer.push_back(data);
171  if(_lastImuStamp != 0.0 && data.stamp() > _lastImuStamp)
172  {
174  }
175  _lastImuStamp = data.stamp();
176  }
177  }
178  _dataMutex.unlock();
179 
180  if(notify)
181  {
183  }
184 }
185 
187 {
188  bool dataFilled = false;
190  _dataMutex.lock();
191  {
192  if(!_dataBuffer.empty())
193  {
194  if(!_imuBuffer.empty())
195  {
196  // Send IMU up to stamp greater than image (OpenVINS needs this).
197  while(!_imuBuffer.empty())
198  {
199  _odometry->process(_imuBuffer.front());
200  double stamp = _imuBuffer.front().stamp();
201  _imuBuffer.pop_front();
202  if(stamp > _dataBuffer.front().stamp())
203  {
204  break;
205  }
206  }
207  }
208 
209  data = _dataBuffer.front();
210  _dataBuffer.pop_front();
211  dataFilled = true;
212  }
213  }
214  _dataMutex.unlock();
215  return dataFilled;
216 }
217 
218 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::OdometryEvent
Definition: OdometryEvent.h:39
rtabmap::OdometryResetEvent::getPose
const Transform & getPose() const
Definition: OdometryEvent.h:103
UMutex::lock
int lock() const
Definition: UMutex.h:87
USemaphore::acquire
bool acquire(int n=1, int ms=0)
Definition: USemaphore.h:101
rtabmap::OdometryThread::getData
bool getData(SensorData &data)
Definition: OdometryThread.cpp:186
rtabmap::IMU::empty
bool empty() const
Definition: IMU.h:67
OdometryInfo.h
UMutex::unlock
int unlock() const
Definition: UMutex.h:113
OdometryThread.h
rtabmap::OdometryThread::_imuEstimatedDelay
double _imuEstimatedDelay
Definition: OdometryThread.h:71
UThread::join
void join(bool killFirst=false)
Definition: UThread.cpp:85
rtabmap::IMUEvent
Definition: IMU.h:85
rtabmap::OdometryThread::addData
void addData(const SensorData &data)
Definition: OdometryThread.cpp:131
rtabmap::OdometryThread::_lastImuStamp
double _lastImuStamp
Definition: OdometryThread.h:70
UEvent::getCode
int getCode() const
Definition: UEvent.h:74
rtabmap::OdometryThread::_resetPose
Transform _resetPose
Definition: OdometryThread.h:69
rtabmap::IMUEvent::getStamp
double getStamp() const
Definition: IMU.h:98
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::OdometryThread::_dataBufferMaxSize
unsigned int _dataBufferMaxSize
Definition: OdometryThread.h:67
UEvent
Definition: UEvent.h:57
rtabmap::OdometryThread::mainLoop
virtual void mainLoop()
Definition: OdometryThread.cpp:104
odometry
Pose2 odometry(2.0, 0.0, 0.0)
Odometry.h
rtabmap::OdometryThread::OdometryThread
OdometryThread(Odometry *odometry, unsigned int dataBufferMaxSize=1)
Definition: OdometryThread.cpp:38
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
data
int data[]
UScopeMutex
Definition: UMutex.h:157
rtabmap::OdometryThread::_resetOdometry
bool _resetOdometry
Definition: OdometryThread.h:68
UEvent::getClassName
virtual std::string getClassName() const =0
rtabmap::OdometryThread::mainLoopKill
virtual void mainLoopKill()
Definition: OdometryThread.cpp:96
OdometryMono.h
rtabmap::Odometry::process
Transform process(SensorData &data, OdometryInfo *info=0)
Definition: Odometry.cpp:298
rtabmap::OdometryThread::_odometry
Odometry * _odometry
Definition: OdometryThread.h:66
rtabmap::SensorEvent::kCodeData
@ kCodeData
Definition: SensorEvent.h:42
rtabmap::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:210
SensorEvent.h
UEventsHandler::unregisterFromEventsManager
void unregisterFromEventsManager()
Definition: UEventsHandler.cpp:33
info
else if n * info
UASSERT
#define UASSERT(condition)
rtabmap::OdometryThread::_dataMutex
UMutex _dataMutex
Definition: OdometryThread.h:63
rtabmap::Transform::setIdentity
void setIdentity()
Definition: Transform.cpp:157
OdometryEvent.h
rtabmap::OdometryThread::mainLoopBegin
virtual void mainLoopBegin()
Definition: OdometryThread.cpp:91
rtabmap::OdometryThread::~OdometryThread
virtual ~OdometryThread()
Definition: OdometryThread.cpp:49
ULogger::registerCurrentThread
static void registerCurrentThread(const std::string &name)
Definition: ULogger.cpp:218
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
USemaphore::release
void release(int n=1)
Definition: USemaphore.h:168
rtabmap::SensorEvent::data
const SensorData & data() const
Definition: SensorEvent.h:79
ULOGGER_ERROR
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
rtabmap::OdometryThread::_dataBuffer
std::list< SensorData > _dataBuffer
Definition: OdometryThread.h:64
UDEBUG
#define UDEBUG(...)
rtabmap::Odometry
Definition: Odometry.h:42
rtabmap::OdometryResetEvent
Definition: OdometryEvent.h:97
rtabmap::OdometryThread::_imuBuffer
std::list< SensorData > _imuBuffer
Definition: OdometryThread.h:65
rtabmap::OdometryInfo
Definition: OdometryInfo.h:40
UThread::isRunning
bool isRunning() const
Definition: UThread.cpp:245
rtabmap::IMUEvent::getData
const IMU & getData() const
Definition: IMU.h:97
false
#define false
Definition: ConvertUTF.c:56
rtabmap::OdometryMono
Definition: OdometryMono.h:39
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::OdometryThread::handleEvent
virtual bool handleEvent(UEvent *event)
Definition: OdometryThread.cpp:57
rtabmap::SensorEvent
Definition: SensorEvent.h:37
rtabmap::OdometryThread::_dataAdded
USemaphore _dataAdded
Definition: OdometryThread.h:62
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:13