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


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