OdometryLOAM.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/OdometryLOAM.h"
29 #include "rtabmap/core/OdometryInfo.h"
30 #include "rtabmap/core/util2d.h"
31 #include "rtabmap/utilite/ULogger.h"
32 #include "rtabmap/utilite/UTimer.h"
33 #include "rtabmap/utilite/UStl.h"
34 #include "rtabmap/core/util3d.h"
35 #include <pcl/common/transforms.h>
36 
37 float SCAN_PERIOD = 0.1f;
38 
39 namespace rtabmap {
40 
45 OdometryLOAM::OdometryLOAM(const ParametersMap & parameters) :
46  Odometry(parameters)
47 #ifdef RTABMAP_LOAM
48  ,lastPose_(Transform::getIdentity())
49  ,scanPeriod_(Parameters::defaultOdomLOAMScanPeriod())
50  ,linVar_(Parameters::defaultOdomLOAMLinVar())
51  ,angVar_(Parameters::defaultOdomLOAMAngVar())
52  ,localMapping_(Parameters::defaultOdomLOAMLocalMapping())
53  ,lost_(false)
54 #endif
55 {
56 #ifdef RTABMAP_LOAM
57  int velodyneType = 0;
58  Parameters::parse(parameters, Parameters::kOdomLOAMSensor(), velodyneType);
59  Parameters::parse(parameters, Parameters::kOdomLOAMScanPeriod(), scanPeriod_);
60  UASSERT(scanPeriod_>0.0f);
61  Parameters::parse(parameters, Parameters::kOdomLOAMLinVar(), linVar_);
62  UASSERT(linVar_>0.0f);
63  Parameters::parse(parameters, Parameters::kOdomLOAMAngVar(), angVar_);
64  UASSERT(angVar_>0.0f);
65  Parameters::parse(parameters, Parameters::kOdomLOAMLocalMapping(), localMapping_);
66  if(velodyneType == 1)
67  {
68  scanMapper_ = loam::MultiScanMapper::Velodyne_HDL_32();
69  }
70  else if(velodyneType == 2)
71  {
72  scanMapper_ = loam::MultiScanMapper::Velodyne_HDL_64E();
73  }
74  else
75  {
76  scanMapper_ = loam::MultiScanMapper::Velodyne_VLP_16();
77  }
78  laserOdometry_ = new loam::BasicLaserOdometry(scanPeriod_);
79  laserMapping_ = new loam::BasicLaserMapping(scanPeriod_);
80 #endif
81 }
82 
83 OdometryLOAM::~OdometryLOAM()
84 {
85 #ifdef RTABMAP_LOAM
86  delete laserOdometry_;
87  delete laserMapping_;
88 #endif
89 }
90 
91 void OdometryLOAM::reset(const Transform & initialPose)
92 {
93  Odometry::reset(initialPose);
94 #ifdef RTABMAP_LOAM
95  lastPose_.setIdentity();
96  scanRegistration_ = loam::BasicScanRegistration();
97  loam::RegistrationParams regParams;
98  regParams.scanPeriod = scanPeriod_;
99  scanRegistration_.configure(regParams);
100  delete laserOdometry_;
101  laserOdometry_ = new loam::BasicLaserOdometry(scanPeriod_);
102  delete laserMapping_;
103  laserMapping_ = new loam::BasicLaserMapping(scanPeriod_);
104  transformMaintenance_ = loam::BasicTransformMaintenance();
105  lost_ = false;
106 #endif
107 }
108 
109 #ifdef RTABMAP_LOAM
110 std::vector<pcl::PointCloud<pcl::PointXYZI> > OdometryLOAM::segmentScanRings(const pcl::PointCloud<pcl::PointXYZ> & laserCloudIn)
111 {
112  std::vector<pcl::PointCloud<pcl::PointXYZI> > laserCloudScans;
113 
114  size_t cloudSize = laserCloudIn.size();
115 
116  // determine scan start and end orientations
117  float startOri = -std::atan2(laserCloudIn[0].y, laserCloudIn[0].x);
118  float endOri = -std::atan2(laserCloudIn[cloudSize - 1].y,
119  laserCloudIn[cloudSize - 1].x) + 2 * float(M_PI);
120  if (endOri - startOri > 3 * M_PI) {
121  endOri -= 2 * M_PI;
122  } else if (endOri - startOri < M_PI) {
123  endOri += 2 * M_PI;
124  }
125 
126  bool halfPassed = false;
127  pcl::PointXYZI point;
128  laserCloudScans.resize(scanMapper_.getNumberOfScanRings());
129  // clear all scanline points
130  std::for_each(laserCloudScans.begin(), laserCloudScans.end(), [](auto&&v) {v.clear(); });
131 
132  // extract valid points from input cloud
133  for (size_t i = 0; i < cloudSize; i++) {
134  point.x = laserCloudIn[i].y;
135  point.y = laserCloudIn[i].z;
136  point.z = laserCloudIn[i].x;
137 
138  // skip NaN and INF valued points
139  if (!pcl_isfinite(point.x) ||
140  !pcl_isfinite(point.y) ||
141  !pcl_isfinite(point.z)) {
142  continue;
143  }
144 
145  // skip zero valued points
146  if (point.x * point.x + point.y * point.y + point.z * point.z < 0.0001) {
147  continue;
148  }
149 
150  // calculate vertical point angle and scan ID
151  float angle = std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z));
152  int scanID = scanMapper_.getRingForAngle(angle);
153  if (scanID >= scanMapper_.getNumberOfScanRings() || scanID < 0 ){
154  continue;
155  }
156 
157  // calculate horizontal point angle
158  float ori = -std::atan2(point.x, point.z);
159  if (!halfPassed) {
160  if (ori < startOri - M_PI / 2) {
161  ori += 2 * M_PI;
162  } else if (ori > startOri + M_PI * 3 / 2) {
163  ori -= 2 * M_PI;
164  }
165 
166  if (ori - startOri > M_PI) {
167  halfPassed = true;
168  }
169  } else {
170  ori += 2 * M_PI;
171 
172  if (ori < endOri - M_PI * 3 / 2) {
173  ori += 2 * M_PI;
174  } else if (ori > endOri + M_PI / 2) {
175  ori -= 2 * M_PI;
176  }
177  }
178 
179  // calculate relative scan time based on point orientation
180  float relTime = SCAN_PERIOD * (ori - startOri) / (endOri - startOri);
181  point.intensity = scanID + relTime;
182 
183  // imu not used...
184  //scanRegistration_.projectPointToStartOfSweep(point, relTime);
185 
186  laserCloudScans[scanID].push_back(point);
187  }
188 
189  return laserCloudScans;
190 }
191 #endif
192 
193 // return not null transform if odometry is correctly computed
194 Transform OdometryLOAM::computeTransform(
195  SensorData & data,
196  const Transform & guess,
197  OdometryInfo * info)
198 {
199  Transform t;
200 #ifdef RTABMAP_LOAM
201  UTimer timer;
202 
203  if(data.laserScanRaw().isEmpty())
204  {
205  UERROR("LOAM works only with laser scans and the current input is empty. Aborting odometry update...");
206  return t;
207  }
208  else if(data.laserScanRaw().is2d())
209  {
210  UERROR("LOAM version used works only with 3D laser scans from Velodyne. Aborting odometry update...");
211  return t;
212  }
213 
214  cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1)*9999;
215  if(!lost_)
216  {
217  pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloudInPtr = util3d::laserScanToPointCloud(data.laserScanRaw());
218  std::vector<pcl::PointCloud<pcl::PointXYZI> > laserCloudScans = segmentScanRings(*laserCloudInPtr);
219 
220  ros::Time stampT;
221  stampT.fromSec(data.stamp());
222  loam::Time scanTime = loam::fromROSTime(stampT);
223  scanRegistration_.processScanlines(scanTime, laserCloudScans);
224 
225  *laserOdometry_->cornerPointsSharp() = scanRegistration_.cornerPointsSharp();
226  *laserOdometry_->cornerPointsLessSharp() = scanRegistration_.cornerPointsLessSharp();
227  *laserOdometry_->surfPointsFlat() = scanRegistration_.surfacePointsFlat();
228  *laserOdometry_->surfPointsLessFlat() = scanRegistration_.surfacePointsLessFlat();
229  *laserOdometry_->laserCloud() = scanRegistration_.laserCloud();
230  pcl::PointCloud<pcl::PointXYZ> imuTrans;
231  imuTrans.resize(4);
232  laserOdometry_->updateIMU(imuTrans);
233  laserOdometry_->process();
234 
235  if(localMapping_)
236  {
237  laserMapping_->laserCloudCornerLast() = *laserOdometry_->lastCornerCloud();
238  laserMapping_->laserCloudSurfLast() = *laserOdometry_->lastSurfaceCloud();
239  laserMapping_->laserCloud() = *laserOdometry_->laserCloud();
240  laserMapping_->updateOdometry(laserOdometry_->transformSum());
241  laserMapping_->process(scanTime);
242  }
243 
244  transformMaintenance_.updateOdometry(
245  laserOdometry_->transformSum().rot_x.rad(),
246  laserOdometry_->transformSum().rot_y.rad(),
247  laserOdometry_->transformSum().rot_z.rad(),
248  laserOdometry_->transformSum().pos.x(),
249  laserOdometry_->transformSum().pos.y(),
250  laserOdometry_->transformSum().pos.z());
251  transformMaintenance_.updateMappingTransform(laserMapping_->transformAftMapped(), laserMapping_->transformBefMapped());
252  transformMaintenance_.transformAssociateToMap();
253  const float * tm = transformMaintenance_.transformMapped();
254  Transform pose = Transform(tm[5], tm[3], tm[4], tm[2], tm[0], tm[1]);
255 
256  if(!pose.isNull())
257  {
258  covariance = cv::Mat::eye(6,6,CV_64FC1);
259  covariance(cv::Range(0,3), cv::Range(0,3)) *= linVar_;
260  covariance(cv::Range(3,6), cv::Range(3,6)) *= angVar_;
261 
262  t = lastPose_.inverse() * pose; // incremental
263  lastPose_ = pose;
264 
265  const Transform & localTransform = data.laserScanRaw().localTransform();
266  if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
267  {
268  // from laser frame to base frame
269  t = localTransform * t * localTransform.inverse();
270  }
271 
272  if(info)
273  {
274  info->type = (int)kTypeLOAM;
275  info->localScanMapSize = laserMapping_->laserCloudSurroundDS().size();
276  if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
277  {
278  info->reg.covariance = covariance;
279  }
280 
281  if(this->isInfoDataFilled())
282  {
283  Transform rot(0,0,1,0,1,0,0,0,0,1,0,0);
284  pcl::PointCloud<pcl::PointXYZI> out;
285  pcl::transformPointCloud(laserMapping_->laserCloudSurroundDS(), out, rot.toEigen3f());
286  info->localScanMap = LaserScan::backwardCompatibility(util3d::laserScanFromPointCloud(out), 0, data.laserScanRaw().maxRange(), data.laserScanRaw().localTransform());
287  }
288  }
289  }
290  else
291  {
292  lost_ = true;
293  UWARN("LOAM failed to register the latest scan, odometry should be reset.");
294  }
295  }
296  UINFO("Odom update time = %fs, lost=%s", timer.elapsed(), lost_?"true":"false");
297 
298 #else
299  UERROR("RTAB-Map is not built with LOAM support! Select another odometry approach.");
300 #endif
301  return t;
302 }
303 
304 } // namespace rtabmap
SCAN_PERIOD
float SCAN_PERIOD
Definition: OdometryLOAM.cpp:37
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true)
Definition: util3d.cpp:1266
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:446
UTimer
Definition: UTimer.h:46
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:89
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
f
f
rtabmap::Odometry::isInfoDataFilled
bool isInfoDataFilled() const
Definition: Odometry.h:72
rtabmap::LaserScan::maxRange
float maxRange() const
Definition: LaserScan.h:65
rtabmap::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:182
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:69
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:72
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:163
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2328
TimeBase< Time, Duration >::fromSec
Time & fromSec(double t)
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
glm::atan
GLM_FUNC_DECL genType atan(genType const &y, genType const &x)
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:67
glm::atan2
GLM_FUNC_QUALIFIER T atan2(T x, T y)
Arc tangent. Returns an angle whose tangent is y/x. The signs of x and y are used to determine what q...
Definition: compatibility.hpp:79
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:154
false
#define false
Definition: ConvertUTF.c:56
rtabmap::OdometryLOAM::computeTransform
virtual Transform computeTransform(SensorData &image, const Transform &guess=Transform(), OdometryInfo *info=0)
Definition: OdometryLOAM.cpp:194
rtabmap::OdometryLOAM::OdometryLOAM
OdometryLOAM(const rtabmap::ParametersMap &parameters=rtabmap::ParametersMap())
Definition: OdometryLOAM.cpp:45
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::OdometryLOAM::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: OdometryLOAM.cpp:91
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:91
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:169
rtabmap::Transform::toEigen3f
Eigen::Affine3f toEigen3f() const
Definition: Transform.cpp:344
rtabmap::LaserScan::backwardCompatibility
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:84
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:32