diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index d378979f..64baec3d 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -68,6 +68,7 @@ public: bool isInfoDataFilled() const {return _fillInfoData;} const Transform & previousVelocityTransform() const {return previousVelocityTransform_;} double previousStamp() const {return previousStamp_;} + unsigned int framesProcessed() const {return framesProcessed_;} private: virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0; @@ -98,6 +99,7 @@ private: Transform previousVelocityTransform_; Transform previousGroundTruthPose_; float distanceTravelled_; + unsigned int framesProcessed_; std::vector particleFilters_; cv::KalmanFilter kalmanFilter_; diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index 8098389d..dd6709ff 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -102,7 +102,8 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : _pose(Transform::getIdentity()), _resetCurrentCount(0), previousStamp_(0), - distanceTravelled_(0) + distanceTravelled_(0), + framesProcessed_(0) { Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown); @@ -168,6 +169,7 @@ void Odometry::reset(const Transform & initialPose) _resetCurrentCount = 0; previousStamp_ = 0; distanceTravelled_ = 0; + framesProcessed_ = 0; if(_force3DoF || particleFilters_.size()) { float x,y,z, roll,pitch,yaw; @@ -544,6 +546,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet distanceTravelled_ += t.getNorm(); info->distanceTravelled = distanceTravelled_; } + ++framesProcessed_; return _pose *= t; // update } diff --git a/corelib/src/OdometryF2F.cpp b/corelib/src/OdometryF2F.cpp index 51520518..73a0fea5 100644 --- a/corelib/src/OdometryF2F.cpp +++ b/corelib/src/OdometryF2F.cpp @@ -83,6 +83,7 @@ Transform OdometryF2F::computeTransform( return output; } + bool addKeyFrame = false; RegistrationInfo regInfo; UASSERT(!this->getPose().isNull()); @@ -99,8 +100,8 @@ Transform OdometryF2F::computeTransform( output = registrationPipeline_->computeTransformationMod( tmpRefFrame, newFrame, - // special case for ICP-only odom, set guess to identity if we just started - !guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(), + // special case for ICP-only odom, set guess to identity if we just started or reset + !guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->framesProcessed()<2?motionSinceLastKeyFrame:Transform(), ®Info); if(output.isNull() && !guess.isNull() && registrationPipeline_->isImageRequired()) @@ -185,7 +186,7 @@ Transform OdometryF2F::computeTransform( { UDEBUG("Update key frame"); int features = newFrame.getWordsDescriptors().size(); - if(features == 0) + if(registrationPipeline_->isImageRequired() && features == 0) { newFrame = Signature(data); // this will generate features only for the first frame or if optical flow was used (no 3d words) @@ -209,6 +210,8 @@ Transform OdometryF2F::computeTransform( //reset motion lastKeyFramePose_.setNull(); + + addKeyFrame = true; } else { @@ -249,6 +252,7 @@ Transform OdometryF2F::computeTransform( info->icpInliersRatio = regInfo.icpInliersRatio; info->matches = regInfo.matches; info->features = newFrame.sensorData().keypoints().size(); + info->keyFrameAdded = addKeyFrame; } UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s", diff --git a/corelib/src/OdometryF2M.cpp b/corelib/src/OdometryF2M.cpp index 9103d8c6..5a5d5d32 100644 --- a/corelib/src/OdometryF2M.cpp +++ b/corelib/src/OdometryF2M.cpp @@ -186,9 +186,10 @@ Transform OdometryF2M::computeTransform( Transform transform = regPipeline_->computeTransformationMod( tmpMap, *lastFrame_, - // special case for ICP-only odom, set guess to identity if we just started - !guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(), + // special case for ICP-only odom, set guess to identity if we just started or reset + !guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->framesProcessed()<2?this->getPose():Transform(), ®Info); + if(transform.isNull() && !guess.isNull() && regPipeline_->isImageRequired()) { tmpMap = *map_; @@ -591,7 +592,7 @@ Transform OdometryF2M::computeTransform( if(lastFrame_->sensorData().laserScanRaw().cols) { - pcl::PointCloud::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan); + pcl::PointCloud::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan, tmpMap.sensorData().laserScanInfo().localTransform()); pcl::PointCloud::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform()); pcl::IndicesPtr frameCloudNormalsIndices(new std::vector); @@ -623,17 +624,6 @@ Transform OdometryF2M::computeTransform( newPoints, scanMaximumMapSize_); - if(newPoints < 20) - { - UWARN("The number of new scan points added to local odometry " - "map is low (%d), you may want to decrease the parameter \"%s\" " - "(current value=%f and ICP inliers ratio is %f)", - newPoints, - Parameters::kOdomScanKeyFrameThr().c_str(), - scanKeyFrameThr_, - regInfo.icpInliersRatio); - } - if(scansBuffer_.size() > 1 && int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_) { @@ -691,7 +681,16 @@ Transform OdometryF2M::computeTransform( *mapCloudNormals += *scansBuffer_.back().first; } } - mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals); + if(mapScan.channels() == 2 || mapScan.channels() == 5) + { + Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0); + mapScan = util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint); + } + else + { + Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0); + mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint); + } modified=true; } } @@ -702,7 +701,18 @@ Transform OdometryF2M::computeTransform( { *map_ = tmpMap; - map_->sensorData().setLaserScanRaw(mapScan, LaserScanInfo(0, 0)); + if(mapScan.channels() == 2 || mapScan.channels() == 5) + { + + map_->sensorData().setLaserScanRaw(mapScan, + LaserScanInfo(0, 0.0f, Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanInfo().localTransform().z(),0,0,0))); + } + else + { + map_->sensorData().setLaserScanRaw(mapScan, + LaserScanInfo(0, 0.0f, newFramePose.translation())); + } + map_->setWords(mapWords); map_->setWords3(mapPoints); map_->setWordsDescriptors(mapDescriptors); @@ -717,7 +727,7 @@ Transform OdometryF2M::computeTransform( if(this->isInfoDataFilled()) { info->localMap = uMultimapToMap(tmpMap.getWords3()); - info->localScanMap = tmpMap.sensorData().laserScanRaw(); + info->localScanMap = util3d::transformLaserScan(tmpMap.sensorData().laserScanRaw(), tmpMap.sensorData().laserScanInfo().localTransform()); } } } @@ -831,7 +841,18 @@ Transform OdometryF2M::computeTransform( frameValid = true; pcl::PointCloud::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform()); scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector))); - map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0)); + if(lastFrame_->sensorData().laserScanRaw().channels() == 2 || lastFrame_->sensorData().laserScanRaw().channels() == 5) + { + Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0); + map_->sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint), + LaserScanInfo(0, 0.0f, Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanInfo().localTransform().z(),0,0,0))); + } + else + { + Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0); + map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), + LaserScanInfo(0, 0.0f, newFramePose.translation())); + } addKeyFrame = true; } else @@ -854,7 +875,7 @@ Transform OdometryF2M::computeTransform( if(this->isInfoDataFilled()) { info->localMap = uMultimapToMap(map_->getWords3()); - info->localScanMap = map_->sensorData().laserScanRaw(); + info->localScanMap = util3d::transformLaserScan(map_->sensorData().laserScanRaw(), map_->sensorData().laserScanInfo().localTransform()); } } } diff --git a/corelib/src/RegistrationIcp.cpp b/corelib/src/RegistrationIcp.cpp index ac7acdfb..99c8c2cf 100644 --- a/corelib/src/RegistrationIcp.cpp +++ b/corelib/src/RegistrationIcp.cpp @@ -505,18 +505,24 @@ Transform RegistrationIcp::computeTransformationImpl( pcl::PointCloud::Ptr toCloudFiltered = toCloud; if(_voxelSize > 0.0f) { - int pointsBeforeFiltering = fromCloudFiltered->size(); + float pointsBeforeFiltering = (float)fromCloudFiltered->size(); fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize); - maxLaserScansFrom = maxLaserScansFrom * fromCloudFiltered->size() / pointsBeforeFiltering; + float ratioFrom = float(fromCloudFiltered->size()) / pointsBeforeFiltering; + maxLaserScansFrom = int(float(maxLaserScansFrom) * ratioFrom); - pointsBeforeFiltering = toCloudFiltered->size(); + pointsBeforeFiltering = (float)toCloudFiltered->size(); toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize); - maxLaserScansTo = maxLaserScansTo * toCloudFiltered->size() / pointsBeforeFiltering; + float ratioTo = float(toCloudFiltered->size()) / pointsBeforeFiltering; + maxLaserScansTo = int(float(maxLaserScansTo) * ratioTo); - UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f ratioTo=%f) = %f s", + UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f->%d/%d ratioTo=%f->%d/%d) = %f s", _voxelSize, - float(fromCloudFiltered->size()) / float(pointsBeforeFiltering), - float(toCloudFiltered->size()) / float(pointsBeforeFiltering), + ratioFrom, + (int)fromCloudFiltered->size(), + maxLaserScansFrom, + ratioTo, + (int)toCloudFiltered->size(), + maxLaserScansTo, timer.ticks()); } @@ -531,11 +537,22 @@ Transform RegistrationIcp::computeTransformationImpl( if(fromScan.channels() == 2 || fromScan.channels() == 5) { - normals = util3d::computeFastOrganizedNormals2D( - fromCloudFiltered, - _pointToPlaneK, - _pointToPlaneRadius, - viewpointFrom); + if(_voxelSize > 0.0f) + { + normals = util3d::computeNormals2D( + fromCloudFiltered, + _pointToPlaneK, + _pointToPlaneRadius, + viewpointFrom); + } + else + { + normals = util3d::computeFastOrganizedNormals2D( + fromCloudFiltered, + _pointToPlaneK, + _pointToPlaneRadius, + viewpointFrom); + } } else { @@ -546,11 +563,22 @@ Transform RegistrationIcp::computeTransformationImpl( if(toScan.channels() == 2 || toScan.channels() == 5) { - normals = util3d::computeFastOrganizedNormals2D( - toCloudFiltered, - _pointToPlaneK, - _pointToPlaneRadius, - viewpointTo); + if(_voxelSize > 0.0f) + { + normals = util3d::computeNormals2D( + toCloudFiltered, + _pointToPlaneK, + _pointToPlaneRadius, + viewpointTo); + } + else + { + normals = util3d::computeFastOrganizedNormals2D( + toCloudFiltered, + _pointToPlaneK, + _pointToPlaneRadius, + viewpointTo); + } } else { @@ -564,7 +592,7 @@ Transform RegistrationIcp::computeTransformationImpl( fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals); // update output scans - if(fromScan.channels() == 2 || toScan.channels() == 5) + if(fromScan.channels() == 2 || fromScan.channels() == 5) { fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform)); } @@ -654,8 +682,22 @@ Transform RegistrationIcp::computeTransformationImpl( if(_voxelSize > 0.0f) { // update output scans - fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform)); - toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform)); + if(fromScan.channels() == 2 || fromScan.channels() == 5) + { + fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform)); + } + else + { + fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform)); + } + if(toScan.channels() == 2 || toScan.channels() == 5) + { + toSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform)); + } + else + { + toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform)); + } } #ifdef RTABMAP_POINTMATCHER diff --git a/corelib/src/util3d_registration.cpp b/corelib/src/util3d_registration.cpp index 73cf184f..86a59078 100644 --- a/corelib/src/util3d_registration.cpp +++ b/corelib/src/util3d_registration.cpp @@ -244,8 +244,8 @@ void computeVarianceAndCorrespondences( correspondencesOut = 0; pcl::registration::CorrespondenceEstimation::Ptr est; est.reset(new pcl::registration::CorrespondenceEstimation); - est->setInputTarget(cloudB); - est->setInputSource(cloudA); + est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB); + est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA); pcl::Correspondences correspondences; est->determineCorrespondences(correspondences, maxCorrespondenceDistance); @@ -277,8 +277,8 @@ void computeVarianceAndCorrespondences( correspondencesOut = 0; pcl::registration::CorrespondenceEstimation::Ptr est; est.reset(new pcl::registration::CorrespondenceEstimation); - est->setInputTarget(cloudB); - est->setInputSource(cloudA); + est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB); + est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA); pcl::Correspondences correspondences; est->determineCorrespondences(correspondences, maxCorrespondenceDistance); diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index e5603b6f..60fb5352 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -2469,7 +2469,8 @@ void DatabaseViewer::update(int value, labelPose->setText(QString("%1xyz=(%2,%3,%4)\nrpy=(%5,%6,%7)").arg(odomPose.isIdentity()?"* ":"").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw)); if(s!=0.0) { - stamp->setText(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz")); + stamp->setText(QString::number(s, 'f')); + stamp->setToolTip(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz")); } if(data.cameraModels().size() || data.stereoCameraModel().isValidForProjection()) {