i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Version 0.11.0: Refactored Visual/ICP transformation estimation approaches, Added Registration classes for convenience, Added Parameters migration approach, 3D laser scans can be used

Browse Source
This commit is contained in:
matlabbe
2015-11-22 18:08:32 -05:00
parent 1e5bcfded8
commit ae9f21acd7
46 changed files with 4934 additions and 4547 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/src/CMakeLists.txt
View File

@@ -44,6 +44,9 @@ SET(SRC_FILES
Compression.cpp
Link.cpp
RegistrationIcp.cpp
RegistrationVis.cpp
Odometry.cpp
OdometryThread.cpp
OdometryBOW.cpp

142
corelib/src/CameraRGB.cpp
View File

@@ -38,6 +38,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/imgproc/imgproc.hpp>
#include <rtabmap/core/util3d.h>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <cmath>
@@ -61,7 +64,36 @@ CameraImages::CameraImages(const std::string & path,
_rectifyImages(rectifyImages),
_isDepth(isDepth),
_count(0),
_dir(0)
_dir(0),
_countScan(0),
_scanDir(0)
{
}
CameraImages::CameraImages(const std::string & scanPath,
const Transform & scanLocalTransform,
int scanMaxPts,
const std::string & path,
int startAt,
bool refreshDir,
bool rectifyImages,
bool isDepth,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
_path(path),
_startAt(startAt),
_refreshDir(refreshDir),
_rectifyImages(rectifyImages),
_isDepth(isDepth),
_count(0),
_dir(0),
_countScan(0),
_scanDir(0),
_scanPath(scanPath),
_scanLocalTransform(scanLocalTransform),
_scanMaxPts(scanMaxPts)
{
}
@@ -72,11 +104,19 @@ CameraImages::~CameraImages(void)
{
delete _dir;
}
if(_scanDir)
{
delete _scanDir;
}
}
bool CameraImages::init(const std::string & calibrationFolder, const std::string & cameraName)
{
_cameraName = cameraName;
_lastFileName.clear();
_lastScanFileName.clear();
_count = 0;
_countScan = 0;
UDEBUG("");
if(_dir)
@@ -87,7 +127,6 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
{
_dir = new UDirectory(_path, "jpg ppm png bmp pnm tiff");
}
_count = 0;
if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
{
_path.append("/");
@@ -105,6 +144,47 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
}
// check for scan directory
if(_scanDir)
{
delete _scanDir;
_scanDir = 0;
}
if(!_scanPath.empty())
{
_scanDir = new UDirectory(_scanPath, "pcd bin"); // "bin" is for KITTI format
if(_scanPath[_scanPath.size()-1] != '\\' && _scanPath[_scanPath.size()-1] != '/')
{
_scanPath.append("/");
}
if(!_scanDir->isValid())
{
UERROR("Scan directory path is not valid \"%s\"", _scanPath.c_str());
delete _scanDir;
_scanDir = 0;
}
else if(_scanDir->getFileNames().size() == 0)
{
UWARN("Scan directory is empty \"%s\"", _scanPath.c_str());
delete _scanDir;
_scanDir = 0;
}
else if(_scanDir->getFileNames().size() != _dir->getFileNames().size())
{
UERROR("Scan and image directories should be the same size \"%s\"(%d) vs \"%s\"(%d)",
_scanPath.c_str(),
(int)_scanDir->getFileNames().size(),
_path.c_str(),
(int)_dir->getFileNames().size());
delete _scanDir;
_scanDir = 0;
}
else
{
UINFO("path=%s scans=%d", _scanPath.c_str(), (int)this->imagesCount());
}
}
// look for calibration files
if(!calibrationFolder.empty() && !cameraName.empty())
{
@@ -164,12 +244,17 @@ std::vector<std::string> CameraImages::filenames() const
SensorData CameraImages::captureImage()
{
cv::Mat img;
cv::Mat scan;
UDEBUG("");
if(_dir->isValid())
{
if(_refreshDir)
{
_dir->update();
if(_scanDir)
{
_scanDir->update();
}
}
if(_startAt == 0)
{
@@ -183,6 +268,28 @@ SensorData CameraImages::captureImage()
img = cv::imread(fullPath.c_str());
}
}
if(_scanDir)
{
const std::list<std::string> & scanFileNames = _scanDir->getFileNames();
if(scanFileNames.size())
{
if(_lastScanFileName.empty() || uStrNumCmp(_lastScanFileName,*scanFileNames.rbegin()) < 0)
{
_lastScanFileName = *scanFileNames.rbegin();
std::string fullPath = _scanPath + _lastScanFileName;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(UFile::getExtension(_lastScanFileName).compare("bin") == 0)
{
cloud = util3d::loadBINCloud(fullPath, 4); // Assume KITTI velodyne format
}
else
{
pcl::io::loadPCDFile(fullPath, *cloud);
}
scan = util3d::laserScanFromPointCloud(*cloud, _scanLocalTransform);
}
}
}
}
else
{
@@ -242,6 +349,33 @@ SensorData CameraImages::captureImage()
}
}
}
if(_scanDir)
{
fileName = _scanDir->getNextFileName();
if(fileName.size())
{
fullPath = _scanPath + fileName;
while(++_countScan < _startAt && (fileName = _scanDir->getNextFileName()).size())
{
fullPath = _scanPath + fileName;
}
if(fileName.size())
{
UDEBUG("Loading scan : %s", fullPath.c_str());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(UFile::getExtension(fileName).compare("bin") == 0)
{
cloud = util3d::loadBINCloud(fullPath, 4); // Assume KITTI velodyne format
}
else
{
pcl::io::loadPCDFile(fullPath, *cloud);
}
scan = util3d::laserScanFromPointCloud(*cloud, _scanLocalTransform);
}
}
}
}
if(!img.empty() && _model.isValid() && _rectifyImages)
@@ -256,9 +390,9 @@ SensorData CameraImages::captureImage()
if(_isDepth)
{
return SensorData(cv::Mat(), img, _model, this->getNextSeqID(), UTimer::now());
return SensorData(scan, scan.empty()?0:_scanMaxPts, 0, cv::Mat(), img, _model, this->getNextSeqID(), UTimer::now());
}
return SensorData(img, _model, this->getNextSeqID(), UTimer::now());
return SensorData(scan, scan.empty()?0:_scanMaxPts, 0, img, cv::Mat(), _model, this->getNextSeqID(), UTimer::now());
}

26
corelib/src/CameraStereo.cpp
View File

@@ -779,6 +779,28 @@ CameraStereoImages::CameraStereoImages(
}
}
CameraStereoImages::CameraStereoImages(
const std::string & scanPath,
const Transform & scanLocalTransform,
int scanMaxPts,
const std::string & pathLeftImages,
const std::string & pathRightImages,
bool filenamesAreTimestamps,
const std::string & timestampsPath,
bool rectifyImages,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
camera_(0),
camera2_(0),
filenamesAreTimestamps_(filenamesAreTimestamps),
timestampsPath_(timestampsPath),
rectifyImages_(rectifyImages)
{
camera_ = new CameraImages(scanPath, scanLocalTransform, scanMaxPts, pathLeftImages);
camera2_ = new CameraImages(pathRightImages);
}
CameraStereoImages::~CameraStereoImages()
{
if(camera_)
@@ -811,6 +833,7 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
stereoModel_.baseline());
}
}
stereoModel_.setLocalTransform(this->getLocalTransform());
if(rectifyImages_ && !stereoModel_.isValid())
{
@@ -968,7 +991,7 @@ SensorData CameraStereoImages::captureImage()
leftImage = stereoModel_.left().rectifyImage(leftImage);
rightImage = stereoModel_.right().rectifyImage(rightImage);
}
data = SensorData(leftImage, rightImage, stereoModel_, this->getNextSeqID(), stamp);
data = SensorData(left.laserScanRaw(), left.laserScanMaxPts(), 0, leftImage, rightImage, stereoModel_, this->getNextSeqID(), stamp);
}
}
}
@@ -1034,6 +1057,7 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
stereoModel_.baseline());
}
}
stereoModel_.setLocalTransform(this->getLocalTransform());
if(rectifyImages_ && !stereoModel_.isValid())
{

5
corelib/src/Compression.cpp
View File

@@ -231,11 +231,6 @@ cv::Mat uncompressData(const unsigned char * bytes, unsigned long size)
int width = *((int*)&bytes[size-2*sizeof(int)]);
int type = *((int*)&bytes[size-1*sizeof(int)]);
// If the size is higher, it may be a wrong data format.
UASSERT_MSG(height>=0 && height<10000 &&
width>=0 && width<10000,
uFormat("size=%d, height=%d width=%d type=%d", size, height, width, type).c_str());
data = cv::Mat(height, width, type);
uLongf totalUncompressed = uLongf(data.total())*uLongf(data.elemSize());

8
corelib/src/Features2d.cpp
View File

@@ -60,18 +60,22 @@ namespace rtabmap {
void Feature2D::filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
float minDepth,
float maxDepth)
{
cv::Mat descriptors;
filterKeypointsByDepth(keypoints, descriptors, depth, maxDepth);
filterKeypointsByDepth(keypoints, descriptors, depth, minDepth, maxDepth);
}
void Feature2D::filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors,
const cv::Mat & depth,
float minDepth,
float maxDepth)
{
UASSERT(minDepth >= 0.0f);
UASSERT(maxDepth <= 0.0f || maxDepth > minDepth);
if(!depth.empty() && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
{
std::vector<cv::KeyPoint> output(keypoints.size());
@@ -85,7 +89,7 @@ void Feature2D::filterKeypointsByDepth(
if(u >=0 && u<depth.cols && v >=0 && v<depth.rows)
{
float d = isInMM?(float)depth.at<uint16_t>(v,u)*0.001f:depth.at<float>(v,u);
if(uIsFinite(d) && d>0.0f && (maxDepth <= 0.0f || d < maxDepth))
if(uIsFinite(d) && d>minDepth && (maxDepth <= 0.0f || d < maxDepth))
{
output[oi++] = keypoints[i];
indexes[i] = 1;

1011
corelib/src/Memory.cpp
View File

File diff suppressed because it is too large Load Diff

44
corelib/src/Odometry.cpp
View File

@@ -35,14 +35,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_roiRatios(Parameters::defaultOdomRoiRatios()),
_minInliers(Parameters::defaultOdomMinInliers()),
_inlierDistance(Parameters::defaultOdomInlierDistance()),
_iterations(Parameters::defaultOdomIterations()),
_refineIterations(Parameters::defaultOdomRefineIterations()),
_maxDepth(Parameters::defaultOdomMaxDepth()),
_roiRatios(Parameters::defaultVisRoiRatios()),
_minInliers(Parameters::defaultVisMinInliers()),
_inlierDistance(Parameters::defaultVisInlierDistance()),
_iterations(Parameters::defaultVisIterations()),
_refineIterations(Parameters::defaultVisRefineIterations()),
_maxDepth(Parameters::defaultVisMaxDepth()),
_resetCountdown(Parameters::defaultOdomResetCountdown()),
_force2D(Parameters::defaultOdomForce2D()),
_force2D(Parameters::defaultVisForce2D()),
_holonomic(Parameters::defaultOdomHolonomic()),
_particleFiltering(Parameters::defaultOdomParticleFiltering()),
_particleSize(Parameters::defaultOdomParticleSize()),
@@ -51,30 +51,30 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_particleNoiseR(Parameters::defaultOdomParticleNoiseR()),
_particleLambdaR(Parameters::defaultOdomParticleLambdaR()),
_fillInfoData(Parameters::defaultOdomFillInfoData()),
_estimationType(Parameters::defaultOdomEstimationType()),
_pnpReprojError(Parameters::defaultOdomPnPReprojError()),
_pnpFlags(Parameters::defaultOdomPnPFlags()),
_varianceFromInliersCount(Parameters::defaultOdomVarianceFromInliersCount()),
_estimationType(Parameters::defaultVisEstimationType()),
_pnpReprojError(Parameters::defaultVisPnPReprojError()),
_pnpFlags(Parameters::defaultVisPnPFlags()),
_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount()),
_resetCurrentCount(0),
previousStamp_(0),
previousTransform_(Transform::getIdentity()),
distanceTravelled_(0)
{
Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers);
Parameters::parse(parameters, Parameters::kOdomInlierDistance(), _inlierDistance);
Parameters::parse(parameters, Parameters::kOdomIterations(), _iterations);
Parameters::parse(parameters, Parameters::kOdomRefineIterations(), _refineIterations);
Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D);
Parameters::parse(parameters, Parameters::kVisMinInliers(), _minInliers);
Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
Parameters::parse(parameters, Parameters::kVisMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kVisRoiRatios(), _roiRatios);
Parameters::parse(parameters, Parameters::kVisForce2D(), _force2D);
Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
Parameters::parse(parameters, Parameters::kOdomEstimationType(), _estimationType);
Parameters::parse(parameters, Parameters::kOdomPnPReprojError(), _pnpReprojError);
Parameters::parse(parameters, Parameters::kOdomPnPFlags(), _pnpFlags);
Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _pnpReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _pnpFlags);
UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
Parameters::parse(parameters, Parameters::kOdomVarianceFromInliersCount(), _varianceFromInliersCount);
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
Parameters::parse(parameters, Parameters::kOdomParticleFiltering(), _particleFiltering);
Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT);

28
corelib/src/OdometryBOW.cpp
View File

@@ -65,26 +65,26 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
int nn = Parameters::defaultOdomBowNNType();
float nndr = Parameters::defaultOdomBowNNDR();
int featureType = Parameters::defaultOdomFeatureType();
int maxFeatures = Parameters::defaultOdomMaxFeatures();
Parameters::parse(parameters, Parameters::kOdomBowNNType(), nn);
Parameters::parse(parameters, Parameters::kOdomBowNNDR(), nndr);
Parameters::parse(parameters, Parameters::kOdomFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
int nn = Parameters::defaultVisNNType();
float nndr = Parameters::defaultVisNNDR();
int featureType = Parameters::defaultVisFeatureType();
int maxFeatures = Parameters::defaultVisMaxFeatures();
Parameters::parse(parameters, Parameters::kVisNNType(), nn);
Parameters::parse(parameters, Parameters::kVisNNDR(), nndr);
Parameters::parse(parameters, Parameters::kVisFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
// Memory's stereo parameters, copy from Odometry
int subPixWinSize = Parameters::defaultOdomSubPixWinSize();
int subPixIterations = Parameters::defaultOdomSubPixIterations();
double subPixEps = Parameters::defaultOdomSubPixEps();
Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize);
Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations);
Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps);
int subPixWinSize = Parameters::defaultVisSubPixWinSize();
int subPixIterations = Parameters::defaultVisSubPixIterations();
double subPixEps = Parameters::defaultVisSubPixEps();
Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize);
Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations);
Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps);
customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));

28
corelib/src/OdometryMono.cpp
View File

@@ -94,25 +94,25 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kKpTfIdfLikelihoodUsed(), "false"));
int nn = Parameters::defaultOdomBowNNType();
float nndr = Parameters::defaultOdomBowNNDR();
int featureType = Parameters::defaultOdomFeatureType();
int maxFeatures = Parameters::defaultOdomMaxFeatures();
Parameters::parse(parameters, Parameters::kOdomBowNNType(), nn);
Parameters::parse(parameters, Parameters::kOdomBowNNDR(), nndr);
Parameters::parse(parameters, Parameters::kOdomFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
int nn = Parameters::defaultVisNNType();
float nndr = Parameters::defaultVisNNDR();
int featureType = Parameters::defaultVisFeatureType();
int maxFeatures = Parameters::defaultVisMaxFeatures();
Parameters::parse(parameters, Parameters::kVisNNType(), nn);
Parameters::parse(parameters, Parameters::kVisNNDR(), nndr);
Parameters::parse(parameters, Parameters::kVisFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
int subPixWinSize = Parameters::defaultOdomSubPixWinSize();
int subPixIterations = Parameters::defaultOdomSubPixIterations();
double subPixEps = Parameters::defaultOdomSubPixEps();
Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize);
Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations);
Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps);
int subPixWinSize = Parameters::defaultVisSubPixWinSize();
int subPixIterations = Parameters::defaultVisSubPixIterations();
double subPixEps = Parameters::defaultVisSubPixEps();
Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize);
Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations);
Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps);
customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));

20
corelib/src/OdometryOpticalFlow.cpp
View File

@@ -54,9 +54,9 @@ OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
stereoEps_(Parameters::defaultStereoEps()),
stereoMaxLevel_(Parameters::defaultStereoMaxLevel()),
stereoMaxSlope_(Parameters::defaultStereoMaxSlope()),
subPixWinSize_(Parameters::defaultOdomSubPixWinSize()),
subPixIterations_(Parameters::defaultOdomSubPixIterations()),
subPixEps_(Parameters::defaultOdomSubPixEps()),
subPixWinSize_(Parameters::defaultVisSubPixWinSize()),
subPixIterations_(Parameters::defaultVisSubPixIterations()),
subPixEps_(Parameters::defaultVisSubPixEps()),
refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
{
Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
@@ -68,20 +68,20 @@ OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kStereoEps(), stereoEps_);
Parameters::parse(parameters, Parameters::kStereoMaxLevel(), stereoMaxLevel_);
Parameters::parse(parameters, Parameters::kStereoMaxSlope(), stereoMaxSlope_);
Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize_);
Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations_);
Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps_);
Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize_);
Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations_);
Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps_);
ParametersMap::const_iterator iter;
Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
if((iter=parameters.find(Parameters::kOdomFeatureType())) != parameters.end())
Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultVisFeatureType();
if((iter=parameters.find(Parameters::kVisFeatureType())) != parameters.end())
{
detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
}
ParametersMap customParameters;
int maxFeatures = Parameters::defaultOdomMaxFeatures();
Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
int maxFeatures = Parameters::defaultVisMaxFeatures();
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
// add only feature stuff
for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)

91
corelib/src/Parameters.cpp
View File

@@ -39,6 +39,8 @@ namespace rtabmap
ParametersMap Parameters::parameters_;
ParametersMap Parameters::descriptions_;
Parameters Parameters::instance_;
std::map<std::string, std::pair<bool, std::string> > Parameters::removedParameters_;
ParametersMap Parameters::backwardCompatibilityMap_;
Parameters::Parameters()
{
@@ -69,6 +71,95 @@ std::string Parameters::getDefaultDatabaseName()
return "rtabmap.db";
}
const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemovedParameters()
{
if(removedParameters_.empty())
{
// removed parameters
removedParameters_.insert(std::make_pair("Mem/LaserScanVoxelSize", std::make_pair(false, Parameters::kMemLaserScanDownsampleStepSize())));
removedParameters_.insert(std::make_pair("RGBD/PoseScanMatching", std::make_pair(true, Parameters::kRGBDIcpOdomRefining())));
removedParameters_.insert(std::make_pair("Odom/FeatureType", std::make_pair(true, Parameters::kVisFeatureType())));
removedParameters_.insert(std::make_pair("Odom/EstimationType", std::make_pair(true, Parameters::kVisEstimationType())));
removedParameters_.insert(std::make_pair("Odom/MaxFeatures", std::make_pair(true, Parameters::kVisMaxFeatures())));
removedParameters_.insert(std::make_pair("Odom/InlierDistance", std::make_pair(true, Parameters::kVisInlierDistance())));
removedParameters_.insert(std::make_pair("Odom/MinInliers", std::make_pair(true, Parameters::kVisMinInliers())));
removedParameters_.insert(std::make_pair("Odom/Iterations", std::make_pair(true, Parameters::kVisIterations())));
removedParameters_.insert(std::make_pair("Odom/RefineIterations", std::make_pair(true, Parameters::kVisRefineIterations())));
removedParameters_.insert(std::make_pair("Odom/MaxDepth", std::make_pair(true, Parameters::kVisMaxDepth())));
removedParameters_.insert(std::make_pair("Odom/RoiRatios", std::make_pair(true, Parameters::kVisRoiRatios())));
removedParameters_.insert(std::make_pair("Odom/Force2D", std::make_pair(true, Parameters::kVisForce2D())));
removedParameters_.insert(std::make_pair("Odom/VarianceFromInliersCount", std::make_pair(true, Parameters::kRegVarianceFromInliersCount())));
removedParameters_.insert(std::make_pair("Odom/PnPReprojError", std::make_pair(true, Parameters::kVisPnPReprojError())));
removedParameters_.insert(std::make_pair("Odom/PnPFlags", std::make_pair(true, Parameters::kVisPnPFlags())));
removedParameters_.insert(std::make_pair("OdomBow/NNType", std::make_pair(true, Parameters::kVisNNType())));
removedParameters_.insert(std::make_pair("OdomBow/NNDR", std::make_pair(true, Parameters::kVisNNDR())));
removedParameters_.insert(std::make_pair("OdomSubPix/WinSize", std::make_pair(true, Parameters::kVisSubPixWinSize())));
removedParameters_.insert(std::make_pair("OdomSubPix/Iterations", std::make_pair(true, Parameters::kVisSubPixIterations())));
removedParameters_.insert(std::make_pair("OdomSubPix/Eps", std::make_pair(true, Parameters::kVisSubPixEps())));
removedParameters_.insert(std::make_pair("LccReextract/Activated", std::make_pair(false, Parameters::kRGBDLoopClosureReextractFeatures())));
removedParameters_.insert(std::make_pair("LccReextract/FeatureType", std::make_pair(false, Parameters::kVisFeatureType())));
removedParameters_.insert(std::make_pair("LccReextract/MaxWords", std::make_pair(false, Parameters::kVisMaxFeatures())));
removedParameters_.insert(std::make_pair("LccReextract/MaxDepth", std::make_pair(false, Parameters::kVisMaxDepth())));
removedParameters_.insert(std::make_pair("LccReextract/RoiRatios", std::make_pair(false, Parameters::kVisRoiRatios())));
removedParameters_.insert(std::make_pair("LccReextract/NNType", std::make_pair(false, Parameters::kVisNNType())));
removedParameters_.insert(std::make_pair("LccReextract/NNDR", std::make_pair(false, Parameters::kVisNNDR())));
removedParameters_.insert(std::make_pair("LccBow/EstimationType", std::make_pair(false, Parameters::kVisEstimationType())));
removedParameters_.insert(std::make_pair("LccBow/InlierDistance", std::make_pair(false, Parameters::kVisInlierDistance())));
removedParameters_.insert(std::make_pair("LccBow/MinInliers", std::make_pair(false, Parameters::kVisMinInliers())));
removedParameters_.insert(std::make_pair("LccBow/Iterations", std::make_pair(false, Parameters::kVisIterations())));
removedParameters_.insert(std::make_pair("LccBow/RefineIterations", std::make_pair(false, Parameters::kVisRefineIterations())));
removedParameters_.insert(std::make_pair("LccBow/Force2D", std::make_pair(false, Parameters::kVisForce2D())));
removedParameters_.insert(std::make_pair("LccBow/VarianceFromInliersCount", std::make_pair(false, Parameters::kRegVarianceFromInliersCount())));
removedParameters_.insert(std::make_pair("LccBow/PnPReprojError", std::make_pair(false, Parameters::kVisPnPReprojError())));
removedParameters_.insert(std::make_pair("LccBow/PnPFlags", std::make_pair(false, Parameters::kVisPnPFlags())));
removedParameters_.insert(std::make_pair("LccBow/EpipolarGeometryVar", std::make_pair(true, Parameters::kVisEpipolarGeometryVar())));
removedParameters_.insert(std::make_pair("LccIcp/Type", std::make_pair(true, Parameters::kRGBDIcpLoopClosureRefining())));
removedParameters_.insert(std::make_pair("LccIcp3/Decimation", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("LccIcp3/MaxDepth", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("LccIcp3/VoxelSize", std::make_pair(false, Parameters::kIcpVoxelSize())));
removedParameters_.insert(std::make_pair("LccIcp3/Samples", std::make_pair(false, Parameters::kIcpDownsamplingStep())));
removedParameters_.insert(std::make_pair("LccIcp3/MaxCorrespondenceDistance", std::make_pair(false, Parameters::kIcpMaxCorrespondenceDistance())));
removedParameters_.insert(std::make_pair("LccIcp3/Iterations", std::make_pair(false, Parameters::kIcpIterations())));
removedParameters_.insert(std::make_pair("LccIcp3/CorrespondenceRatio", std::make_pair(false, Parameters::kIcpCorrespondenceRatio())));
removedParameters_.insert(std::make_pair("LccIcp3/PointToPlane", std::make_pair(true, Parameters::kIcpPointToPlane())));
removedParameters_.insert(std::make_pair("LccIcp3/PointToPlaneNormalNeighbors", std::make_pair(true, Parameters::kIcpPointToPlaneNormalNeighbors())));
removedParameters_.insert(std::make_pair("LccIcp2/MaxCorrespondenceDistance", std::make_pair(true, Parameters::kIcpMaxCorrespondenceDistance())));
removedParameters_.insert(std::make_pair("LccIcp2/Iterations", std::make_pair(true, Parameters::kIcpIterations())));
removedParameters_.insert(std::make_pair("LccIcp2/CorrespondenceRatio", std::make_pair(true, Parameters::kIcpCorrespondenceRatio())));
removedParameters_.insert(std::make_pair("LccIcp2/VoxelSize", std::make_pair(true, Parameters::kIcpVoxelSize())));
}
return removedParameters_;
}
const ParametersMap & Parameters::getBackwardCompatibilityMap()
{
if(backwardCompatibilityMap_.empty())
{
getRemovedParameters(); // make sure removedParameters is filled
// compatibility
for(std::map<std::string, std::pair<bool, std::string> >::iterator iter=removedParameters_.begin();
iter!=removedParameters_.end();
++iter)
{
if(iter->second.first)
{
backwardCompatibilityMap_.insert(ParametersPair(iter->second.second, iter->first));
}
}
}
return backwardCompatibilityMap_;
}
std::string Parameters::getDescription(const std::string & paramKey)
{
std::string description;

366
corelib/src/RegistrationIcp.cpp Normal file
View File

@@ -0,0 +1,366 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/RegistrationIcp.h>
#include <rtabmap/core/util3d_registration.h>
#include <rtabmap/core/util3d_surface.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <pcl/io/pcd_io.h>
namespace rtabmap {
RegistrationIcp::RegistrationIcp(const ParametersMap & parameters) :
_icpMaxTranslation(Parameters::defaultIcpMaxTranslation()),
_icpMaxRotation(Parameters::defaultIcpMaxRotation()),
_icp2D(Parameters::defaultIcp2D()),
_icpVoxelSize(Parameters::defaultIcpVoxelSize()),
_icpDownsamplingStep(Parameters::defaultIcpDownsamplingStep()),
_icpMaxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()),
_icpMaxIterations(Parameters::defaultIcpIterations()),
_icpCorrespondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
_icpPointToPlane(Parameters::defaultIcpPointToPlane()),
_icpPointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors())
{
this->parseParameters(parameters);
}
void RegistrationIcp::parseParameters(const ParametersMap & parameters)
{
Registration::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kIcpMaxTranslation(), _icpMaxTranslation);
Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _icpMaxRotation);
Parameters::parse(parameters, Parameters::kIcp2D(), _icp2D);
Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _icpVoxelSize);
Parameters::parse(parameters, Parameters::kIcpDownsamplingStep(), _icpDownsamplingStep);
Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _icpMaxCorrespondenceDistance);
Parameters::parse(parameters, Parameters::kIcpIterations(), _icpMaxIterations);
Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _icpCorrespondenceRatio);
Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _icpPointToPlane);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneNormalNeighbors(), _icpPointToPlaneNormalNeighbors);
UASSERT_MSG(_icpVoxelSize >= 0, uFormat("value=%d", _icpVoxelSize).c_str());
UASSERT_MSG(_icpDownsamplingStep >= 0, uFormat("value=%d", _icpDownsamplingStep).c_str());
UASSERT_MSG(_icpMaxCorrespondenceDistance > 0.0f, uFormat("value=%f", _icpMaxCorrespondenceDistance).c_str());
UASSERT_MSG(_icpMaxIterations > 0, uFormat("value=%d", _icpMaxIterations).c_str());
UASSERT_MSG(_icpCorrespondenceRatio >=0.0f && _icpCorrespondenceRatio <=1.0f, uFormat("value=%f", _icpCorrespondenceRatio).c_str());
UASSERT_MSG(_icpPointToPlaneNormalNeighbors > 0, uFormat("value=%d", _icpPointToPlaneNormalNeighbors).c_str());
}
Transform RegistrationIcp::computeTransformation(
const Signature & fromSignature,
const Signature & toSignature,
Transform guess,
std::string * rejectedMsg,
int * inliersOut,
float * varianceOut,
float * inliersRatioOut)
{
return computeTransformation(
fromSignature.sensorData(),
toSignature.sensorData(),
guess,
rejectedMsg,
inliersOut,
varianceOut,
inliersRatioOut);
}
Transform RegistrationIcp::computeTransformation(
const SensorData & dataFrom,
const SensorData & dataTo,
Transform guess,
std::string * rejectedMsg,
int * inliersOut,
float * varianceOut,
float * inliersRatioOut)
{
UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
UDEBUG("Voxel size=%f", _icpVoxelSize);
UDEBUG("2D=%d", _icp2D?1:0);
UDEBUG("PointToPlane=%d", _icpPointToPlane?1:0);
UDEBUG("Normal neighborhood=%d", _icpPointToPlaneNormalNeighbors);
UDEBUG("Max corrrespondence distance=%f", _icpMaxCorrespondenceDistance);
UDEBUG("Max Iterations=%d", _icpMaxIterations);
UDEBUG("Variance from inliers count=%d", _bowVarianceFromInliersCount?1:0);
UDEBUG("Correspondence Ratio=%f", _icpCorrespondenceRatio);
UDEBUG("Max translation=%f", _icpMaxTranslation);
UDEBUG("Max rotation=%f", _icpMaxRotation);
UDEBUG("Downsampling step=%d", _icpDownsamplingStep);
std::string msg;
Transform transform;
// ICP with guess transform
if(!dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
{
int maxLaserScans = dataTo.laserScanMaxPts();
cv::Mat fromScan = dataFrom.laserScanRaw();
cv::Mat toScan = dataTo.laserScanRaw();
if(_icpDownsamplingStep>1)
{
fromScan = util3d::downsample(fromScan, _icpDownsamplingStep);
toScan = util3d::downsample(toScan, _icpDownsamplingStep);
maxLaserScans/=_icpDownsamplingStep;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, Transform());
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess);
if(toCloud->size() && fromCloud->size())
{
//filtering
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
bool filtered = false;
if(_icpVoxelSize > 0.0f)
{
fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _icpVoxelSize);
toCloudFiltered = util3d::voxelize(toCloudFiltered, _icpVoxelSize);
filtered = true;
}
Transform icpT;
bool hasConverged = false;
float correspondencesRatio = 0.0f;
int correspondences = 0;
double variance = 1.0;
bool correspondencesComputed = false;
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
if(!_icp2D) // 3D ICP
{
if(_icpPointToPlane)
{
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::computeNormals(fromCloudFiltered, _icpPointToPlaneNormalNeighbors);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::computeNormals(toCloudFiltered, _icpPointToPlaneNormalNeighbors);
std::vector<int> indices;
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
if(toCloudNormals->size() && fromCloudNormals->size())
{
pcl::PointCloud<pcl::PointNormal>::Ptr newCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
icpT = util3d::icpPointToPlane(
toCloudNormals,
fromCloudNormals,
_icpMaxCorrespondenceDistance,
_icpMaxIterations,
hasConverged,
*newCloudNormalsRegistered);
if(!filtered &&
!icpT.isNull() &&
hasConverged)
{
util3d::computeVarianceAndCorrespondences(
newCloudNormalsRegistered,
fromCloudNormals,
_icpMaxCorrespondenceDistance,
variance,
correspondences);
correspondencesComputed = true;
}
}
}
else
{
icpT = util3d::icp(
toCloudFiltered,
fromCloudFiltered,
_icpMaxCorrespondenceDistance,
_icpMaxIterations,
hasConverged,
*newCloudRegistered);
}
}
else // 2D ICP
{
icpT = util3d::icp2D(
toCloudFiltered,
fromCloudFiltered,
_icpMaxCorrespondenceDistance,
_icpMaxIterations,
hasConverged,
*newCloudRegistered);
}
/*pcl::io::savePCDFile("fromCloud.pcd", *fromCloud);
pcl::io::savePCDFile("toCloud.pcd", *toCloud);
UWARN("saved fromCloud.pcd and toCloud.pcd");
if(!icpT.isNull())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudTmp = util3d::transformPointCloud(toCloud, icpT);
pcl::io::savePCDFile("newCloudFinal.pcd", *toCloudTmp);
UWARN("saved toCloudFinal.pcd");
}*/
if(!icpT.isNull() &&
hasConverged)
{
float ix,iy,iz, iroll,ipitch,iyaw;
icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
if((_icpMaxTranslation>0.0f &&
(fabs(ix) > _icpMaxTranslation ||
fabs(iy) > _icpMaxTranslation ||
fabs(iz) > _icpMaxTranslation))
||
(_icpMaxRotation>0.0f &&
(fabs(iroll) > _icpMaxRotation ||
fabs(ipitch) > _icpMaxRotation ||
fabs(iyaw) > _icpMaxRotation)))
{
msg = uFormat("Cannot compute transform (ICP correction too large -> %f m %f rad, limits=%f m, %f rad)",
uMax3(fabs(ix), fabs(iy), fabs(iz)),
uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw)),
_icpMaxTranslation,
_icpMaxRotation);
UINFO(msg.c_str());
}
else
{
if(!correspondencesComputed)
{
if(filtered)
{
fromCloud = util3d::transformPointCloud(fromCloud, icpT);
}
else
{
fromCloud = newCloudRegistered;
}
util3d::computeVarianceAndCorrespondences(
toCloud,
fromCloud,
_icpMaxCorrespondenceDistance,
variance,
correspondences);
}
// verify if there are enough correspondences
if(maxLaserScans)
{
correspondencesRatio = float(correspondences)/float(maxLaserScans);
}
else
{
UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!",
dataTo.id());
correspondencesRatio = float(correspondences)/float(toCloud->size()>fromCloud->size()?toCloud->size():fromCloud->size());
}
UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
dataTo.id(), dataFrom.id(),
hasConverged?"true":"false",
variance,
correspondences,
maxLaserScans>0?maxLaserScans:dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():(int)(toCloud->size()>fromCloud->size()?toCloud->size():fromCloud->size()),
correspondencesRatio*100.0f);
if(_bowVarianceFromInliersCount)
{
variance = correspondencesRatio > 0?1.0/double(correspondencesRatio):1.0;
}
if(varianceOut)
{
*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
}
if(inliersOut)
{
*inliersOut = correspondences;
}
if(inliersRatioOut)
{
*inliersRatioOut = correspondencesRatio;
}
if(correspondencesRatio < _icpCorrespondenceRatio)
{
msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f)",
correspondences, correspondencesRatio, _icpCorrespondenceRatio);
UINFO(msg.c_str());
}
else
{
transform = guess*icpT;
}
}
}
else
{
msg = uFormat("Cannot compute transform (converged=%s var=%f)",
hasConverged?"true":"false", variance);
UINFO(msg.c_str());
}
// still compute the variance for information
/*if(variance == 1 && varianceOut)
{
util3d::computeVarianceAndCorrespondences(
toCloudFiltered,
fromCloudFiltered,
_icpMaxCorrespondenceDistance,
variance,
correspondences);
if(variance > 0)
{
*varianceOut = variance;
}
}*/
}
else
{
msg = "Laser scans empty ?!?";
UWARN(msg.c_str());
}
}
else
{
msg = uFormat("Laser scans empty?!? (new[%d]=%d old[%d]=%d)",
dataTo.id(), dataTo.laserScanRaw().total(),
dataFrom.id(), dataFrom.laserScanRaw().total());
UERROR(msg.c_str());
}
if(rejectedMsg)
{
*rejectedMsg = msg;
}
UDEBUG("New transform = %s", transform.prettyPrint().c_str());
return transform;
}
}

384
corelib/src/RegistrationVis.cpp Normal file
View File

@@ -0,0 +1,384 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/RegistrationVis.h>
#include <rtabmap/core/util3d_motion_estimation.h>
#include <rtabmap/core/util3d_features.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
namespace rtabmap {
RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
_bowMinInliers(Parameters::defaultVisMinInliers()),
_bowInlierDistance(Parameters::defaultVisInlierDistance()),
_bowIterations(Parameters::defaultVisIterations()),
_bowRefineIterations(Parameters::defaultVisRefineIterations()),
_bowForce2D(Parameters::defaultVisForce2D()),
_bowEpipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
_bowEstimationType(Parameters::defaultVisEstimationType()),
_bowPnPReprojError(Parameters::defaultVisPnPReprojError()),
_bowPnPFlags(Parameters::defaultVisPnPFlags()),
_reextractNNType(Parameters::defaultVisNNType()),
_reextractNNDR(Parameters::defaultVisNNDR()),
_reextractFeatureType(Parameters::defaultVisFeatureType()),
_reextractMaxWords(Parameters::defaultVisMaxFeatures()),
_reextractMaxDepth(Parameters::defaultVisMaxDepth()),
_reextractMinDepth(Parameters::defaultVisMinDepth()),
_reextractRoiRatios(Parameters::defaultVisRoiRatios()),
_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
_subPixIterations(Parameters::defaultKpSubPixIterations()),
_subPixEps(Parameters::defaultKpSubPixEps())
{
this->parseParameters(parameters);
}
void RegistrationVis::parseParameters(const ParametersMap & parameters)
{
Registration::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kVisMinInliers(), _bowMinInliers);
Parameters::parse(parameters, Parameters::kVisInlierDistance(), _bowInlierDistance);
Parameters::parse(parameters, Parameters::kVisIterations(), _bowIterations);
Parameters::parse(parameters, Parameters::kVisRefineIterations(), _bowRefineIterations);
Parameters::parse(parameters, Parameters::kVisForce2D(), _bowForce2D);
Parameters::parse(parameters, Parameters::kVisEstimationType(), _bowEstimationType);
Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _bowEpipolarGeometryVar);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _bowPnPReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _bowPnPFlags);
Parameters::parse(parameters, Parameters::kVisNNType(), _reextractNNType);
Parameters::parse(parameters, Parameters::kVisNNDR(), _reextractNNDR);
Parameters::parse(parameters, Parameters::kVisFeatureType(), _reextractFeatureType);
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), _reextractMaxWords);
Parameters::parse(parameters, Parameters::kVisMaxDepth(), _reextractMaxDepth);
Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
UASSERT_MSG(_bowMinInliers >= 1, uFormat("value=%d", _bowMinInliers).c_str());
UASSERT_MSG(_bowInlierDistance > 0.0f, uFormat("value=%f", _bowInlierDistance).c_str());
UASSERT_MSG(_bowIterations > 0, uFormat("value=%d", _bowIterations).c_str());
}
Transform RegistrationVis::computeTransformation(
const Signature & fromSignature,
const Signature & toSignature,
Transform guess, // guess is ignored for RegistrationVis
std::string * rejectedMsg,
int * inliersOut,
float * varianceOut,
float * inliersRatioOut)
{
Transform transform;
std::string msg;
// Guess transform from visual words
int inliersCount= 0;
double variance = 1.0;
// Extract features?
const std::multimap<int, cv::KeyPoint> * wordsFrom = 0;
const std::multimap<int, cv::KeyPoint> * wordsTo = 0;
const std::multimap<int, pcl::PointXYZ> * words3From = 0;
const std::multimap<int, pcl::PointXYZ> * words3To = 0;
std::multimap<int, cv::KeyPoint> extractedWordsFrom, extractedWordsTo;
std::multimap<int, pcl::PointXYZ> extractedWords3From, extractedWords3To;
if(fromSignature.getWords().size() == 0 && toSignature.getWords().size() == 0)
{
// Use the Memory class to extract features
ParametersMap customParameters;
// override some parameters
uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
uInsert(customParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_reextractMaxDepth)));
uInsert(customParameters, ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(_reextractMinDepth)));
uInsert(customParameters, ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(_subPixEps)));
uInsert(customParameters, ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(_subPixIterations)));
uInsert(customParameters, ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(_subPixWinSize)));
uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), _reextractRoiRatios));
uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "true"));
Memory memory(customParameters);
// Add signatures
SensorData dataFrom = fromSignature.sensorData();
SensorData dataTo = toSignature.sensorData();
// make sure there are no features already in the SensorData
dataFrom.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
dataTo.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
UTimer timeT;
memory.update(dataFrom);
if(memory.getLastWorkingSignature() == 0)
{
UWARN("Failed to extract features for node %d", dataFrom.id());
}
else
{
extractedWordsFrom = memory.getLastWorkingSignature()->getWords();
extractedWords3From = memory.getLastWorkingSignature()->getWords3();
UDEBUG("timeTo = %fs", timeT.ticks());
memory.update(dataTo);
if(memory.getLastWorkingSignature() == 0)
{
UWARN("Failed to extract features for node %d", dataTo.id());
}
else
{
extractedWordsTo = memory.getLastWorkingSignature()->getWords();
extractedWords3To = memory.getLastWorkingSignature()->getWords3();
UDEBUG("timeFrom = %fs", timeT.ticks());
}
}
wordsFrom = &extractedWordsFrom;
wordsTo = &extractedWordsTo;
words3From = &extractedWords3From;
words3To = &extractedWords3To;
}
else
{
wordsFrom = &fromSignature.getWords();
wordsTo = &toSignature.getWords();
words3From = &fromSignature.getWords3();
words3To = &toSignature.getWords3();
}
if(_bowEstimationType == 2) // Epipolar Geometry
{
if(!toSignature.sensorData().stereoCameraModel().isValid() &&
(toSignature.sensorData().cameraModels().size() != 1 ||
!toSignature.sensorData().cameraModels()[0].isValid()))
{
UERROR("Calibrated camera required (multi-cameras not supported).");
}
else if((int)wordsFrom->size() >= _bowMinInliers &&
(int)wordsTo->size() >= _bowMinInliers)
{
UASSERT(fromSignature.sensorData().stereoCameraModel().isValid() || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = fromSignature.sensorData().stereoCameraModel().isValid()?fromSignature.sensorData().stereoCameraModel().left():fromSignature.sensorData().cameraModels()[0];
// we only need the camera transform, send guess words3 for scale estimation
Transform cameraTransform;
std::multimap<int, pcl::PointXYZ> inliers3D = util3d::generateWords3DMono(
*wordsFrom,
*wordsTo,
cameraModel,
cameraTransform,
_bowIterations,
_bowPnPReprojError,
_bowPnPFlags, // cv::SOLVEPNP_ITERATIVE
1.0f,
0.99f,
*words3From, // for scale estimation
&variance);
inliersCount = (int)inliers3D.size();
if(!cameraTransform.isNull())
{
if((int)inliers3D.size() >= _bowMinInliers)
{
if(variance <= _bowEpipolarGeometryVar)
{
transform = cameraTransform;
}
else
{
msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _bowEpipolarGeometryVar, variance);
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _bowMinInliers);
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("No camera transform found");
UINFO(msg.c_str());
}
}
else if(words3From->size() == 0)
{
msg = uFormat("No 3D guess words found");
UWARN(msg.c_str());
}
else
{
msg = uFormat("No camera model");
UWARN(msg.c_str());
}
}
else if(_bowEstimationType == 1) // PnP
{
if(!toSignature.sensorData().stereoCameraModel().isValid() &&
(toSignature.sensorData().cameraModels().size() != 1 ||
!toSignature.sensorData().cameraModels()[0].isValid()))
{
UERROR("Calibrated camera required (multi-cameras not supported). Id=%d Models=%d StereoModel=%d weight=%d",
toSignature.id(),
(int)toSignature.sensorData().cameraModels().size(),
toSignature.sensorData().stereoCameraModel().isValid()?1:0,
toSignature.getWeight());
}
else
{
// 3D to 2D
if((int)words3From->size() >= _bowMinInliers &&
(int)wordsTo->size() >= _bowMinInliers)
{
UASSERT(toSignature.sensorData().stereoCameraModel().isValid() || (toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModel().isValid()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0];
std::vector<int> inliersV;
transform = util3d::estimateMotion3DTo2D(
uMultimapToMap(*words3From),
uMultimapToMap(*wordsTo),
cameraModel,
_bowMinInliers,
_bowIterations,
_bowPnPReprojError,
_bowPnPFlags,
Transform::getIdentity(),
uMultimapToMap(*words3To),
&variance,
0,
&inliersV);
inliersCount = (int)inliersV.size();
if(transform.isNull())
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
inliersCount, _bowMinInliers, fromSignature.id(), toSignature.id());
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
(int)words3From->size(), (int)wordsTo->size(), _bowMinInliers);
UINFO(msg.c_str());
}
}
}
else
{
// 3D -> 3D
if((int)words3From->size() >= _bowMinInliers &&
(int)words3To->size() >= _bowMinInliers)
{
std::vector<int> inliersV;
transform = util3d::estimateMotion3DTo3D(
uMultimapToMap(*words3From),
uMultimapToMap(*words3To),
_bowMinInliers,
_bowInlierDistance,
_bowIterations,
_bowRefineIterations,
&variance,
0,
&inliersV);
inliersCount = (int)inliersV.size();
if(transform.isNull())
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
inliersCount, _bowMinInliers, fromSignature.id(), toSignature.id());
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
(int)words3From->size(), (int)words3To->size(), _bowMinInliers);
UINFO(msg.c_str());
}
}
if(!transform.isNull())
{
// verify if it is a 180 degree transform, well verify > 90
float x,y,z, roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(fabs(roll) > CV_PI/2 ||
fabs(pitch) > CV_PI/2 ||
fabs(yaw) > CV_PI/2)
{
transform.setNull();
msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
roll, pitch, yaw);
UWARN(msg.c_str());
}
else if(_bowForce2D)
{
UDEBUG("Forcing 2D...");
transform = Transform(x,y,0, 0, 0, yaw);
}
}
if(_bowVarianceFromInliersCount)
{
variance = inliersCount > 0?1.0/double(inliersCount):1.0;
}
if(rejectedMsg)
{
*rejectedMsg = msg;
}
if(inliersOut)
{
*inliersOut = inliersCount;
}
if(varianceOut)
{
*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
}
UDEBUG("transform=%s", transform.prettyPrint().c_str());
return transform;
}
}

276
corelib/src/Rtabmap.cpp
View File

@@ -90,8 +90,8 @@ Rtabmap::Rtabmap() :
_rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()),
_rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
_newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
_globalLoopClosureIcpType(Parameters::defaultLccIcpType()),
_poseScanMatching(Parameters::defaultRGBDPoseScanMatching()),
_loopClosureIcpRefining(Parameters::defaultRGBDIcpLoopClosureRefining()),
_odomIcpRefining(Parameters::defaultRGBDIcpOdomRefining()),
_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
_scanMatchingIdsSavedInLinks(Parameters::defaultRGBDScanMatchingIdsSavedInLinks()),
@@ -100,15 +100,10 @@ Rtabmap::Rtabmap() :
_localDetectMaxGraphDepth(Parameters::defaultRGBDLocalLoopDetectionMaxGraphDepth()),
_localPathFilteringRadius(Parameters::defaultRGBDLocalLoopDetectionPathFilteringRadius()),
_localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()),
_localPathScansMerged(Parameters::defaultRGBDLocalLoopDetectionPathScansMerged()),
_databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
_optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()),
_reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
_reextractNNType(Parameters::defaultLccReextractNNType()),
_reextractNNDR(Parameters::defaultLccReextractNNDR()),
_reextractFeatureType(Parameters::defaultLccReextractFeatureType()),
_reextractMaxWords(Parameters::defaultLccReextractMaxWords()),
_reextractMaxDepth(Parameters::defaultLccReextractMaxDepth()),
_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
_goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
@@ -402,7 +397,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance);
Parameters::parse(parameters, Parameters::kRGBDPoseScanMatching(), _poseScanMatching);
Parameters::parse(parameters, Parameters::kRGBDIcpOdomRefining(), _odomIcpRefining);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
Parameters::parse(parameters, Parameters::kRGBDScanMatchingIdsSavedInLinks(), _scanMatchingIdsSavedInLinks);
@@ -411,38 +406,20 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxGraphDepth(), _localDetectMaxGraphDepth);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathScansMerged(), _localPathScansMerged);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError);
Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
Parameters::parse(parameters, Parameters::kLccReextractNNDR(), _reextractNNDR);
Parameters::parse(parameters, Parameters::kLccReextractFeatureType(), _reextractFeatureType);
Parameters::parse(parameters, Parameters::kLccReextractMaxWords(), _reextractMaxWords);
Parameters::parse(parameters, Parameters::kLccReextractMaxDepth(), _reextractMaxDepth);
Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
Parameters::parse(parameters, Parameters::kRGBDIcpLoopClosureRefining(), _loopClosureIcpRefining);
UASSERT(_rgbdLinearUpdate >= 0.0f);
UASSERT(_rgbdAngularUpdate >= 0.0f);
// RGB-D SLAM stuff
if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
{
int icpType = std::atoi((*iter).second.c_str());
if(icpType >= 0 && icpType <= 2)
{
_globalLoopClosureIcpType = icpType;
}
else
{
UERROR("Icp type must be 0, 1 or 2 (value=%d)", icpType);
}
}
// By default, we create our strategies if they are not already created.
// If they already exists, we check the parameters if a change is requested
@@ -1023,17 +1000,17 @@ bool Rtabmap::process(
//============================================================
// Scan matching
//============================================================
if(_poseScanMatching &&
if(_odomIcpRefining &&
!signature->sensorData().laserScanCompressed().empty() &&
rehearsedId == 0) // don't do it if rehearsal happened
{
UINFO("Odometry correction by scan matching");
Transform guess = signature->getLinks().begin()->second.transform();
double variance = 1.0;
Transform guess = signature->getLinks().begin()->second.transform().inverse();
float variance = 1.0f;
int inliers = 0;
float inliersRatio = 0;
std::string rejectedMsg;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &rejectedMsg, &inliers, &variance, &inliersRatio);
if(!t.isNull())
{
UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
@@ -1043,14 +1020,14 @@ bool Rtabmap::process(
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str());
UASSERT(variance > 0.0);
_memory->updateLink(signature->id(), oldId, t, variance, variance);
_memory->updateLink(oldId, signature->id(), t, variance, variance);
if(_optimizeFromGraphEnd)
{
// update all previous nodes
// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
// parameters just changed state, we should put back all poses without map correction.
Transform u = guess.inverse() * t;
Transform u = guess * t.inverse();
std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId);
UASSERT(jter!=_optimizedPoses.end());
Transform up = jter->second * u * jter->second.inverse();
@@ -1074,6 +1051,7 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers);
statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio);
statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance);
statistics_.addStatistic(Statistics::kOdomCorrectionPts(), signature->sensorData().laserScanRaw().cols);
}
timeScanMatching = timer.ticks();
ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
@@ -1179,12 +1157,12 @@ bool Rtabmap::process(
{
std::string rejectedMsg;
UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
double variance = 1.0;
float variance = 1.0f;
int inliers = -1;
Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg, &inliers, &variance);
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
Transform transform = _memory->computeVisualTransform(signature->id(), *iter, &rejectedMsg, &inliers, &variance);
if(!transform.isNull() && _loopClosureIcpRefining)
{
transform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg, 0, &variance);
transform = _memory->computeIcpTransform(signature->id(), *iter, transform, &rejectedMsg, 0, &variance);
}
if(!transform.isNull())
{
@@ -1733,72 +1711,16 @@ bool Rtabmap::process(
{
//Compute transform if metric data are present
Transform transform;
double variance = 1;
float variance = 1.0f;
if(_rgbdSlamMode)
{
std::string rejectedMsg;
if(_reextractLoopClosureFeatures)
transform = _memory->computeVisualTransform(signature->id(), _loopClosureHypothesis.first, &rejectedMsg, &loopClosureVisualInliers, &variance);
if(!transform.isNull() && _loopClosureIcpRefining)
{
ParametersMap customParameters = _modifiedParameters; // get BOW LCC parameters
// override some parameters
uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
uInsert(customParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_reextractMaxDepth)));
uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0"));
uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
//for(ParametersMap::iterator iter = customParameters.begin(); iter!=customParameters.end(); ++iter)
//{
// UDEBUG("%s=%s", iter->first.c_str(), iter->second.c_str());
//}
Memory memory(customParameters);
UTimer timeT;
// Add signatures
SensorData dataFrom = data;
dataFrom.setId(signature->id());
SensorData dataTo = _memory->getNodeData(_loopClosureHypothesis.first, true);
UDEBUG("timeTo = %fs", timeT.ticks());
if(!dataFrom.depthOrRightRaw().empty() &&
!dataTo.depthOrRightRaw().empty() &&
dataFrom.id() != Memory::kIdInvalid &&
dataTo.id() != Memory::kIdInvalid)
{
memory.update(dataTo);
UDEBUG("timeUpTo = %fs", timeT.ticks());
memory.update(dataFrom);
UDEBUG("timeUpFrom = %fs", timeT.ticks());
transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
UDEBUG("timeTransform = %fs", timeT.ticks());
}
else
{
// Fallback to normal way (raw data not kept in database...)
UWARN("Loop closure: Some images not found in memory for re-extracting "
"features, is Mem/RawDataKept=false? Falling back with already extracted 3D features.");
transform = _memory->computeVisualTransform(_loopClosureHypothesis.first, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
}
}
else
{
transform = _memory->computeVisualTransform(_loopClosureHypothesis.first, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
}
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
transform = _memory->computeIcpTransform(_loopClosureHypothesis.first, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg, 0, &variance);
transform = _memory->computeIcpTransform(signature->id(), _loopClosureHypothesis.first, transform, &rejectedMsg, 0, &variance);
}
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)
@@ -1896,70 +1818,11 @@ bool Rtabmap::process(
(_localPathFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius*_localPathFilteringRadius))
{
double variance = 1.0;
Transform transform;
if(_reextractLoopClosureFeatures)
float variance = 1.0f;
Transform transform = _memory->computeVisualTransform(signature->id(), nearestId, 0, 0, &variance);
if(!transform.isNull() && _loopClosureIcpRefining)
{
ParametersMap customParameters = _modifiedParameters; // get BOW LCC parameters
// override some parameters
uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
uInsert(customParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_reextractMaxDepth)));
uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0"));
uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
//for(ParametersMap::iterator iter = customParameters.begin(); iter!=customParameters.end(); ++iter)
//{
// UDEBUG("%s=%s", iter->first.c_str(), iter->second.c_str());
//}
Memory memory(customParameters);
UTimer timeT;
// Add signatures
SensorData dataFrom = data;
dataFrom.setId(signature->id());
SensorData dataTo = _memory->getNodeData(nearestId, true);
UDEBUG("timeTo = %fs", timeT.ticks());
if(!dataFrom.depthOrRightRaw().empty() &&
!dataTo.depthOrRightRaw().empty() &&
dataFrom.id() != Memory::kIdInvalid &&
dataTo.id() != Memory::kIdInvalid)
{
memory.update(dataTo);
UDEBUG("timeUpTo = %fs", timeT.ticks());
memory.update(dataFrom);
UDEBUG("timeUpFrom = %fs", timeT.ticks());
transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), 0, 0, &variance);
UDEBUG("timeTransform = %fs", timeT.ticks());
}
else
{
// Fallback to normal way (raw data not kept in database...)
UWARN("Loop closure: Some images not found in memory for re-extracting "
"features, is Mem/RawDataKept=false? Falling back with already extracted 3D features.");
transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
}
}
else
{
transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
}
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
transform = _memory->computeIcpTransform(nearestId, signature->id(), transform, _globalLoopClosureIcpType == 1, 0, 0, &variance);
transform = _memory->computeIcpTransform(signature->id(), nearestId, transform, 0, 0, &variance);
}
if(!transform.isNull())
{
@@ -2019,38 +1882,48 @@ bool Rtabmap::process(
(_localPathFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius*_localPathFilteringRadius))
{
// Assemble scans in the path and do ICP only
if(_localPathOdomPosesUsed)
if(!_localPathScansMerged)
{
//optimize the path's poses locally
path = optimizeGraph(nearestId, uKeysSet(path), false);
// transform local poses in optimized graph referential
UASSERT(uContains(path, nearestId));
Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
//only keep the nearest node
std::map<int, Transform> tmp;
tmp.insert(*path.find(nearestId));
path = tmp;
}
else
{
// Assemble scans in the path and do ICP only
if(_localPathOdomPosesUsed)
{
jter->second = t * jter->second;
//optimize the path's poses locally
path = optimizeGraph(nearestId, uKeysSet(path), false);
// transform local poses in optimized graph referential
UASSERT(uContains(path, nearestId));
Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
{
jter->second = t * jter->second;
}
}
if(path.size() > 2 && _localPathFilteringRadius > 0.0f)
{
// path filtering
std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, 0, true);
// make sure the nearest and farthest poses are still here
filteredPath.insert(*path.find(nearestId));
filteredPath.insert(*path.begin());
filteredPath.insert(*path.rbegin());
path = filteredPath;
}
}
if(_localPathFilteringRadius > 0.0f)
{
// path filtering
std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, 0, true);
// make sure the nearest and farthest poses are still here
filteredPath.insert(*path.find(nearestId));
filteredPath.insert(*path.begin());
filteredPath.insert(*path.rbegin());
path = filteredPath;
}
if(path.size() > 2) // more than current+nearest
if(path.size() > 0)
{
// add current node to poses
path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
//The nearest will be the reference for a loop closure transform
if(signature->getLinks().find(nearestId) == signature->getLinks().end())
{
double variance = 1.0;
float variance = 1.0f;
Transform transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, &variance);
if(!transform.isNull())
{
@@ -2340,7 +2213,7 @@ bool Rtabmap::process(
// timings...
statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
statistics_.addStatistic(Statistics::kTimingScan_matching(), timeScanMatching*1000);
statistics_.addStatistic(Statistics::kTimingOdom_correction(), timeScanMatching*1000);
statistics_.addStatistic(Statistics::kTimingLocal_detection_TIME(), timeLocalTimeDetection*1000);
statistics_.addStatistic(Statistics::kTimingLocal_detection_SPACE(), timeLocalSpaceDetection*1000);
statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000);
@@ -3836,12 +3709,41 @@ void Rtabmap::readParameters(const std::string & configFile, ParametersMap & par
else
{
key = uReplaceChar(key, '\\', '/'); // Ini files use \ by default for separators, so replace them
// look for old parameter name
bool addParameter = true;
std::map<std::string, std::pair<bool, std::string> >::const_iterator oldIter = Parameters::getRemovedParameters().find(key);
if(oldIter!=Parameters::getRemovedParameters().end())
{
addParameter = oldIter->second.first;
if(addParameter)
{
key = oldIter->second.second;
UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s).",
oldIter->first.c_str(), oldIter->second.second.c_str(), iter->second);
}
else if(oldIter->second.second.empty())
{
UWARN("Parameter \"%s\" doesn't exist anymore.",
oldIter->first.c_str());
}
else
{
UWARN("Parameter \"%s\" doesn't exist anymore, you may want to use this similar parameter \"%s\":\"%s\".",
oldIter->first.c_str(), oldIter->second.second.c_str(), Parameters::getDescription(oldIter->second.second).c_str());
}
}
ParametersMap::iterator jter = parameters.find(key);
if(jter != parameters.end())
{
parameters.erase(jter);
}
parameters.insert(ParametersPair(key, (*iter).second));
if(addParameter)
{
parameters.insert(ParametersPair(key, iter->second));
}
}
}
}

10
corelib/src/RtabmapThread.cpp
View File

@@ -553,12 +553,10 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
}
if(ignoreFrame)
{
// remove data from the frame, keeping only constraints
SensorData tmp(
cv::Mat(),
odomEvent.data().id(),
odomEvent.data().stamp(),
odomEvent.data().userDataRaw());
// set negative id so rtabmap will detect it as an intermediate node
SensorData tmp = odomEvent.data();
tmp.setId(-1);
tmp.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());// remove features
_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance));
}
else

6
corelib/src/SensorData.cpp
View File

@@ -199,7 +199,7 @@ SensorData::SensorData(
_depthOrRightRaw = depth;
}
if(laserScan.type() == CV_32FC2)
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3)
{
_laserScanRaw = laserScan;
}
@@ -306,7 +306,7 @@ SensorData::SensorData(
_depthOrRightRaw = depth;
}
if(laserScan.type() == CV_32FC2)
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3)
{
_laserScanRaw = laserScan;
}
@@ -412,7 +412,7 @@ SensorData::SensorData(
_depthOrRightRaw = right;
}
if(laserScan.type() == CV_32FC2)
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3)
{
_laserScanRaw = laserScan;
}

42
corelib/src/Transform.cpp
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <iomanip>
namespace rtabmap {
@@ -315,4 +316,45 @@ Transform Transform::fromEigen3d(const Eigen::Isometry3d & matrix)
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
/**
* Format (6 values): x y z roll pitch yaw.
* Format (9 [+3] values): r11 r12 r13 r21 r22 r23 r31 r32 r33 [tx ty tz].
*/
Transform Transform::fromString(const std::string & string)
{
Transform t;
std::list<std::string> list = uSplit(string, ' ');
if(list.size() == 6 || list.size() == 9 || list.size() == 12)
{
std::vector<float> numbers(list.size());
int i = 0;
for(std::list<std::string>::iterator iter=list.begin(); iter!=list.end(); ++iter)
{
numbers[i++] = uStr2Float(*iter);
}
if(numbers.size() == 6)
{
t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5]);
}
else if(numbers.size() == 9)
{
t = Transform(numbers[0], numbers[1], numbers[2], 0,
numbers[3], numbers[4], numbers[5], 0,
numbers[6], numbers[7], numbers[8], 0);
}
else if(numbers.size() == 12)
{
t = Transform(numbers[0], numbers[1], numbers[2], numbers[9],
numbers[3], numbers[4], numbers[5], numbers[10],
numbers[6], numbers[7], numbers[8], numbers[11]);
}
}
else
{
UERROR("Local transform is wrong! must have 6 or 9 items (%s)", string.c_str());
}
return t;
}
}

119
corelib/src/util3d.cpp
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UFile.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>
#include <opencv2/imgproc/imgproc.hpp>
@@ -550,7 +551,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
if(tmp->size() && samples)
{
tmp = util3d::sampling(tmp, samples);
tmp = util3d::randomSampling(tmp, samples);
filtered = true;
}
@@ -685,7 +686,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
if(tmp->size() && samples)
{
tmp = util3d::sampling(tmp, samples);
tmp = util3d::randomSampling(tmp, samples);
filtered = true;
}
@@ -789,64 +790,60 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
return scan;
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
bool nullTransform = transform.isNull();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(unsigned int i=0; i<cloud.size(); ++i)
{
laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
if(!nullTransform)
{
pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
laserScan.at<cv::Vec3f>(i)[0] = pt.x;
laserScan.at<cv::Vec3f>(i)[1] = pt.y;
laserScan.at<cv::Vec3f>(i)[2] = pt.z;
}
else
{
laserScan.at<cv::Vec3f>(i)[0] = cloud.at(i).x;
laserScan.at<cv::Vec3f>(i)[1] = cloud.at(i).y;
laserScan.at<cv::Vec3f>(i)[2] = cloud.at(i).z;
}
}
return laserScan;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
output->resize(laserScan.cols);
bool nullTransform = transform.isNull();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.cols; ++i)
{
output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
if(laserScan.type() == CV_32FC2)
{
output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
}
else
{
output->at(i).x = laserScan.at<cv::Vec3f>(i)[0];
output->at(i).y = laserScan.at<cv::Vec3f>(i)[1];
output->at(i).z = laserScan.at<cv::Vec3f>(i)[2];
}
if(!nullTransform)
{
output->at(i) = pcl::transformPoint(output->at(i), transform3f);
}
}
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
const cv::Mat & matrix,
const Transform & tranform)
{
UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
UASSERT(matrix.rows == 1);
Eigen::Affine3f t = tranform.toEigen3f();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(matrix.cols);
if(matrix.channels() == 2)
{
for(int i=0; i<matrix.cols; ++i)
{
cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
cloud->at(i).z = 0.0f;
cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
}
}
else // channels=3
{
for(int i=0; i<matrix.cols; ++i)
{
cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
}
}
return cloud;
}
// inspired from ROS image_geometry/src/stereo_camera_model.cpp
pcl::PointXYZ projectDisparityTo3D(
const cv::Point2f & pt,
@@ -950,6 +947,42 @@ void savePCDWords(
}
}
pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, int dim)
{
UASSERT(dim > 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
long bytes = UFile::length(fileName);
if(bytes)
{
UASSERT(bytes % sizeof(float) == 0);
int32_t num = bytes/sizeof(float);
UASSERT(num % dim == 0);
float *data = (float*)malloc(num*sizeof(float));
// pointers
float *px = data+0;
float *py = data+1;
float *pz = data+2;
float *pr = data+3;
// load point cloud
FILE *stream;
stream = fopen (fileName.c_str(),"rb");
num = fread(data,sizeof(float),num,stream)/4;
cloud->resize(num);
for (int32_t i=0; i<num; i++) {
(*cloud)[i].x = *px;
(*cloud)[i].y = *py;
(*cloud)[i].z = *pz;
px+=4; py+=4; pz+=4; pr+=4;
}
fclose(stream);
}
return cloud;
}
}
}

99
corelib/src/util3d_filtering.cpp
View File

@@ -49,6 +49,101 @@ namespace rtabmap
namespace util3d
{
cv::Mat downsample(
const cv::Mat & cloud,
int step)
{
// 2D or 3D point clouds (laser scans)
UASSERT(cloud.type() == CV_32FC2 || cloud.type() == CV_32FC3);
UASSERT(step > 0);
cv::Mat output;
if(step == 1)
{
// no sampling
output = cloud.clone();
}
else
{
if(cloud.cols > step)
{
int finalSize = cloud.cols/step;
output = cv::Mat(1, finalSize, cloud.type());
int oi = 0;
for(unsigned int i=0; i<cloud.cols-step+1; i+=step)
{
cv::Mat(cloud, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
++oi;
}
}
else if(cloud.cols)
{
output = cv::Mat(1, 1, cloud.type());
cv::Mat(cloud, cv::Range::all(), cv::Range(0,1)).copyTo(output); // first point
}
}
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr downsample(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int step)
{
UASSERT(step > 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
if(step == 1)
{
// no sampling
*output = *cloud;
}
else
{
if(cloud->size() > step)
{
int finalSize = cloud->size()/step;
output->resize(finalSize);
int oi = 0;
for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
{
(*output)[oi++] = cloud->at(i);
}
}
else if(cloud->size())
{
output->push_back(cloud->at(0));
}
}
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int step)
{
UASSERT(step > 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
if(step == 1)
{
// no sampling
*output = *cloud;
}
else
{
if(cloud->size() > step)
{
int finalSize = cloud->size()/step;
output->resize(finalSize);
int oi = 0;
for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
{
(*output)[oi++] = cloud->at(i);
}
}
else if(cloud->size())
{
output->push_back(cloud->at(0));
}
}
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float voxelSize)
@@ -87,7 +182,7 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
}
pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
pcl::PointCloud<pcl::PointXYZ>::Ptr randomSampling(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int samples)
{
UASSERT(samples > 0);
@@ -98,7 +193,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr sampling(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr randomSampling(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int samples)
{
UASSERT(samples > 0);

9
corelib/src/util3d_registration.cpp
View File

@@ -306,7 +306,7 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (maximumIterations);
// Set the transformation epsilon (criterion 2)
//icp.setTransformationEpsilon (transformationEpsilon);
//icp.setTransformationEpsilon (1e-8);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
@@ -340,7 +340,7 @@ Transform icpPointToPlane(
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (maximumIterations);
// Set the transformation epsilon (criterion 2)
//icp.setTransformationEpsilon (transformationEpsilon);
//icp.setTransformationEpsilon (1e-8);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
@@ -373,7 +373,7 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (maximumIterations);
// Set the transformation epsilon (criterion 2)
//icp.setTransformationEpsilon (transformationEpsilon);
//icp.setTransformationEpsilon (1e-8);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
@@ -423,7 +423,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
}
else if(samples>0 && (int)cloud->size() > samples)
{
cloud = sampling(cloud, samples);
cloud = randomSampling(cloud, samples);
}
if(cloud->size())
@@ -439,7 +439,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
return cloud;
}
}
}