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

OdomF2M: added support to laser scan

Browse Source
This commit is contained in:
matlabbe
2016-03-06 15:11:09 -05:00
parent eefd557ab4
commit 7a1cf84b08
22 changed files with 696 additions and 226 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/OdometryF2F.h
View File

@@ -51,6 +51,7 @@ private:
private:
//Parameters:
float keyFrameThr_;
float scanKeyFrameThr_;
Registration * registrationPipeline_;
Signature refFrame_;

10
corelib/include/rtabmap/core/OdometryF2M.h
View File

@@ -29,11 +29,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define ODOMETRYF2M_H_
#include <rtabmap/core/Odometry.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
namespace rtabmap {
class Signature;
class RegistrationVis;
class Registration;
class RTABMAP_EXP OdometryF2M : public Odometry
{
@@ -53,11 +55,15 @@ private:
int maximumMapSize_;
float keyFrameThr_;
int maxNewFeatures_;
float scanKeyFrameThr_;
int scanMaximumMapSize_;
float scanSubstractRadius_;
std::string fixedMapPath_;
RegistrationVis * regVis_;
Registration * regPipeline_;
Signature * map_;
Signature * lastFrame_;
std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr > scansBuffer_;
};
}

10
corelib/include/rtabmap/core/OdometryInfo.h
View File

@@ -45,6 +45,7 @@ public:
variance(0.0f),
features(0),
localMapSize(0),
localScanMapSize(0),
timeEstimation(0.0f),
timeParticleFiltering(0.0f),
stamp(0),
@@ -63,6 +64,7 @@ public:
output.variance = variance;
output.features = features;
output.localMapSize = localMapSize;
output.localScanMapSize = localScanMapSize;
output.timeEstimation = timeEstimation;
output.timeParticleFiltering = timeParticleFiltering;
output.stamp = stamp;
@@ -80,6 +82,7 @@ public:
float variance;
int features;
int localMapSize;
int localScanMapSize;
float timeEstimation;
float timeParticleFiltering;
double stamp;
@@ -89,15 +92,16 @@ public:
Transform transformGroundTruth;
float distanceTravelled;
int type; // 0=BOW, 1=F2F, 2=ICP, 3=Mono
int type; // 0=F2M, 1=F2F
// BOW
// F2M
std::multimap<int, cv::KeyPoint> words;
std::vector<int> wordMatches;
std::vector<int> wordInliers;
std::map<int, cv::Point3f> localMap;
cv::Mat localScanMap;
// F2F && Mono
// F2F
std::vector<cv::Point2f> refCorners;
std::vector<cv::Point2f> newCorners;
std::vector<int> cornerInliers;

11
corelib/include/rtabmap/core/Parameters.h
View File

@@ -360,12 +360,15 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Odom, KalmanProcessNoise, float, 0.001, "Process noise covariance value.");
RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01, "Process measurement covariance value.");
RTABMAP_PARAM(Odom, GuessMotion, bool, true, "Guess next transformation from the last motion computed.");
RTABMAP_PARAM(Odom, KeyFrameThr, float, 0.5, "Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(Odom, KeyFrameThr, float, 0.3, "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(Odom, ScanKeyFrameThr, float, 0.7, "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
// Odometry Bag-of-words
RTABMAP_PARAM(OdomF2M, MaxSize, int, 1000, "Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
RTABMAP_PARAM(OdomF2M, MaxNewFeatures, int, 0, "Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
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(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, 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_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
RTABMAP_PARAM(OdomMono, InitMinFlow, float, 100, "Minimum optical flow required for the initialization step.");

30
corelib/include/rtabmap/core/util3d_filtering.h
View File

@@ -233,6 +233,34 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
float radiusSearch,
int minNeighborsInRadius = 1);
/**
* For convenience.
*/
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
float radiusSearch,
float maxAngle = M_PI/4.0f,
int minNeighborsInRadius = 1);
/**
* Subtract a cloud from another one using radius filtering.
* @param cloud the input cloud.
* @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
* @param cloud the input cloud to subtract.
* @param indices the input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
* @param radiusSearch the radius in meter.
* @return the indices of the points satisfying the parameters.
*/
pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
float radiusSearch,
float maxAngle = M_PI/4.0f,
int minNeighborsInRadius = 1);
/**
* For convenience.
*/
@@ -240,7 +268,7 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
float radiusSearch,
float maxAngle,
float maxAngle = M_PI/4.0f,
int minNeighborsInRadius = 1);
/**

6
corelib/include/rtabmap/core/util3d_registration.h
View File

@@ -79,7 +79,7 @@ Transform RTABMAP_EXP icp(
int maximumIterations,
bool & hasConverged,
pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
double epsilon = 0,
float epsilon = 0.0f,
bool icp2D = false);
Transform RTABMAP_EXP icpPointToPlane(
@@ -88,7 +88,9 @@ Transform RTABMAP_EXP icpPointToPlane(
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
pcl::PointCloud<pcl::PointNormal> & cloud_source_registered);
pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
float epsilon = 0.0f,
bool icp2D = false);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
const cv::Mat & depth,

3
corelib/include/rtabmap/core/util3d_transforms.h
View File

@@ -62,6 +62,9 @@ pcl::PointXYZ RTABMAP_EXP transformPoint(
pcl::PointXYZRGB RTABMAP_EXP transformPoint(
const pcl::PointXYZRGB & pt,
const Transform & transform);
pcl::PointNormal RTABMAP_EXP transformPoint(
const pcl::PointNormal & point,
const Transform & transform);
} // namespace util3d
} // namespace rtabmap

22
corelib/src/CameraThread.cpp
View File

@@ -32,11 +32,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/StereoDense.h"
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/ULogger.h>
#include <pcl/io/io.h>
namespace rtabmap
{
@@ -170,7 +173,22 @@ void CameraThread::mainLoop()
{
UASSERT(_scanDecimation >= 1);
UTimer timer;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth, _scanVoxelSize);
pcl::IndicesPtr validIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth, 0.0f, 0, validIndices.get());
float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
if(_scanVoxelSize>0.0f)
{
cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
float ratio = float(cloud->size()) / float(validIndices->size());
maxPoints = ratio * maxPoints;
}
else if(!cloud->is_dense)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
cloud = denseCloud;
}
cv::Mat scan;
if(_scanNormalsK>0)
{
@@ -180,7 +198,7 @@ void CameraThread::mainLoop()
{
scan = util3d::laserScanFromPointCloud(*cloud);
}
data.setLaserScanRaw(scan, (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation), _scanMaxDepth);
data.setLaserScanRaw(scan, (int)maxPoints, _scanMaxDepth);
info.timeScanFromDepth = timer.ticks();
UINFO("Computing scan from depth = %f s", info.timeScanFromDepth);
}

2
corelib/src/Memory.cpp
View File

@@ -3107,7 +3107,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
std::vector<cv::Point3f> keypoints3D;
if(!_useOdometryFeatures || (data.keypoints().size() != data.descriptors().rows))
if(!_useOdometryFeatures || data.keypoints().empty() || data.keypoints().size() != data.descriptors().rows)
{
if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
{

2
corelib/src/Odometry.cpp
View File

@@ -196,7 +196,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
}
double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
Transform guess;
Transform guess = dt?Transform::getIdentity():Transform();
UASSERT(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull()));
if(!previousVelocityTransform_.isNull())
{

6
corelib/src/OdometryF2F.cpp
View File

@@ -39,11 +39,14 @@ namespace rtabmap {
OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
motionSinceLastKeyFrame_(Transform::getIdentity())
{
registrationPipeline_ = Registration::create(parameters);
Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
UASSERT(keyFrameThr_>=0.0f && keyFrameThr_<=1.0f);
UASSERT(scanKeyFrameThr_>=0.0f && scanKeyFrameThr_<=1.0f);
}
OdometryF2F::~OdometryF2F()
@@ -136,7 +139,8 @@ Transform OdometryF2F::computeTransform(
motionSinceLastKeyFrame_ *= output;
// new key-frame?
if(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()))
if( (registrationPipeline_->isImageRequired() && (keyFrameThr_ == 0 || float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()))) ||
(registrationPipeline_->isScanRequired() && (scanKeyFrameThr_ == 0 || regInfo.icpInliersRatio <= scanKeyFrameThr_)))
{
UDEBUG("Update key frame");
int features = newFrame.getWordsDescriptors().size();

275
corelib/src/OdometryF2M.cpp
View File

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/Optimizer.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/util3d.h"
@@ -57,8 +58,11 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
maximumMapSize_(Parameters::defaultOdomF2MMaxSize()),
keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
scanSubstractRadius_(Parameters::defaultOdomF2MScanSubstractRadius()),
fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
regVis_(new RegistrationVis(parameters)),
regPipeline_(Registration::create(parameters)),
map_(new Signature(-1)),
lastFrame_(new Signature(1))
{
@@ -66,9 +70,13 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_);
Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
Parameters::parse(parameters, Parameters::kOdomF2MScanSubstractRadius(), scanSubstractRadius_);
Parameters::parse(parameters, Parameters::kOdomF2MFixedMapPath(), fixedMapPath_);
UASSERT(maximumMapSize_ >= 0);
UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
UASSERT(maxNewFeatures_ >= 0);
if(!fixedMapPath_.empty())
@@ -142,8 +150,9 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
UERROR("No pose loaded from database \"%s\"", fixedMapPath_.c_str());
}
}
if((int)map_->getWords3().size() < regVis_->getMinInliers() || map_->getWords3().size() == 0)
if((int)map_->getWords3().size() < regPipeline_->getMinVisualCorrespondences() || map_->getWords3().size() == 0)
{
// TODO: support geometric-only maps?
UERROR("The loaded fixed map from \"%s\" is too small! Only %d unique features loaded. Odometry won't be computed!",
fixedMapPath_.c_str(), (int)map_->getWords3().size());
}
@@ -195,10 +204,11 @@ Transform OdometryF2M::computeTransform(
// Generate keypoints from the new data
if(lastFrame_->sensorData().isValid())
{
if(map_->getWords3().size() && lastFrame_->sensorData().isValid())
if((map_->getWords3().size() || !map_->sensorData().laserScanRaw().empty()) &&
lastFrame_->sensorData().isValid())
{
Signature tmpMap = *map_;
Transform transform = regVis_->computeTransformationMod(
Transform transform = regPipeline_->computeTransformationMod(
tmpMap,
*lastFrame_,
guess.isNull()?Transform():this->getPose()*guess,
@@ -222,134 +232,230 @@ Transform OdometryF2M::computeTransform(
if(!transform.isNull())
{
if(fixedMapPath_.empty() &&
(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())))
{
output = transform;
output = transform;
if(fixedMapPath_.empty())
{
bool modified = false;
Transform newFramePose = this->getPose()*output;
// fields to update
cv::Mat mapScan = tmpMap.sensorData().laserScanRaw();
std::multimap<int, cv::Point3f> mapPoints = tmpMap.getWords3();
std::multimap<int, cv::Mat> mapDescriptors = tmpMap.getWordsDescriptors();
//Visual
int added = 0;
int removed = 0;
// update local map
*map_ = tmpMap;
std::multimap<int, cv::Point3f> mapPoints = map_->getWords3();
std::multimap<int, cv::Mat> mapDescriptors = map_->getWordsDescriptors();
Transform t = this->getPose()*output;
UASSERT(mapPoints.size() == mapDescriptors.size());
UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
// sort by feature response
std::multimap<float, std::pair<int, cv::Point3f> > newIds;
int lastId = 0;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().begin();
for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D)
UDEBUG("keyframeThr=%f inliers=%d features=%d", keyFrameThr_, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size());
if(regPipeline_->isImageRequired() &&
(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())))
{
if(iter == lastFrame_->getWords3().begin() ||
(iter != lastFrame_->getWords3().begin() && lastId != iter->first))
{
newIds.insert(std::make_pair(iter2D->second.response, std::make_pair(iter->first, iter->second)));
lastId = iter->first;
}
}
UDEBUG("Update local map");
for(std::multimap<float, std::pair<int, cv::Point3f> >::reverse_iterator iter=newIds.rbegin(); iter!=newIds.rend(); ++iter)
{
if(maxNewFeatures_ == 0 || added < maxNewFeatures_)
// update local map
UASSERT(mapPoints.size() == mapDescriptors.size());
UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
// sort by feature response
std::multimap<float, std::pair<int, cv::Point3f> > newIds;
int lastId = 0;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().begin();
for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D)
{
if(mapPoints.find(iter->second.first) == mapPoints.end() && util3d::isFinite(iter->second.second))
if(iter == lastFrame_->getWords3().begin() ||
(iter != lastFrame_->getWords3().begin() && lastId != iter->first))
{
mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(iter->second.second, t)));
mapDescriptors.insert(std::make_pair(iter->second.first, lastFrame_->getWordsDescriptors().find(iter->second.first)->second));
++added;
newIds.insert(std::make_pair(iter2D->second.response, std::make_pair(iter->first, iter->second)));
lastId = iter->first;
}
}
for(std::multimap<float, std::pair<int, cv::Point3f> >::reverse_iterator iter=newIds.rbegin(); iter!=newIds.rend(); ++iter)
{
if(maxNewFeatures_ == 0 || added < maxNewFeatures_)
{
if(mapPoints.find(iter->second.first) == mapPoints.end() && util3d::isFinite(iter->second.second))
{
mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(iter->second.second, newFramePose)));
mapDescriptors.insert(std::make_pair(iter->second.first, lastFrame_->getWordsDescriptors().find(iter->second.first)->second));
++added;
}
}
}
// remove words in map if max size is reached
if((int)mapPoints.size() > maximumMapSize_)
{
// remove oldest first, keep matched features
std::set<int> matches(regInfo.matchesIDs.begin(), regInfo.matchesIDs.end());
std::multimap<int, cv::Mat>::iterator iterMapWords = mapDescriptors.begin();
for(std::multimap<int, cv::Point3f>::iterator iter = mapPoints.begin();
iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();)
{
if(matches.find(iter->first) == matches.end())
{
iter = mapPoints.erase(iter);
iterMapWords = mapDescriptors.erase(iterMapWords);
++removed;
}
else
{
++iter;
++iterMapWords;
}
}
}
modified = true;
}
// remove words in map if max size is reached
if((int)mapPoints.size() > maximumMapSize_)
// Geometric
UDEBUG("scankeyframeThr=%f icpInliersRatio=%f", scanKeyFrameThr_, regInfo.icpInliersRatio);
if(regPipeline_->isScanRequired() &&
(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_))
{
// remove oldest first, keep matched features
std::set<int> matches(regInfo.matchesIDs.begin(), regInfo.matchesIDs.end());
std::multimap<int, cv::Mat>::iterator iterMapWords = mapDescriptors.begin();
for(std::multimap<int, cv::Point3f>::iterator iter = mapPoints.begin();
iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();)
UINFO("Update local scan map %d", lastFrame_->id());
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)
{
if(matches.find(iter->first) == matches.end())
frameCloudNormals = util3d::subtractFiltering(frameCloudNormals, mapCloudNormals, scanSubstractRadius_, 0.0f);
}
if(frameCloudNormals->size())
{
scansBuffer_.insert(std::make_pair(lastFrame_->id(), frameCloudNormals));
//remove points if too big
UDEBUG("scansBuffer=%d, mapSize=%d maxPoints=%d", (int)scansBuffer_.size(), int(mapCloudNormals->size() + frameCloudNormals->size()), scanMaximumMapSize_);
if(scansBuffer_.size() > 1 && int(mapCloudNormals->size() + frameCloudNormals->size()) > scanMaximumMapSize_)
{
iter = mapPoints.erase(iter);
iterMapWords = mapDescriptors.erase(iterMapWords);
++removed;
//asssemble
mapCloudNormals->clear();
std::list<int> toRemove;
for(std::map<int, pcl::PointCloud<pcl::PointNormal>::Ptr>::reverse_iterator iter=scansBuffer_.rbegin();
iter!=scansBuffer_.rend();
++iter)
{
if(mapCloudNormals->empty())
{
*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
{
++iter;
++iterMapWords;
//assemble
*mapCloudNormals += *frameCloudNormals;
}
mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
modified=true;
}
}
map_->setWords3(mapPoints);
map_->setWordsDescriptors(mapDescriptors);
if(modified)
{
*map_ = tmpMap;
UINFO("Updated map: %d added %d removed (new map size=%d)", added, removed, (int)mapPoints.size());
map_->sensorData().setLaserScanRaw(mapScan, 0, 0);
map_->setWords3(mapPoints);
map_->setWordsDescriptors(mapDescriptors);
UINFO("Updated map: %d added %d removed (new map size=%d)", added, removed, (int)mapPoints.size());
}
}
else
{
// fixed local map, don't update with the new signature
output = transform;
}
}
if(this->isInfoDataFilled())
if(info)
{
// use tmpMap instead of map_ to make sure that correspondences with the new frame matches
info->localMapSize = (int)tmpMap.getWords3().size();
info->localMap = uMultimapToMap(tmpMap.getWords3());
info->localScanMapSize = tmpMap.sensorData().laserScanRaw().cols;
if(this->isInfoDataFilled())
{
info->localMap = uMultimapToMap(tmpMap.getWords3());
info->localScanMap = tmpMap.sensorData().laserScanRaw();
}
}
}
else
{
// just generate keypoints for the new signature
Signature dummy;
regVis_->computeTransformationMod(
*lastFrame_,
dummy);
if(regPipeline_->isImageRequired())
{
Signature dummy;
regPipeline_->computeTransformationMod(
*lastFrame_,
dummy);
}
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());
if(fixedMapPath_.empty() && (int)lastFrame_->getWords3().size() >= regVis_->getMinInliers())
if(fixedMapPath_.empty())
{
output.setIdentity();
// a very high variance tells that the new pose is not linked with the previous one
regInfo.variance = 9999;
Transform t = this->getPose(); // initial pose may be not identity...
std::multimap<int, cv::Point3f> transformedPoints;
std::multimap<int, cv::Mat> descriptors;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
iter!=lastFrame_->getWords3().end();
++iter,++descIter)
Transform newFramePose = this->getPose(); // initial pose may be not identity...
if(regPipeline_->isImageRequired() &&
(int)lastFrame_->getWords3().size() >= regPipeline_->getMinVisualCorrespondences())
{
if(util3d::isFinite(iter->second))
std::multimap<int, cv::Point3f> transformedPoints;
std::multimap<int, cv::Mat> descriptors;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
iter!=lastFrame_->getWords3().end();
++iter,++descIter)
{
transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
descriptors.insert(std::make_pair(iter->first, descIter->second));
if(util3d::isFinite(iter->second))
{
transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
descriptors.insert(std::make_pair(iter->first, descIter->second));
}
}
map_->setWords3(transformedPoints);
map_->setWordsDescriptors(descriptors);
map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
}
if(regPipeline_->isScanRequired())
{
pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
scansBuffer_.insert(std::make_pair(lastFrame_->id(), mapCloudNormals));
map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), 0,0);
}
map_->setWords3(transformedPoints);
map_->setWordsDescriptors(descriptors);
map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
}
if(this->isInfoDataFilled())
if(info)
{
info->localMapSize = (int)map_->getWords3().size();
info->localMap = uMultimapToMap(map_->getWords3());
info->localScanMapSize = map_->sensorData().laserScanRaw().cols;
if(this->isInfoDataFilled())
{
info->localMap = uMultimapToMap(map_->getWords3());
info->localScanMap = map_->sensorData().laserScanRaw();
}
}
}
@@ -358,7 +464,10 @@ Transform OdometryF2M::computeTransform(
nFeatures = lastFrame_->getWords().size();
if(this->isInfoDataFilled() && info)
{
info->words = lastFrame_->getWords();
if(regPipeline_->isImageRequired())
{
info->words = lastFrame_->getWords();
}
}
}
@@ -367,6 +476,7 @@ Transform OdometryF2M::computeTransform(
info->variance = regInfo.variance;
info->inliers = regInfo.inliers;
info->matches = regInfo.matches;
info->icpInliersRatio = regInfo.icpInliersRatio;
info->features = nFeatures;
if(this->isInfoDataFilled())
@@ -376,14 +486,15 @@ Transform OdometryF2M::computeTransform(
}
}
UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance=%f local_map=%d",
UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance=%f local_map=%d local_scan_map=%d",
timer.elapsed(),
output.isNull()?"true":"false",
nFeatures,
regInfo.inliers,
regInfo.matches,
regInfo.variance,
(int)map_->getWords3().size());
regPipeline_->isImageRequired()?(int)map_->getWords3().size():0,
regPipeline_->isScanRequired()?(int)map_->sensorData().laserScanRaw().cols:0);
return output;
}

61
corelib/src/RegistrationIcp.cpp
View File

@@ -110,9 +110,9 @@ Transform RegistrationIcp::computeTransformationImpl(
dataTo.laserScanRaw().cols,
dataTo.laserScanRaw().channels());
// ICP with guess transform
if(!dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
if(!guess.isNull() && !dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
{
// ICP with guess transform
int maxLaserScans = dataTo.laserScanMaxPts();
cv::Mat fromScan = dataFrom.laserScanRaw();
cv::Mat toScan = dataTo.laserScanRaw();
@@ -132,8 +132,7 @@ Transform RegistrationIcp::computeTransformationImpl(
int correspondences = 0;
double variance = 1.0;
if( !force3DoF() &&
_pointToPlane &&
if( _pointToPlane &&
_voxelSize == 0.0f &&
fromScan.channels() == 6 &&
toScan.channels() == 6)
@@ -149,7 +148,9 @@ Transform RegistrationIcp::computeTransformationImpl(
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
*fromCloudNormalsRegistered);
*fromCloudNormalsRegistered,
_epsilon,
this->force3DoF());
if(!icpT.isNull() && hasConverged)
{
util3d::computeVarianceAndCorrespondences(
@@ -158,6 +159,20 @@ Transform RegistrationIcp::computeTransformationImpl(
_maxCorrespondenceDistance,
variance,
correspondences);
/*
UWARN("icpT=%s", icpT.prettyPrint().c_str());
pcl::io::savePCDFile("fromCloud.pcd", *fromCloudNormals);
pcl::io::savePCDFile("toCloud.pcd", *toCloudNormals);
UWARN("saved fromCloud.pcd and toCloud.pcd");
if(!icpT.isNull())
{
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudTmp = util3d::transformPointCloud(fromCloudNormals, icpT);
pcl::io::savePCDFile("fromCloudFinal.pcd", *fromCloudTmp);
pcl::io::savePCDFile("fromCloudFinal2.pcd", *fromCloudNormalsRegistered);
UWARN("saved fromCloudFinal.pcd");
}
*/
}
}
else
@@ -178,7 +193,7 @@ Transform RegistrationIcp::computeTransformationImpl(
bool correspondencesComputed = false;
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
if(!force3DoF() && _pointToPlane) // ICP Point To Plane, only in 3D
if(_pointToPlane) // ICP Point To Plane, only in 3D
{
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneNormalNeighbors);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::computeNormals(toCloudFiltered, _pointToPlaneNormalNeighbors);
@@ -198,7 +213,9 @@ Transform RegistrationIcp::computeTransformationImpl(
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
*fromCloudNormalsRegistered);
*fromCloudNormalsRegistered,
_epsilon,
this->force3DoF());
if(!filtered &&
!icpT.isNull() &&
hasConverged)
@@ -263,16 +280,13 @@ Transform RegistrationIcp::computeTransformationImpl(
hasConverged)
{
float ix,iy,iz, iroll,ipitch,iyaw;
icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
Transform icpInTargetReferential = guess.inverse() * icpT.inverse() * guess; // actual local ICP refinement
icpInTargetReferential.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
if((_maxTranslation>0.0f &&
(fabs(ix) > _maxTranslation ||
fabs(iy) > _maxTranslation ||
fabs(iz) > _maxTranslation))
||
(_maxRotation>0.0f &&
(fabs(iroll) > _maxRotation ||
fabs(ipitch) > _maxRotation ||
fabs(iyaw) > _maxRotation)))
uMax3(fabs(ix), fabs(iy), fabs(iz)) > _maxTranslation)
||
(_maxRotation>0.0f &&
uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw)) > _maxRotation))
{
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)),
@@ -331,11 +345,18 @@ Transform RegistrationIcp::computeTransformationImpl(
UWARN(msg.c_str());
}
}
else
else if(dataTo.isValid())
{
msg = uFormat("Laser scans empty?!? (new[%d]=%d old[%d]=%d)",
dataTo.id(), dataTo.laserScanRaw().total(),
dataFrom.id(), dataFrom.laserScanRaw().total());
if(guess.isNull())
{
msg = "RegistrationIcp cannot do registration with a null guess.";
}
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());
}

13
corelib/src/Transform.cpp
View File

@@ -243,9 +243,16 @@ Transform Transform::interpolate(float t, const Transform & other) const
std::string Transform::prettyPrint() const
{
float x,y,z,roll,pitch,yaw;
getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
return uFormat("xyz=%f,%f,%f rpy=%f,%f,%f", x,y,z, roll,pitch,yaw);
if(this->isNull())
{
return uFormat("xyz=[null] rpy=[null]");
}
else
{
float x,y,z,roll,pitch,yaw;
getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
return uFormat("xyz=%f,%f,%f rpy=%f,%f,%f", x,y,z, roll,pitch,yaw);
}
}
Transform Transform::operator*(const Transform & t) const

3
corelib/src/util3d.cpp
View File

@@ -1025,7 +1025,6 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat
pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
output->resize(laserScan.cols);
bool nullTransform = transform.isNull();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.cols; ++i)
{
if(laserScan.type() == CV_32FC2)
@@ -1051,7 +1050,7 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat
if(!nullTransform)
{
output->at(i) = pcl::transformPoint(output->at(i), transform3f);
output->at(i) = util3d::transformPoint(output->at(i), transform);
}
}
return output;

143
corelib/src/util3d_filtering.cpp
View File

@@ -673,6 +673,149 @@ pcl::IndicesPtr subtractFiltering(
}
}
pcl::PointCloud<pcl::PointNormal>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
float radiusSearch,
float maxAngle,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
pcl::PointCloud<pcl::PointNormal>::Ptr out(new pcl::PointCloud<pcl::PointNormal>);
pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out;
}
pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
float radiusSearch,
float maxAngle,
int minNeighborsInRadius)
{
UASSERT(minNeighborsInRadius > 0);
pcl::search::KdTree<pcl::PointNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointNormal>(false));
if(indices->size())
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
if(substractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
}
else
{
tree->setInputCloud(substractCloud);
}
for(unsigned int i=0; i<indices->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
if(k>=minNeighborsInRadius && maxAngle > 0.0f)
{
Eigen::Vector4f normal(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
if (uIsFinite(normal[0]) &&
uIsFinite(normal[1]) &&
uIsFinite(normal[2]))
{
int count = k;
for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
{
Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
if(uIsFinite(v[0]) &&
uIsFinite(v[1]) &&
uIsFinite(v[2]))
{
float angle = pcl::getAngle3D(normal, v);
if(angle > maxAngle)
{
k-=1;
}
}
else
{
k-=1;
}
}
}
else
{
k=0;
}
}
if(k < minNeighborsInRadius)
{
output->at(oi++) = indices->at(i);
}
}
output->resize(oi);
return output;
}
else
{
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator
if(substractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
}
else
{
tree->setInputCloud(substractCloud);
}
for(unsigned int i=0; i<cloud->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
if(k>=minNeighborsInRadius && maxAngle > 0.0f)
{
Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
if (uIsFinite(normal[0]) &&
uIsFinite(normal[1]) &&
uIsFinite(normal[2]))
{
int count = k;
for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
{
Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
if(uIsFinite(v[0]) &&
uIsFinite(v[1]) &&
uIsFinite(v[2]))
{
float angle = pcl::getAngle3D(normal, v);
if(angle > maxAngle)
{
k-=1;
}
}
else
{
k-=1;
}
}
}
else
{
k=0;
}
}
if(k < minNeighborsInRadius)
{
output->at(oi++) = i;
}
}
output->resize(oi);
return output;
}
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,

2
corelib/src/util3d_mapping.cpp
View File

@@ -401,7 +401,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
float minMapSize,
float scanMaxRange)
{
UDEBUG("poses=%d, scans = %d", poses.size(), scans.size());
UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange);
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans;
// For computation issue, the maximum scan range allowed is 6 meters

15
corelib/src/util3d_registration.cpp
View File

@@ -306,7 +306,7 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
int maximumIterations,
bool & hasConverged,
pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
double epsilon,
float epsilon,
bool icp2D)
{
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
@@ -344,13 +344,22 @@ Transform icpPointToPlane(
double maxCorrespondenceDistance,
int maximumIterations,
bool & hasConverged,
pcl::PointCloud<pcl::PointNormal> & cloud_source_registered)
pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
float epsilon,
bool icp2D)
{
pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
// Set the input source and target
icp.setInputTarget (cloud_target);
icp.setInputSource (cloud_source);
if(icp2D)
{
pcl::registration::TransformationEstimation2D<pcl::PointNormal, pcl::PointNormal>::Ptr est;
est.reset(new pcl::registration::TransformationEstimation2D<pcl::PointNormal, pcl::PointNormal>);
icp.setTransformationEstimation(est);
}
pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>::Ptr est;
est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>);
icp.setTransformationEstimation(est);
@@ -360,7 +369,7 @@ Transform icpPointToPlane(
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (maximumIterations);
// Set the transformation epsilon (criterion 2)
//icp.setTransformationEpsilon (1e-8);
icp.setTransformationEpsilon (epsilon);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);

17
corelib/src/util3d_transforms.cpp
View File

@@ -90,6 +90,23 @@ pcl::PointXYZRGB transformPoint(
{
return pcl::transformPoint(pt, transform.toEigen3f());
}
pcl::PointNormal transformPoint(
const pcl::PointNormal & point,
const Transform & transform)
{
pcl::PointNormal ret;
Eigen::Matrix<float, 3, 1> pt (point.x, point.y, point.z);
ret.x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
ret.y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
ret.z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
// Rotate normals
Eigen::Matrix<float, 3, 1> nt (point.normal_x, point.normal_y, point.normal_z);
ret.normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
return ret;
}
}