35 #include <pcl/common/transforms.h>
47 ,scanPeriod_(
Parameters::defaultOdomLOAMScanPeriod())
50 ,localMapping_(
Parameters::defaultOdomLOAMLocalMapping())
55 int velodyneType = Parameters::defaultOdomLOAMSensor();
56 float mapResolution = Parameters::defaultOdomLOAMResolution();
65 Parameters::parse(parameters, Parameters::kOdomLOAMLocalMapping(), localMapping_);
68 scanMapper_ = loam::MultiScanMapper::Velodyne_HDL_32();
70 else if(velodyneType == 2)
72 scanMapper_ = loam::MultiScanMapper::Velodyne_HDL_64E();
76 scanMapper_ = loam::MultiScanMapper::Velodyne_VLP_16();
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.0
f, mapResolution*2.0
f, mapResolution*2.0
f);
88 delete laserOdometry_;
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();
112 std::vector<pcl::PointCloud<pcl::PointXYZI> > OdometryLOAM::segmentScanRings(
const pcl::PointCloud<pcl::PointXYZ> & laserCloudIn)
114 std::vector<pcl::PointCloud<pcl::PointXYZI> > laserCloudScans;
116 size_t cloudSize = laserCloudIn.size();
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) {
124 }
else if (endOri - startOri <
M_PI) {
128 bool halfPassed =
false;
129 pcl::PointXYZI
point;
130 laserCloudScans.resize(scanMapper_.getNumberOfScanRings());
132 std::for_each(laserCloudScans.begin(), laserCloudScans.end(), [](
auto&&v) {v.clear(); });
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;
141 if (!pcl_isfinite(
point.x) ||
142 !pcl_isfinite(
point.y) ||
143 !pcl_isfinite(
point.z)) {
154 int scanID = scanMapper_.getRingForAngle(
angle);
155 if (scanID >= scanMapper_.getNumberOfScanRings() || scanID < 0 ){
162 if (ori < startOri -
M_PI / 2) {
164 }
else if (ori > startOri +
M_PI * 3 / 2) {
168 if (ori - startOri >
M_PI) {
174 if (ori < endOri -
M_PI * 3 / 2) {
176 }
else if (ori > endOri +
M_PI / 2) {
182 float relTime = scanPeriod_ * (ori - startOri) / (endOri - startOri);
183 point.intensity = scanID + relTime;
188 laserCloudScans[scanID].push_back(point);
191 return laserCloudScans;
205 if(
data.laserScanRaw().isEmpty())
207 UERROR(
"LOAM works only with laser scans and the current input is empty. Aborting odometry update...");
210 else if(
data.laserScanRaw().is2d())
212 UERROR(
"LOAM version used works only with 3D laser scans from Velodyne. Aborting odometry update...");
216 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1)*9999;
220 std::vector<pcl::PointCloud<pcl::PointXYZI> > laserCloudScans = segmentScanRings(*laserCloudInPtr);
224 loam::Time scanTime = loam::fromROSTime(stampT);
225 scanRegistration_.processScanlines(scanTime, laserCloudScans);
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;
234 laserOdometry_->updateIMU(imuTrans);
235 laserOdometry_->process();
239 laserMapping_->laserCloudCornerLast() = *laserOdometry_->lastCornerCloud();
240 laserMapping_->laserCloudSurfLast() = *laserOdometry_->lastSurfaceCloud();
241 laserMapping_->laserCloud() = *laserOdometry_->laserCloud();
242 laserMapping_->updateOdometry(laserOdometry_->transformSum());
243 laserMapping_->process(scanTime);
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();
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_;
264 t = lastPose_.inverse() * pose;
267 const Transform & localTransform =
data.laserScanRaw().localTransform();
268 if(!
t.isNull() && !
t.isIdentity() && !localTransform.
isIdentity() && !localTransform.
isNull())
271 t = localTransform *
t * localTransform.
inverse();
277 info->localScanMapSize = laserMapping_->laserCloudSurroundDS().size();
278 if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
280 info->reg.covariance = covariance;
285 Transform rot(0,0,1,0,1,0,0,0,0,1,0,0);
286 pcl::PointCloud<pcl::PointXYZI>
out;
295 UWARN(
"LOAM failed to register the latest scan, odometry should be reset.");
301 UERROR(
"RTAB-Map is not built with LOAM support! Select another odometry approach.");