35 #include <pcl/common/transforms.h> 49 ,scanPeriod_(
Parameters::defaultOdomLOAMScanPeriod())
52 ,localMapping_(
Parameters::defaultOdomLOAMLocalMapping())
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_);
86 delete laserOdometry_;
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();
110 std::vector<pcl::PointCloud<pcl::PointXYZI> > OdometryLOAM::segmentScanRings(
const pcl::PointCloud<pcl::PointXYZ> & laserCloudIn)
112 std::vector<pcl::PointCloud<pcl::PointXYZI> > laserCloudScans;
114 size_t cloudSize = laserCloudIn.size();
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) {
122 }
else if (endOri - startOri <
M_PI) {
126 bool halfPassed =
false;
127 pcl::PointXYZI point;
128 laserCloudScans.resize(scanMapper_.getNumberOfScanRings());
130 std::for_each(laserCloudScans.begin(), laserCloudScans.end(), [](
auto&&v) {v.clear(); });
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;
139 if (!pcl_isfinite(point.x) ||
140 !pcl_isfinite(point.y) ||
141 !pcl_isfinite(point.z)) {
146 if (point.x * point.x + point.y * point.y + point.z * point.z < 0.0001) {
152 int scanID = scanMapper_.getRingForAngle(angle);
153 if (scanID >= scanMapper_.getNumberOfScanRings() || scanID < 0 ){
160 if (ori < startOri -
M_PI / 2) {
162 }
else if (ori > startOri +
M_PI * 3 / 2) {
166 if (ori - startOri >
M_PI) {
172 if (ori < endOri -
M_PI * 3 / 2) {
174 }
else if (ori > endOri +
M_PI / 2) {
180 float relTime =
SCAN_PERIOD * (ori - startOri) / (endOri - startOri);
181 point.intensity = scanID + relTime;
186 laserCloudScans[scanID].push_back(point);
189 return laserCloudScans;
205 UERROR(
"LOAM works only with laser scans and the current input is empty. Aborting odometry update...");
210 UERROR(
"LOAM version used works only with 3D laser scans from Velodyne. Aborting odometry update...");
214 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1)*9999;
218 std::vector<pcl::PointCloud<pcl::PointXYZI> > laserCloudScans = segmentScanRings(*laserCloudInPtr);
222 loam::Time scanTime = loam::fromROSTime(stampT);
223 scanRegistration_.processScanlines(scanTime, laserCloudScans);
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;
232 laserOdometry_->updateIMU(imuTrans);
233 laserOdometry_->process();
237 laserMapping_->laserCloudCornerLast() = *laserOdometry_->lastCornerCloud();
238 laserMapping_->laserCloudSurfLast() = *laserOdometry_->lastSurfaceCloud();
239 laserMapping_->laserCloud() = *laserOdometry_->laserCloud();
240 laserMapping_->updateOdometry(laserOdometry_->transformSum());
241 laserMapping_->process(scanTime);
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();
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_;
262 t = lastPose_.
inverse() * pose;
269 t = localTransform * t * localTransform.
inverse();
276 if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
283 Transform rot(0,0,1,0,1,0,0,0,0,1,0,0);
284 pcl::PointCloud<pcl::PointXYZI> out;
293 UWARN(
"LOAM failed to register the latest scan, odometry should be reset.");
296 UINFO(
"Odom update time = %fs, lost=%s", timer.
elapsed(), lost_?
"true":
"false");
299 UERROR(
"RTAB-Map is not built with LOAM support! Select another odometry approach.");
static bool parse(const ParametersMap ¶meters, const std::string &key, bool &value)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
bool isInfoDataFilled() const
virtual void reset(const Transform &initialPose=Transform::getIdentity())
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
std::map< std::string, std::string > ParametersMap
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
const LaserScan & laserScanRaw() const
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
#define UASSERT(condition)
Wrappers of STL for convenient functions.
GLM_FUNC_DECL genType atan(genType const &y, genType const &x)
Transform localTransform() const
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...
virtual Transform computeTransform(SensorData &image, const Transform &guess=Transform(), OdometryInfo *info=0)
OdometryLOAM(const rtabmap::ParametersMap ¶meters=rtabmap::ParametersMap())
ULogger class and convenient macros.
virtual void reset(const Transform &initialPose=Transform::getIdentity())
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true, bool is2D=false, const Transform &transform=Transform())