IMUThread.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 "rtabmap/core/IMUThread.h"
29 #include "rtabmap/core/IMU.h"
30 #include "rtabmap/core/IMUFilter.h"
31 #include <rtabmap/utilite/UTimer.h>
34 
35 namespace rtabmap
36 {
37 
38 IMUThread::IMUThread(int rate, const Transform & localTransform) :
39  rate_(rate),
40  localTransform_(localTransform),
41  captureDelay_(0.0),
42  previousStamp_(0.0),
43  _imuFilter(0),
44  _imuBaseFrameConversion(false)
45 {
46 }
47 
49 {
50  imuFile_.close();
51  delete _imuFilter;
52 }
53 
54 bool IMUThread::init(const std::string & path)
55 {
56  imuFile_.close();
57  captureDelay_ = 0.0;
58  previousStamp_ = 0.0;
59 
60  // open the IMU file
61  std::string line;
62  imuFile_.open(path.c_str());
63  if (!imuFile_.good()) {
64  UERROR("no imu file found at %s",path.c_str());
65  return false;
66  }
67  int number_of_lines = 0;
68  while (std::getline(imuFile_, line))
69  ++number_of_lines;
70  printf("No. IMU measurements: %d\n", number_of_lines-1);
71  if (number_of_lines - 1 <= 0) {
72  UERROR("no imu messages present in %s", path.c_str());
73  return false;
74  }
75  // set reading position to second line
76  imuFile_.clear();
77  imuFile_.seekg(0, std::ios::beg);
78  std::getline(imuFile_, line);
79 
80  return true;
81 }
82 
83 void IMUThread::setRate(int rate)
84 {
85  rate_ = rate;
86 }
87 
88 void IMUThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion)
89 {
90  delete _imuFilter;
91  _imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters);
92  _imuBaseFrameConversion = baseFrameConversion;
93 }
94 
96 {
97  delete _imuFilter;
98  _imuFilter = 0;
99 }
100 
102 {
105 }
106 
108 {
109  UTimer totalTime;
110  UDEBUG("");
111 
112  if(rate_>0 || captureDelay_)
113  {
114  double delay = rate_>0?1000.0/double(rate_):1000.0f*captureDelay_;
115  int sleepTime = delay - 1000.0f*frameRateTimer_.getElapsedTime();
116  if(sleepTime > 2)
117  {
118  uSleep(sleepTime-2);
119  }
120 
121  // Add precision at the cost of a small overhead
122  delay/=1000.0;
123  while(frameRateTimer_.getElapsedTime() < delay-0.000001)
124  {
125  //
126  }
127 
129  }
130  captureDelay_ = 0.0;
131 
132  std::string line;
133  if (std::getline(imuFile_, line))
134  {
135  std::stringstream stream(line);
136  std::string s;
137  std::getline(stream, s, ',');
138  std::string nanoseconds = s.substr(s.size() - 9, 9);
139  std::string seconds = s.substr(0, s.size() - 9);
140 
141  cv::Vec3d gyr;
142  for (int j = 0; j < 3; ++j) {
143  std::getline(stream, s, ',');
144  gyr[j] = uStr2Double(s);
145  }
146 
147  cv::Vec3d acc;
148  for (int j = 0; j < 3; ++j) {
149  std::getline(stream, s, ',');
150  acc[j] = uStr2Double(s);
151  }
152 
153  double stamp = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9;
154  if(previousStamp_>0 && stamp > previousStamp_)
155  {
156  captureDelay_ = stamp - previousStamp_;
157  }
158  previousStamp_ = stamp;
159 
160  IMU imu(gyr, cv::Mat(), acc, cv::Mat(), localTransform_);
161 
162  // IMU filtering
163  if(_imuFilter && !imu.empty())
164  {
165  if(imu.angularVelocity()[0] == 0 &&
166  imu.angularVelocity()[1] == 0 &&
167  imu.angularVelocity()[2] == 0 &&
168  imu.linearAcceleration()[0] == 0 &&
169  imu.linearAcceleration()[1] == 0 &&
170  imu.linearAcceleration()[2] == 0)
171  {
172  UWARN("IMU's acc and gyr values are null! Please disable IMU filtering.");
173  }
174  else
175  {
176  // Transform IMU data in base_link to correctly initialize yaw
178  {
179  UASSERT(!imu.localTransform().isNull());
180  imu.convertToBaseFrame();
181 
182  }
184  imu.angularVelocity()[0],
185  imu.angularVelocity()[1],
186  imu.angularVelocity()[2],
187  imu.linearAcceleration()[0],
188  imu.linearAcceleration()[1],
189  imu.linearAcceleration()[2],
190  stamp);
191  double qx,qy,qz,qw;
192  _imuFilter->getOrientation(qx,qy,qz,qw);
193 
194  imu = IMU(
195  cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1),
198  imu.localTransform());
199 
200  UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)",
201  imu.orientation()[0],
202  imu.orientation()[1],
203  imu.orientation()[2],
204  imu.orientation()[3],
205  imu.angularVelocity()[0],
206  imu.angularVelocity()[1],
207  imu.angularVelocity()[2],
208  imu.linearAcceleration()[0],
209  imu.linearAcceleration()[1],
210  imu.linearAcceleration()[2],
211  stamp);
212  }
213  }
214 
215  this->post(new IMUEvent(imu, stamp));
216  }
217  else if(!this->isKilled())
218  {
219  UWARN("no more imu data...");
220  this->kill();
221  this->post(new IMUEvent());
222  }
223 }
224 
225 } // namespace rtabmap
rtabmap::IMUThread::localTransform_
Transform localTransform_
Definition: IMUThread.h:68
rtabmap::IMU::linearAcceleration
const cv::Vec3d linearAcceleration() const
Definition: IMU.h:59
rtabmap::IMU::empty
bool empty() const
Definition: IMU.h:67
rtabmap::IMUThread::init
bool init(const std::string &path)
Definition: IMUThread.cpp:54
s
RealScalar s
IMUThread.h
stream
stream
rtabmap::IMUEvent
Definition: IMU.h:85
rtabmap::IMU::angularVelocityCovariance
const cv::Mat & angularVelocityCovariance() const
Definition: IMU.h:57
rtabmap::IMUThread::mainLoop
virtual void mainLoop()
Definition: IMUThread.cpp:107
rtabmap::IMUThread::~IMUThread
virtual ~IMUThread()
Definition: IMUThread.cpp:48
UTimer::start
void start()
Definition: UTimer.cpp:87
uStr2Double
double UTILITE_EXPORT uStr2Double(const std::string &str)
Definition: UConversion.cpp:147
rtabmap::IMU::localTransform
const Transform & localTransform() const
Definition: IMU.h:62
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UTimer.h
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::IMUThread::previousStamp_
double previousStamp_
Definition: IMUThread.h:72
rtabmap::IMUFilter::create
static IMUFilter * create(const ParametersMap &parameters=ParametersMap())
Definition: IMUFilter.cpp:38
rtabmap::IMUFilter::update
void update(double gx, double gy, double gz, double ax, double ay, double az, double stamp)
Definition: IMUFilter.cpp:72
rtabmap::IMUFilter::getOrientation
virtual void getOrientation(double &qx, double &qy, double &qz, double &qw) const =0
rtabmap::IMUFilter::Type
Type
Definition: IMUFilter.h:40
rtabmap::IMU::linearAccelerationCovariance
const cv::Mat & linearAccelerationCovariance() const
Definition: IMU.h:60
rtabmap::IMU
Definition: IMU.h:19
UThread::isKilled
bool isKilled() const
Definition: UThread.cpp:255
j
std::ptrdiff_t j
UConversion.h
Some conversion functions.
UThread::kill
void kill()
Definition: UThread.cpp:48
rtabmap::IMUThread::rate_
int rate_
Definition: IMUThread.h:67
rtabmap::IMU::orientation
const cv::Vec4d & orientation() const
Definition: IMU.h:53
UASSERT
#define UASSERT(condition)
rtabmap::IMUThread::enableIMUFiltering
void enableIMUFiltering(int filteringStrategy=1, const ParametersMap &parameters=ParametersMap(), bool baseFrameConversion=false)
Definition: IMUThread.cpp:88
rtabmap::IMUThread::imuFile_
std::ifstream imuFile_
Definition: IMUThread.h:69
rtabmap::IMUThread::_imuBaseFrameConversion
bool _imuBaseFrameConversion
Definition: IMUThread.h:74
path
path
UWARN
#define UWARN(...)
ULogger::registerCurrentThread
static void registerCurrentThread(const std::string &name)
Definition: ULogger.cpp:218
qz
RealQZ< MatrixXf > qz(4)
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
rtabmap::IMUThread::captureDelay_
double captureDelay_
Definition: IMUThread.h:71
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
rtabmap::IMUThread::setRate
void setRate(int rate)
Definition: IMUThread.cpp:83
uStr2Int
int UTILITE_EXPORT uStr2Int(const std::string &str)
Definition: UConversion.cpp:125
IMUFilter.h
rtabmap::IMU::convertToBaseFrame
void convertToBaseFrame()
Definition: IMU.cpp:33
rtabmap::IMUThread::disableIMUFiltering
void disableIMUFiltering()
Definition: IMUThread.cpp:95
UDEBUG
#define UDEBUG(...)
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
UTimer
Definition: UTimer.h:46
rtabmap::IMUThread::frameRateTimer_
UTimer frameRateTimer_
Definition: IMUThread.h:70
rtabmap::IMUThread::mainLoopBegin
virtual void mainLoopBegin()
Definition: IMUThread.cpp:101
rtabmap::IMU::angularVelocity
const cv::Vec3d & angularVelocity() const
Definition: IMU.h:56
IMU.h
false
#define false
Definition: ConvertUTF.c:56
UTimer::getElapsedTime
double getElapsedTime()
Definition: UTimer.cpp:97
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
rtabmap::IMUThread::IMUThread
IMUThread(int rate, const Transform &localTransform)
Definition: IMUThread.cpp:38
rtabmap::IMUThread::_imuFilter
IMUFilter * _imuFilter
Definition: IMUThread.h:73
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:11