mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-01 17:10:26 +08:00
OdometryF2M: updated how local scan map is updated
This commit is contained in:
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|||||||
#include <rtabmap/core/Odometry.h>
|
#include <rtabmap/core/Odometry.h>
|
||||||
#include <pcl/point_cloud.h>
|
#include <pcl/point_cloud.h>
|
||||||
#include <pcl/point_types.h>
|
#include <pcl/point_types.h>
|
||||||
|
#include <pcl/pcl_base.h>
|
||||||
|
|
||||||
namespace rtabmap {
|
namespace rtabmap {
|
||||||
|
|
||||||
@@ -57,13 +58,13 @@ private:
|
|||||||
int maxNewFeatures_;
|
int maxNewFeatures_;
|
||||||
float scanKeyFrameThr_;
|
float scanKeyFrameThr_;
|
||||||
int scanMaximumMapSize_;
|
int scanMaximumMapSize_;
|
||||||
float scanSubstractRadius_;
|
float scanSubtractRadius_;
|
||||||
std::string fixedMapPath_;
|
std::string fixedMapPath_;
|
||||||
|
|
||||||
Registration * regPipeline_;
|
Registration * regPipeline_;
|
||||||
Signature * map_;
|
Signature * map_;
|
||||||
Signature * lastFrame_;
|
Signature * lastFrame_;
|
||||||
std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr > scansBuffer_;
|
std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -372,7 +372,7 @@ class RTABMAP_EXP Parameters
|
|||||||
RTABMAP_PARAM(OdomF2M, MaxSize, int, 2000, "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
|
RTABMAP_PARAM(OdomF2M, MaxSize, int, 2000, "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
|
||||||
RTABMAP_PARAM(OdomF2M, MaxNewFeatures, int, 0, "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
|
RTABMAP_PARAM(OdomF2M, MaxNewFeatures, int, 0, "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
|
||||||
RTABMAP_PARAM(OdomF2M, ScanMaxSize, int, 2000, "[Geometry] Maximum local scan map size.");
|
RTABMAP_PARAM(OdomF2M, ScanMaxSize, int, 2000, "[Geometry] Maximum local scan map size.");
|
||||||
RTABMAP_PARAM(OdomF2M, ScanSubstractRadius, float, 0.05, "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
|
RTABMAP_PARAM(OdomF2M, ScanSubtractRadius, float, 0.05, "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
|
||||||
RTABMAP_PARAM_STR(OdomF2M, FixedMapPath, "", "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.")
|
RTABMAP_PARAM_STR(OdomF2M, FixedMapPath, "", "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.")
|
||||||
|
|
||||||
// Odometry Mono
|
// Odometry Mono
|
||||||
|
|||||||
@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|||||||
#include "rtabmap/utilite/UConversion.h"
|
#include "rtabmap/utilite/UConversion.h"
|
||||||
#include <opencv2/calib3d/calib3d.hpp>
|
#include <opencv2/calib3d/calib3d.hpp>
|
||||||
#include <rtabmap/core/OdometryF2M.h>
|
#include <rtabmap/core/OdometryF2M.h>
|
||||||
|
#include <pcl/common/io.h>
|
||||||
|
|
||||||
#if _MSC_VER
|
#if _MSC_VER
|
||||||
#define ISFINITE(value) _finite(value)
|
#define ISFINITE(value) _finite(value)
|
||||||
@@ -60,7 +61,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
|
|||||||
maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
|
maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
|
||||||
scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
|
scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
|
||||||
scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
|
scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
|
||||||
scanSubstractRadius_(Parameters::defaultOdomF2MScanSubstractRadius()),
|
scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
|
||||||
fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
|
fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
|
||||||
regPipeline_(Registration::create(parameters)),
|
regPipeline_(Registration::create(parameters)),
|
||||||
map_(new Signature(-1)),
|
map_(new Signature(-1)),
|
||||||
@@ -72,7 +73,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
|
|||||||
Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
|
Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
|
||||||
Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
|
Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
|
||||||
Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
|
Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
|
||||||
Parameters::parse(parameters, Parameters::kOdomF2MScanSubstractRadius(), scanSubstractRadius_);
|
Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
|
||||||
Parameters::parse(parameters, Parameters::kOdomF2MFixedMapPath(), fixedMapPath_);
|
Parameters::parse(parameters, Parameters::kOdomF2MFixedMapPath(), fixedMapPath_);
|
||||||
UASSERT(maximumMapSize_ >= 0);
|
UASSERT(maximumMapSize_ >= 0);
|
||||||
UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
|
UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
|
||||||
@@ -327,55 +328,115 @@ Transform OdometryF2M::computeTransform(
|
|||||||
(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_))
|
(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_))
|
||||||
{
|
{
|
||||||
UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);
|
UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);
|
||||||
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
|
|
||||||
pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
|
|
||||||
|
|
||||||
if(mapCloudNormals->size() && scanSubstractRadius_ > 0.0f)
|
UTimer tmpTimer;
|
||||||
|
if(lastFrame_->sensorData().laserScanRaw().cols)
|
||||||
{
|
{
|
||||||
frameCloudNormals = util3d::subtractFiltering(frameCloudNormals, mapCloudNormals, scanSubstractRadius_, 0.0f);
|
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
|
||||||
}
|
pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
|
||||||
if(frameCloudNormals->size())
|
|
||||||
{
|
|
||||||
scansBuffer_.insert(std::make_pair(lastFrame_->id(), frameCloudNormals));
|
|
||||||
|
|
||||||
//remove points if too big
|
pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
|
||||||
UDEBUG("scansBuffer=%d, mapSize=%d maxPoints=%d", (int)scansBuffer_.size(), int(mapCloudNormals->size() + frameCloudNormals->size()), scanMaximumMapSize_);
|
int newPoints;
|
||||||
if(scansBuffer_.size() > 1 && int(mapCloudNormals->size() + frameCloudNormals->size()) > scanMaximumMapSize_)
|
if(mapCloudNormals->size() && scanSubtractRadius_ > 0.0f)
|
||||||
{
|
{
|
||||||
//asssemble
|
frameCloudNormalsIndices = util3d::subtractFiltering(
|
||||||
mapCloudNormals->clear();
|
frameCloudNormals,
|
||||||
std::list<int> toRemove;
|
pcl::IndicesPtr(new std::vector<int>),
|
||||||
for(std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr>::reverse_iterator iter=scansBuffer_.rbegin();
|
mapCloudNormals,
|
||||||
iter!=scansBuffer_.rend();
|
pcl::IndicesPtr(new std::vector<int>),
|
||||||
++iter)
|
scanSubtractRadius_,
|
||||||
{
|
0.0f);
|
||||||
if(mapCloudNormals->empty())
|
newPoints = frameCloudNormalsIndices->size();
|
||||||
{
|
|
||||||
*mapCloudNormals = *iter->second;
|
|
||||||
}
|
|
||||||
else if((int)mapCloudNormals->size() < scanMaximumMapSize_)
|
|
||||||
{
|
|
||||||
*mapCloudNormals += *iter->second;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
toRemove.push_back(iter->first);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
for(std::list<int>::iterator iter=toRemove.begin(); iter!=toRemove.end(); ++iter)
|
|
||||||
{
|
|
||||||
scansBuffer_.erase(*iter);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
//assemble
|
newPoints = mapCloudNormals->size();
|
||||||
*mapCloudNormals += *frameCloudNormals;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
|
if(newPoints)
|
||||||
modified=true;
|
{
|
||||||
|
scansBuffer_.push_back(std::make_pair(frameCloudNormals, frameCloudNormalsIndices));
|
||||||
|
|
||||||
|
//remove points if too big
|
||||||
|
UDEBUG("scansBuffer=%d, mapSize=%d newPoints=%d maxPoints=%d",
|
||||||
|
(int)scansBuffer_.size(),
|
||||||
|
int(mapCloudNormals->size()),
|
||||||
|
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_)
|
||||||
|
{
|
||||||
|
//regenerate the local map
|
||||||
|
mapCloudNormals->clear();
|
||||||
|
std::list<int> toRemove;
|
||||||
|
int i = int(scansBuffer_.size())-1;
|
||||||
|
for(; i>=0; --i)
|
||||||
|
{
|
||||||
|
int pointsToAdd = scansBuffer_[i].second->size()?scansBuffer_[i].second->size():scansBuffer_[i].first->size();
|
||||||
|
if((int)mapCloudNormals->size() + pointsToAdd > scanMaximumMapSize_ ||
|
||||||
|
i == 0)
|
||||||
|
{
|
||||||
|
*mapCloudNormals += *scansBuffer_[i].first;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
if(scansBuffer_[i].second->size())
|
||||||
|
{
|
||||||
|
pcl::PointCloud<pcl::PointNormal> tmp;
|
||||||
|
pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
|
||||||
|
*mapCloudNormals += tmp;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
*mapCloudNormals += *scansBuffer_[i].first;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// remove old clouds
|
||||||
|
if(i > 0)
|
||||||
|
{
|
||||||
|
std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
|
||||||
|
int oi = 0;
|
||||||
|
for(; i<(int)scansBuffer_.size(); ++i)
|
||||||
|
{
|
||||||
|
UASSERT(oi < (int)scansTmp.size());
|
||||||
|
scansTmp[oi++] = scansBuffer_[i];
|
||||||
|
}
|
||||||
|
scansBuffer_ = scansTmp;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// just append the last cloud
|
||||||
|
if(scansBuffer_.back().second->size())
|
||||||
|
{
|
||||||
|
pcl::PointCloud<pcl::PointNormal> tmp;
|
||||||
|
pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
|
||||||
|
*mapCloudNormals += tmp;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
*mapCloudNormals += *scansBuffer_.back().first;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
|
||||||
|
modified=true;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
UDEBUG("Update local map = %fs", tmpTimer.ticks());
|
||||||
}
|
}
|
||||||
|
|
||||||
if(modified)
|
if(modified)
|
||||||
@@ -473,13 +534,13 @@ Transform OdometryF2M::computeTransform(
|
|||||||
if (fixedMapPath_.empty())
|
if (fixedMapPath_.empty())
|
||||||
{
|
{
|
||||||
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
|
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
|
||||||
scansBuffer_.insert(std::make_pair(lastFrame_->id(), mapCloudNormals));
|
scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
|
||||||
map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), 0, 0);
|
map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), 0,0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
UWARN("Mising scan to initialize odometry.");
|
UWARN("Missing scan to initialize odometry.");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -221,6 +221,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
|
|||||||
|
|
||||||
// 0.11.8
|
// 0.11.8
|
||||||
removedParameters_.insert(std::make_pair("Reg/Force2D", std::make_pair(true, Parameters::kRegForce3DoF())));
|
removedParameters_.insert(std::make_pair("Reg/Force2D", std::make_pair(true, Parameters::kRegForce3DoF())));
|
||||||
|
removedParameters_.insert(std::make_pair("OdomF2M/ScanSubstractRadius", std::make_pair(true, Parameters::kOdomF2MScanSubtractRadius())));
|
||||||
|
|
||||||
// 0.11.6
|
// 0.11.6
|
||||||
removedParameters_.insert(std::make_pair("RGBD/ProximityPathScansMerged", std::make_pair(false, "")));
|
removedParameters_.insert(std::make_pair("RGBD/ProximityPathScansMerged", std::make_pair(false, "")));
|
||||||
|
|||||||
@@ -113,14 +113,16 @@ Transform RegistrationIcp::computeTransformationImpl(
|
|||||||
if(!guess.isNull() && !dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
|
if(!guess.isNull() && !dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
|
||||||
{
|
{
|
||||||
// ICP with guess transform
|
// ICP with guess transform
|
||||||
int maxLaserScans = dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():dataFrom.laserScanMaxPts();
|
int maxLaserScansTo = dataTo.laserScanMaxPts();
|
||||||
|
int maxLaserScansFrom = dataFrom.laserScanMaxPts();
|
||||||
cv::Mat fromScan = dataFrom.laserScanRaw();
|
cv::Mat fromScan = dataFrom.laserScanRaw();
|
||||||
cv::Mat toScan = dataTo.laserScanRaw();
|
cv::Mat toScan = dataTo.laserScanRaw();
|
||||||
if(_downsamplingStep>1)
|
if(_downsamplingStep>1)
|
||||||
{
|
{
|
||||||
fromScan = util3d::downsample(fromScan, _downsamplingStep);
|
fromScan = util3d::downsample(fromScan, _downsamplingStep);
|
||||||
toScan = util3d::downsample(toScan, _downsamplingStep);
|
toScan = util3d::downsample(toScan, _downsamplingStep);
|
||||||
maxLaserScans/=_downsamplingStep;
|
maxLaserScansTo/=_downsamplingStep;
|
||||||
|
maxLaserScansFrom/=_downsamplingStep;
|
||||||
UDEBUG("Downsampling time (step=%d) = %f s", _downsamplingStep, timer.ticks());
|
UDEBUG("Downsampling time (step=%d) = %f s", _downsamplingStep, timer.ticks());
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -140,6 +142,7 @@ Transform RegistrationIcp::computeTransformationImpl(
|
|||||||
//special case if we have already normals computed and there is no filtering
|
//special case if we have already normals computed and there is no filtering
|
||||||
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, Transform());
|
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, Transform());
|
||||||
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess);
|
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess);
|
||||||
|
|
||||||
UDEBUG("Conversion time = %f s", timer.ticks());
|
UDEBUG("Conversion time = %f s", timer.ticks());
|
||||||
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
|
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
|
||||||
icpT = util3d::icpPointToPlane(
|
icpT = util3d::icpPointToPlane(
|
||||||
@@ -186,14 +189,19 @@ Transform RegistrationIcp::computeTransformationImpl(
|
|||||||
bool filtered = false;
|
bool filtered = false;
|
||||||
if(_voxelSize > 0.0f)
|
if(_voxelSize > 0.0f)
|
||||||
{
|
{
|
||||||
|
int pointsBeforeFiltering = fromCloudFiltered->size();
|
||||||
fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
|
fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
|
||||||
int pointsBeforeFiltering = toCloudFiltered->size();
|
maxLaserScansFrom = maxLaserScansFrom * fromCloudFiltered->size() / pointsBeforeFiltering;
|
||||||
|
|
||||||
|
pointsBeforeFiltering = toCloudFiltered->size();
|
||||||
toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
|
toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
|
||||||
|
maxLaserScansTo = maxLaserScansTo * toCloudFiltered->size() / pointsBeforeFiltering;
|
||||||
|
|
||||||
filtered = true;
|
filtered = true;
|
||||||
UDEBUG("Voxel filtering time (voxel=%f m) = %f s", _voxelSize, timer.ticks());
|
UDEBUG("Voxel filtering time (voxel=%f m) = %f s", _voxelSize, timer.ticks());
|
||||||
|
|
||||||
//Adjust maxLaserScans
|
//Adjust maxLaserScans
|
||||||
maxLaserScans = maxLaserScans * toCloudFiltered->size() / pointsBeforeFiltering;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool correspondencesComputed = false;
|
bool correspondencesComputed = false;
|
||||||
@@ -214,6 +222,10 @@ Transform RegistrationIcp::computeTransformationImpl(
|
|||||||
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
|
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
|
||||||
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
|
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
|
||||||
|
|
||||||
|
// update output scans
|
||||||
|
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals), maxLaserScansFrom, fromSignature.sensorData().laserScanMaxRange());
|
||||||
|
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, guess.inverse()), maxLaserScansTo, toSignature.sensorData().laserScanMaxRange());
|
||||||
|
|
||||||
UDEBUG("Compute normals time = %f s", timer.ticks());
|
UDEBUG("Compute normals time = %f s", timer.ticks());
|
||||||
|
|
||||||
if(toCloudNormals->size() && fromCloudNormals->size())
|
if(toCloudNormals->size() && fromCloudNormals->size())
|
||||||
@@ -244,6 +256,13 @@ Transform RegistrationIcp::computeTransformationImpl(
|
|||||||
}
|
}
|
||||||
else // ICP Point to Point
|
else // ICP Point to Point
|
||||||
{
|
{
|
||||||
|
if(_voxelSize > 0.0f)
|
||||||
|
{
|
||||||
|
// update output scans
|
||||||
|
fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered), maxLaserScansFrom, fromSignature.sensorData().laserScanMaxRange());
|
||||||
|
toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, guess.inverse()), maxLaserScansTo, toSignature.sensorData().laserScanMaxRange());
|
||||||
|
}
|
||||||
|
|
||||||
icpT = util3d::icp(
|
icpT = util3d::icp(
|
||||||
fromCloudFiltered,
|
fromCloudFiltered,
|
||||||
toCloudFiltered,
|
toCloudFiltered,
|
||||||
@@ -310,6 +329,7 @@ Transform RegistrationIcp::computeTransformationImpl(
|
|||||||
else
|
else
|
||||||
{
|
{
|
||||||
// verify if there are enough correspondences
|
// verify if there are enough correspondences
|
||||||
|
int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
|
||||||
if(maxLaserScans)
|
if(maxLaserScans)
|
||||||
{
|
{
|
||||||
correspondencesRatio = float(correspondences)/float(maxLaserScans);
|
correspondencesRatio = float(correspondences)/float(maxLaserScans);
|
||||||
@@ -331,7 +351,7 @@ Transform RegistrationIcp::computeTransformationImpl(
|
|||||||
hasConverged?"true":"false",
|
hasConverged?"true":"false",
|
||||||
variance,
|
variance,
|
||||||
correspondences,
|
correspondences,
|
||||||
maxLaserScans>0?maxLaserScans:dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():(int)(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols),
|
maxLaserScans>0?maxLaserScans:(int)(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols),
|
||||||
correspondencesRatio*100.0f);
|
correspondencesRatio*100.0f);
|
||||||
|
|
||||||
info.variance = variance>0.0f?variance:0.0001; // epsilon if exact transform
|
info.variance = variance>0.0f?variance:0.0001; // epsilon if exact transform
|
||||||
|
|||||||
@@ -782,7 +782,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
|
|||||||
_ui->odom_localHistory->setObjectName(Parameters::kOdomF2MMaxSize().c_str());
|
_ui->odom_localHistory->setObjectName(Parameters::kOdomF2MMaxSize().c_str());
|
||||||
_ui->spinBox_odom_f2m_maxNewFeatures->setObjectName(Parameters::kOdomF2MMaxNewFeatures().c_str());
|
_ui->spinBox_odom_f2m_maxNewFeatures->setObjectName(Parameters::kOdomF2MMaxNewFeatures().c_str());
|
||||||
_ui->spinBox_odom_f2m_scanMaxSize->setObjectName(Parameters::kOdomF2MScanMaxSize().c_str());
|
_ui->spinBox_odom_f2m_scanMaxSize->setObjectName(Parameters::kOdomF2MScanMaxSize().c_str());
|
||||||
_ui->doubleSpinBox_odom_f2m_scanRadius->setObjectName(Parameters::kOdomF2MScanSubstractRadius().c_str());
|
_ui->doubleSpinBox_odom_f2m_scanRadius->setObjectName(Parameters::kOdomF2MScanSubtractRadius().c_str());
|
||||||
_ui->odom_fixedLocalMapPath->setObjectName(Parameters::kOdomF2MFixedMapPath().c_str());
|
_ui->odom_fixedLocalMapPath->setObjectName(Parameters::kOdomF2MFixedMapPath().c_str());
|
||||||
connect(_ui->toolButton_odomBowFixedLocalMap, SIGNAL(clicked()), this, SLOT(changeOdomBowFixedLocalMapPath()));
|
connect(_ui->toolButton_odomBowFixedLocalMap, SIGNAL(clicked()), this, SLOT(changeOdomBowFixedLocalMapPath()));
|
||||||
|
|
||||||
|
|||||||
@@ -95,7 +95,7 @@
|
|||||||
<enum>QFrame::Raised</enum>
|
<enum>QFrame::Raised</enum>
|
||||||
</property>
|
</property>
|
||||||
<property name="currentIndex">
|
<property name="currentIndex">
|
||||||
<number>3</number>
|
<number>14</number>
|
||||||
</property>
|
</property>
|
||||||
<widget class="QWidget" name="page_22">
|
<widget class="QWidget" name="page_22">
|
||||||
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
|
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
|
||||||
@@ -8218,7 +8218,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
|
|||||||
<item row="2" column="2">
|
<item row="2" column="2">
|
||||||
<widget class="QLabel" name="label_195">
|
<widget class="QLabel" name="label_195">
|
||||||
<property name="text">
|
<property name="text">
|
||||||
<string>[Geometry] Maximum scan map size is defined by this factor times the maximum size of a single scan. For example, if the laser scans have 1000 values, then the maximum local map size will be 2000 if the factor is 2.</string>
|
<string>[Geometry] Maximum local scan map size (points).</string>
|
||||||
</property>
|
</property>
|
||||||
<property name="wordWrap">
|
<property name="wordWrap">
|
||||||
<bool>true</bool>
|
<bool>true</bool>
|
||||||
|
|||||||
Reference in New Issue
Block a user