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

Refactoring Registration: added RegistrationInfo class, pipeline registration. Camera source: create laser scan from depth image. Removed OdometryICP as the same behavior can be achieved with OdometryF2F and "Reg/Strategy" = 1 (ICP) or 2 (Vis+ICP).

Browse Source
This commit is contained in:
matlabbe
2016-01-06 17:26:55 -05:00
parent 1c66ad1db2
commit 28d9bbb85d
35 changed files with 1209 additions and 1362 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/CMakeLists.txt
View File

@@ -50,6 +50,7 @@ SET(SRC_FILES
OptimizerGTSAM.cpp
OptimizerCVSBA.cpp
Registration.cpp
RegistrationIcp.cpp
RegistrationVis.cpp
@@ -57,7 +58,6 @@ SET(SRC_FILES
OdometryThread.cpp
OdometryLocalMap.cpp
OdometryMono.cpp
OdometryICP.cpp
OdometryF2F.cpp
Stereo.cpp

4
corelib/src/Camera.cpp
View File

@@ -102,8 +102,8 @@ SensorData Camera::takeImage(CameraInfo * info)
}
if(info)
{
info->id_ = data.id();
info->timeCapture_ = captureTime;
info->id = data.id();
info->timeCapture = captureTime;
}
return data;
}

32
corelib/src/CameraThread.cpp
View File

@@ -45,6 +45,9 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
_mirroring(false),
_colorOnly(false),
_stereoToDepth(false),
_scanFromDepth(false),
_scanDecimation(4),
_scanMaxDepth(4.0f),
_stereoDense(new StereoBM(parameters))
{
UASSERT(_camera != 0);
@@ -103,7 +106,7 @@ void CameraThread::mainLoop()
cv::flip(data.depthRaw(), tmpDepth, 1);
data.setDepthOrRightRaw(tmpDepth);
}
info.timeMirroring_ = timer.ticks();
info.timeMirroring = timer.ticks();
}
if(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty())
{
@@ -115,10 +118,31 @@ void CameraThread::mainLoop()
data.setCameraModel(data.stereoCameraModel().left());
data.setDepthOrRightRaw(depth);
data.setStereoCameraModel(StereoCameraModel());
info.timeDisparity_ = timer.ticks();
UINFO("Computing disparity = %f s", info.timeDisparity_);
info.timeDisparity = timer.ticks();
UINFO("Computing disparity = %f s", info.timeDisparity);
}
info.cameraName_ = _camera->getSerial();
if(_scanFromDepth &&
data.cameraModels().size() &&
data.cameraModels().at(0).isValid() &&
!data.depthRaw().empty())
{
if(data.laserScanRaw().empty())
{
UASSERT(_scanDecimation >= 1);
UTimer timer;
cv::Mat scan = util3d::laserScanFromPointCloud(*util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth));
data.setLaserScanRaw(scan, (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation), _scanMaxDepth);
info.timeScanFromDepth = timer.ticks();
UINFO("Computing scan from depth = %f s", info.timeScanFromDepth);
}
else
{
UWARN("Option to create laser scan from depth image is enabled, but "
"there is already a laser scan in the captured sensor data. Scan from "
"depth will not be created.");
}
}
info.cameraName = _camera->getSerial();
this->post(new CameraEvent(data, info));
}
else if(!this->isKilled())

107
corelib/src/Memory.cpp
View File

@@ -41,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/VisualWord.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/RegistrationIcp.h"
#include "rtabmap/core/Registration.h"
#include "rtabmap/core/RegistrationVis.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/util3d_features.h"
@@ -102,7 +103,7 @@ Memory::Memory(const ParametersMap & parameters) :
{
_feature2D = Feature2D::create(parameters);
_vwd = new VWDictionary(parameters);
_registrationVis = new RegistrationVis(parameters);
_registrationPipeline = Registration::create(parameters);
_registrationIcp = new RegistrationIcp(parameters);
this->parseParameters(parameters);
}
@@ -365,9 +366,9 @@ Memory::~Memory()
{
delete _vwd;
}
if(_registrationVis)
if(_registrationPipeline)
{
delete _registrationVis;
delete _registrationPipeline;
}
if(_registrationIcp)
{
@@ -447,10 +448,27 @@ void Memory::parseParameters(const ParametersMap & parameters)
_feature2D->parseParameters(parameters);
}
if(_registrationVis)
Registration::Type regStrategy = Registration::kTypeUndef;
if((iter=parameters.find(Parameters::kRegStrategy())) != parameters.end())
{
_registrationVis->parseParameters(parameters);
regStrategy = (Registration::Type)std::atoi((*iter).second.c_str());
}
if(regStrategy!=Registration::kTypeUndef)
{
UDEBUG("new registration strategy %d", int(regStrategy));
if(_registrationPipeline)
{
delete _registrationPipeline;
_registrationPipeline = 0;
}
_registrationPipeline = Registration::create(regStrategy, parameters_);
}
else if(_registrationPipeline)
{
_registrationPipeline->parseParameters(parameters);
}
if(_registrationIcp)
{
_registrationIcp->parseParameters(parameters);
@@ -2012,12 +2030,10 @@ void Memory::removeLink(int oldId, int newId)
}
// compute transform fromId -> toId
Transform Memory::computeVisualTransform(
Transform Memory::computeTransform(
int fromId,
int toId,
std::string * rejectedMsg,
int * inliers,
float * variance)
RegistrationInfo * info)
{
const Signature * fromS = this->getSignature(fromId);
const Signature * toS = this->getSignature(toId);
@@ -2026,37 +2042,49 @@ Transform Memory::computeVisualTransform(
if(fromS && toS)
{
// compute transform fromId -> toId
std::vector<int> inliersV;
if(_reextractLoopClosureFeatures)
// make sure we have all data needed
if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) ||
_registrationPipeline->isScanRequired() ||
_registrationPipeline->isUserDataRequired())
{
getNodeData(fromS->id(), true);
getNodeData(toS->id(), true);
}
// compute transform fromId -> toId
std::vector<int> inliersV;
if(_reextractLoopClosureFeatures || (fromS->getWords().size() && toS->getWords().size()))
{
Signature tmpFrom = *fromS;
Signature tmpTo = *toS;
tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
tmpTo.setWords3(std::multimap<int, cv::Point3f>());
transform = _registrationVis->computeTransformation(tmpFrom, tmpTo, Transform::getIdentity(), rejectedMsg, &inliersV, variance);
}
else if(fromS->getWords().size() && toS->getWords().size())
{
transform = _registrationVis->computeTransformation(*fromS, *toS, Transform::getIdentity(), rejectedMsg, &inliersV, variance);
}
if(inliers)
{
*inliers = (int)inliersV.size();
if(_reextractLoopClosureFeatures)
{
tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
tmpTo.setWords3(std::multimap<int, cv::Point3f>());
}
Transform guess = Transform::getIdentity();
if(!_registrationPipeline->isImageRequired())
{
// no visual in the pipeline, make visual registration for guess
RegistrationVis regVis(parameters_);
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
}
if(!guess.isNull())
{
transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
}
}
}
else
{
std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
if(rejectedMsg)
if(info)
{
*rejectedMsg = msg;
info->rejectedMsg_ = msg;
}
UWARN(msg.c_str());
}
@@ -2068,10 +2096,7 @@ Transform Memory::computeIcpTransform(
int fromId,
int toId,
Transform guess,
std::string * rejectedMsg,
int * inliers,
float * variance,
float * inliersRatio)
RegistrationInfo * info)
{
Signature * fromS = this->_getSignature(fromId);
Signature * toS = this->_getSignature(toId);
@@ -2107,18 +2132,14 @@ Transform Memory::computeIcpTransform(
// compute transform fromId -> toId
std::vector<int> inliersV;
t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, rejectedMsg, &inliersV, variance, inliersRatio);
if(inliers)
{
*inliers = (int)inliersV.size();
}
t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, info);
}
else
{
std::string msg = uFormat("Did not find nodes %d and/or %d", fromId, toId);
if(rejectedMsg)
if(info)
{
*rejectedMsg = msg;
info->rejectedMsg_ = msg;
}
UWARN(msg.c_str());
}
@@ -2130,9 +2151,7 @@ Transform Memory::computeIcpTransformMulti(
int fromId,
int toId,
const std::map<int, Transform> & poses,
std::string * rejectedMsg,
int * inliers,
float * variance)
RegistrationInfo * info)
{
UASSERT(uContains(poses, fromId) && uContains(_signatures, fromId));
UASSERT(uContains(poses, toId) && uContains(_signatures, toId));
@@ -2192,11 +2211,7 @@ Transform Memory::computeIcpTransformMulti(
Transform guess = poses.at(fromId).inverse() * poses.at(toId);
std::vector<int> inliersV;
t = _registrationIcp->computeTransformation(fromS->sensorData(), assembledData, guess, rejectedMsg, &inliersV, variance);
if(inliers)
{
*inliers = (int)inliersV.size();
}
t = _registrationIcp->computeTransformation(fromS->sensorData(), assembledData, guess, info);
}
return t;

4
corelib/src/Odometry.cpp
View File

@@ -70,7 +70,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_minDepth(Parameters::defaultVisMinDepth()),
_maxDepth(Parameters::defaultVisMaxDepth()),
_resetCountdown(Parameters::defaultOdomResetCountdown()),
_force2D(Parameters::defaultVisForce2D()),
_force2D(Parameters::defaultRegForce3DoF()),
_holonomic(Parameters::defaultOdomHolonomic()),
_filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
_particleSize(Parameters::defaultOdomParticleSize()),
@@ -100,7 +100,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kVisMinDepth(), _minDepth);
Parameters::parse(parameters, Parameters::kVisMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kVisRoiRatios(), _roiRatios);
Parameters::parse(parameters, Parameters::kVisForce2D(), _force2D);
Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force2D);
Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);

63
corelib/src/OdometryF2F.cpp
View File

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/OdometryF2F.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Registration.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
@@ -37,15 +38,16 @@ OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomFlowKeyFrameThr()),
guessFromMotion_(Parameters::defaultOdomFlowGuessMotion()),
registration_(parameters),
motionSinceLastKeyFrame_(Transform::getIdentity())
{
registrationPipeline_ = Registration::create(parameters);
Parameters::parse(parameters, Parameters::kOdomFlowKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomFlowGuessMotion(), guessFromMotion_);
}
OdometryF2F::~OdometryF2F()
{
delete registrationPipeline_;
}
void OdometryF2F::reset(const Transform & initialPose)
@@ -74,20 +76,16 @@ Transform OdometryF2F::computeTransform(
return output;
}
float variance = 0;
std::vector<int> inliers;
RegistrationInfo regInfo;
Signature newFrame(data);
if(refFrame_.getWords().size())
if(refFrame_.sensorData().isValid())
{
std::string rejectedMsg;
output = registration_.computeTransformationMod(
output = registrationPipeline_->computeTransformationMod(
refFrame_,
newFrame,
guessFromMotion_?motionSinceLastKeyFrame_*this->previousTransform():Transform(),
&rejectedMsg,
&inliers,
&variance);
&regInfo);
if(info && this->isInfoDataFilled())
{
@@ -106,11 +104,11 @@ Transform OdometryF2F::computeTransform(
idToIndex.insert(std::make_pair(iter->first, i));
++i;
}
info->cornerInliers.resize(inliers.size(), 1);
info->cornerInliers.resize(regInfo.inliersIndexes_.size(), 1);
i=0;
for(; i<(int)inliers.size(); ++i)
for(; i<(int)regInfo.inliersIndexes_.size(); ++i)
{
info->cornerInliers[i] = idToIndex.at(inliers[i]);
info->cornerInliers[i] = idToIndex.at(regInfo.inliersIndexes_[i]);
}
}
@@ -127,17 +125,24 @@ Transform OdometryF2F::computeTransform(
motionSinceLastKeyFrame_ *= output;
// new key-frame?
if(keyFrameThr_ <= 0 || (int)inliers.size() <= keyFrameThr_)
if(keyFrameThr_ <= 0 || (int)regInfo.inliers <= keyFrameThr_)
{
UDEBUG("Update key frame");
// only generate features for the first frame
Signature newRefFrame(data);
Signature dummy;
registration_.computeTransformationMod(
newRefFrame,
dummy);
if((int)newRefFrame.getWords().size() >= this->getMinInliers())
int features = -1;
if(registrationPipeline_->isImageRequired())
{
// this will generate features only for the first frame
Signature dummy;
registrationPipeline_->computeTransformationMod(
newRefFrame,
dummy);
features = (int)newRefFrame.getWords().size();
}
if((features < 0 || features >= this->getMinInliers()) &&
(!registrationPipeline_->isScanRequired() || newRefFrame.sensorData().laserScanRaw().cols))
{
refFrame_ = newRefFrame;
@@ -146,23 +151,33 @@ Transform OdometryF2F::computeTransform(
}
else
{
UWARN("Too low 2D corners (%d), keeping last key frame...",
(int)newRefFrame.getWords().size());
if(features >= 0 && features < this->getMinInliers())
{
UWARN("Too low 2D features (%d), keeping last key frame...", features);
}
if(registrationPipeline_->isScanRequired() && newRefFrame.sensorData().laserScanRaw().cols==0)
{
UWARN("Too low scan points (%d), keeping last key frame...", newRefFrame.sensorData().laserScanRaw().cols);
}
}
}
}
else if(!regInfo.rejectedMsg_.empty())
{
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg_.c_str());
}
if(info)
{
info->type = 1;
info->variance = variance;
info->inliers = (int)inliers.size();
info->variance = regInfo.variance;
info->inliers = regInfo.inliers;
}
UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",
timer.elapsed(),
output.isNull()?"true":"false",
(int)inliers.size(),
(int)regInfo.inliers,
(int)refFrame_.getWords().size(),
!output.isNull()?"true":"false");

216
corelib/src/OdometryICP.cpp
View File

@@ -1,216 +0,0 @@
/*
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/OdometryICP.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
namespace rtabmap {
OdometryICP::OdometryICP(int decimation,
float voxelSize,
int samples,
float maxCorrespondenceDistance,
int maxIterations,
float correspondenceRatio,
bool pointToPlane,
const ParametersMap & odometryParameter) :
Odometry(odometryParameter),
_decimation(decimation),
_voxelSize(voxelSize),
_samples(samples),
_maxCorrespondenceDistance(maxCorrespondenceDistance),
_maxIterations(maxIterations),
_correspondenceRatio(correspondenceRatio),
_pointToPlane(pointToPlane),
_previousCloudNormal(new pcl::PointCloud<pcl::PointNormal>),
_previousCloud(new pcl::PointCloud<pcl::PointXYZ>)
{
}
void OdometryICP::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
_previousCloudNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
_previousCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
}
// return not null transform if odometry is correctly computed
Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * info)
{
UTimer timer;
Transform output;
bool hasConverged = false;
double variance = 0;
unsigned int minPoints = 100;
int correspondences = 0;
if(!data.depthOrRightRaw().empty())
{
if(data.depthOrRightRaw().type() == CV_8UC1)
{
UERROR("ICP 3D cannot be done on stereo images!");
return output;
}
if(!(data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()))
{
UERROR("ICP 3D cannot be done without calibration or on multi-camera!");
return output;
}
const CameraModel & cameraModel = data.cameraModels()[0];
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
data.depthOrRightRaw(),
cameraModel.fx(),
cameraModel.fy(),
cameraModel.cx(),
cameraModel.cy(),
_decimation,
this->getMaxDepth(),
_voxelSize,
_samples,
cameraModel.localTransform());
if(_pointToPlane)
{
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
if(newCloudXYZ->size() != newCloud->size())
{
UWARN("removed nan normals...");
}
if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints)
{
pcl::PointCloud<pcl::PointNormal>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointNormal>);
Transform transform = util3d::icpPointToPlane(
newCloud,
_previousCloudNormal,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
*newCloudRegistered);
util3d::computeVarianceAndCorrespondences(
newCloudRegistered,
_previousCloudNormal,
_maxCorrespondenceDistance,
variance,
correspondences);
// verify if there are enough correspondences
float correspondencesRatio = float(correspondences)/float(_previousCloudNormal->size()>newCloud->size()?_previousCloudNormal->size():newCloud->size());
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= _correspondenceRatio)
{
output = transform;
_previousCloudNormal = newCloud;
}
else
{
UWARN("Transform not valid (hasConverged=%s variance = %f)",
hasConverged?"true":"false", variance);
}
}
else if(newCloud->size() > minPoints)
{
output.setIdentity();
_previousCloudNormal = newCloud;
}
}
else
{
//point to point
if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>);
Transform transform = util3d::icp(
newCloudXYZ,
_previousCloud,
_maxCorrespondenceDistance,
_maxIterations,
hasConverged,
*newCloudRegistered);
util3d::computeVarianceAndCorrespondences(
newCloudRegistered,
_previousCloud,
_maxCorrespondenceDistance,
variance,
correspondences);
// verify if there are enough correspondences
float correspondencesRatio = float(correspondences)/float(_previousCloud->size()>newCloudXYZ->size()?_previousCloud->size():newCloudXYZ->size());
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= _correspondenceRatio)
{
output = transform;
_previousCloud = newCloudXYZ;
}
else
{
UWARN("Transform not valid (hasConverged=%s variance = %f)",
hasConverged?"true":"false", variance);
}
}
else if(newCloudXYZ->size() > minPoints)
{
output.setIdentity();
_previousCloud = newCloudXYZ;
}
}
}
else
{
UERROR("Depth is empty?!?");
}
if(info)
{
info->variance = variance;
info->inliers = correspondences;
}
UINFO("Odom update time = %fs hasConverged=%s variance=%f cloud=%d",
timer.elapsed(),
hasConverged?"true":"false",
variance,
(int)(_pointToPlane?_previousCloudNormal->size():_previousCloud->size()));
return output;
}
} // namespace rtabmap

6
corelib/src/Parameters.cpp
View File

@@ -158,7 +158,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
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/Force2D", std::make_pair(true, Parameters::kRegForce3DoF())));
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())));
@@ -188,13 +188,13 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
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/Force2D", std::make_pair(false, Parameters::kRegForce3DoF())));
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::kRGBDLoopClosureLinkRefining())));
removedParameters_.insert(std::make_pair("LccIcp/Type", std::make_pair(false, Parameters::kRegStrategy())));
removedParameters_.insert(std::make_pair("LccIcp3/Decimation", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("LccIcp3/MaxDepth", std::make_pair(false, "")));

190
corelib/src/Registration.cpp Normal file
View File

@@ -0,0 +1,190 @@
/*
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/RegistrationIcp.h>
#include <rtabmap/utilite/ULogger.h>
namespace rtabmap {
Registration * Registration::create(const ParametersMap & parameters)
{
int regTypeInt = Parameters::defaultRegStrategy();
Parameters::parse(parameters, Parameters::kRegStrategy(), regTypeInt);
Registration::Type type = (Registration::Type)regTypeInt;
return create(type, parameters);
}
Registration * Registration::create(Registration::Type & type, const ParametersMap & parameters)
{
UDEBUG("type=%d", (int)type);
Registration * reg = 0;
switch(type)
{
case Registration::kTypeIcp:
reg = new RegistrationIcp(parameters);
break;
case Registration::kTypeVisIcp:
reg = new RegistrationVis(parameters, new RegistrationIcp(parameters));
break;
default: // kTypeVis
reg = new RegistrationVis(parameters);
type = Registration::kTypeVis;
break;
}
return reg;
}
Registration::Registration(const ParametersMap & parameters, Registration * child) :
varianceFromInliersCount_(Parameters::defaultRegVarianceFromInliersCount()),
force3DoF_(Parameters::defaultRegForce3DoF()),
child_(child)
{
this->parseParameters(parameters);
}
Registration::~Registration()
{
if(child_)
{
delete child_;
}
}
void Registration::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), varianceFromInliersCount_);
Parameters::parse(parameters, Parameters::kRegForce3DoF(), force3DoF_);
if(child_)
{
child_->parseParameters(parameters);
}
}
bool Registration::isImageRequired() const
{
bool val = isImageRequiredImpl();
if(!val && child_)
{
val = child_->isImageRequired();
}
return val;
}
bool Registration::isScanRequired() const
{
bool val = isScanRequiredImpl();
if(!val && child_)
{
val = child_->isScanRequired();
}
return val;
}
bool Registration::isUserDataRequired() const
{
bool val = isUserDataRequiredImpl();
if(!val && child_)
{
val = child_->isUserDataRequired();
}
return val;
}
void Registration::setChildRegistration(Registration * child)
{
if(child_)
{
delete child_;
}
child_ = child;
}
Transform Registration::computeTransformation(
const Signature & from,
const Signature & to,
Transform guess,
RegistrationInfo * infoOut) const
{
Signature fromCopy(from);
Signature toCopy(to);
return computeTransformationMod(fromCopy, toCopy, guess, infoOut);
}
Transform Registration::computeTransformation(
const SensorData & from,
const SensorData & to,
Transform guess,
RegistrationInfo * infoOut) const
{
Signature fromCopy(from);
Signature toCopy(to);
return computeTransformationMod(fromCopy, toCopy, guess, infoOut);
}
Transform Registration::computeTransformationMod(
Signature & from,
Signature & to,
Transform guess,
RegistrationInfo * infoOut) const
{
RegistrationInfo info;
Transform t = computeTransformationImpl(from, to, guess, info);
if(child_)
{
if(!t.isNull())
{
t = child_->computeTransformationMod(from, to, force3DoF_?t.to3DoF():t, &info);
}
}
else if(!t.isNull() && force3DoF_)
{
t = t.to3DoF();
}
if(varianceFromInliersCount_)
{
if(info.inliersRatio)
{
info.variance = info.inliersRatio > 0?1.0/double(info.inliersRatio):1.0;
}
else
{
info.variance = info.inliers > 0?1.0f/float(info.inliers):1.0f;
}
info.variance = info.variance>0.0f?info.variance:0.0001f; // epsilon if exact transform
}
if(infoOut)
{
*infoOut = info;
}
return t;
}
}

63
corelib/src/RegistrationIcp.cpp
View File

@@ -39,10 +39,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
RegistrationIcp::RegistrationIcp(const ParametersMap & parameters) :
RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration * child) :
Registration(parameters, child),
_maxTranslation(Parameters::defaultIcpMaxTranslation()),
_maxRotation(Parameters::defaultIcpMaxRotation()),
_icp2D(Parameters::defaultIcp2D()),
_voxelSize(Parameters::defaultIcpVoxelSize()),
_downsamplingStep(Parameters::defaultIcpDownsamplingStep()),
_maxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()),
@@ -60,7 +60,6 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kIcpMaxTranslation(), _maxTranslation);
Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _maxRotation);
Parameters::parse(parameters, Parameters::kIcp2D(), _icp2D);
Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _voxelSize);
Parameters::parse(parameters, Parameters::kIcpDownsamplingStep(), _downsamplingStep);
Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _maxCorrespondenceDistance);
@@ -77,42 +76,18 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
UASSERT_MSG(_pointToPlaneNormalNeighbors > 0, uFormat("value=%d", _pointToPlaneNormalNeighbors).c_str());
}
Transform RegistrationIcp::computeTransformationMod(
Transform RegistrationIcp::computeTransformationImpl(
Signature & fromSignature,
Signature & toSignature,
Transform guess,
std::string * rejectedMsg,
std::vector<int> * inliersOut,
float * varianceOut,
float * inliersRatioOut) const
{
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,
std::vector<int> * inliersOut,
float * varianceOut,
float * inliersRatioOut) const
RegistrationInfo & info) const
{
UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
UDEBUG("Voxel size=%f", _voxelSize);
UDEBUG("2D=%d", _icp2D?1:0);
UDEBUG("PointToPlane=%d", _pointToPlane?1:0);
UDEBUG("Normal neighborhood=%d", _pointToPlaneNormalNeighbors);
UDEBUG("Max corrrespondence distance=%f", _maxCorrespondenceDistance);
UDEBUG("Max Iterations=%d", _maxIterations);
UDEBUG("Variance from inliers count=%d", _varianceFromInliersCount?1:0);
UDEBUG("Correspondence Ratio=%f", _correspondenceRatio);
UDEBUG("Max translation=%f", _maxTranslation);
UDEBUG("Max rotation=%f", _maxRotation);
@@ -121,6 +96,9 @@ Transform RegistrationIcp::computeTransformation(
std::string msg;
Transform transform;
SensorData & dataFrom = fromSignature.sensorData();
SensorData & dataTo = toSignature.sensorData();
// ICP with guess transform
if(!dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
{
@@ -157,7 +135,7 @@ Transform RegistrationIcp::computeTransformation(
double variance = 1.0;
bool correspondencesComputed = false;
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
if(!_icp2D) // 3D ICP
if(!force3DoF()) // 3D ICP
{
if(_pointToPlane)
{
@@ -287,23 +265,9 @@ Transform RegistrationIcp::computeTransformation(
maxLaserScans>0?maxLaserScans:dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():(int)(toCloud->size()>fromCloud->size()?toCloud->size():fromCloud->size()),
correspondencesRatio*100.0f);
if(_varianceFromInliersCount)
{
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->push_back(correspondences);
}
if(inliersRatioOut)
{
*inliersRatioOut = correspondencesRatio;
}
info.variance = variance>0.0f?variance:0.0001; // epsilon if exact transform
info.inliers = correspondences;
info.inliersRatio = correspondencesRatio;
if(correspondencesRatio < _correspondenceRatio)
{
@@ -354,10 +318,7 @@ Transform RegistrationIcp::computeTransformation(
}
if(rejectedMsg)
{
*rejectedMsg = msg;
}
info.rejectedMsg_ = msg;
UDEBUG("New transform = %s", transform.prettyPrint().c_str());
return transform;

94
corelib/src/RegistrationVis.cpp
View File

@@ -41,12 +41,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration * child) :
Registration(parameters, child),
_minInliers(Parameters::defaultVisMinInliers()),
_inlierDistance(Parameters::defaultVisInlierDistance()),
_iterations(Parameters::defaultVisIterations()),
_refineIterations(Parameters::defaultVisRefineIterations()),
_force2D(Parameters::defaultVisForce2D()),
_epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
_estimationType(Parameters::defaultVisEstimationType()),
_forwardEstimateOnly(Parameters::defaultVisForwardEstOnly()),
@@ -82,7 +82,6 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
Parameters::parse(parameters, Parameters::kVisForce2D(), _force2D);
Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
@@ -154,19 +153,15 @@ RegistrationVis::~RegistrationVis()
{
}
Transform RegistrationVis::computeTransformationMod(
Transform RegistrationVis::computeTransformationImpl(
Signature & fromSignature,
Signature & toSignature,
Transform guess, // guess is only used by Optical Flow correspondences (flowMaxLevel is set to 0 when guess is used)
std::string * rejectedMsg,
std::vector<int> * inliersOut,
float * varianceOut,
float * inliersRatioOut) const
RegistrationInfo & info) const
{
UDEBUG("%s=%d", Parameters::kVisMinInliers().c_str(), _minInliers);
UDEBUG("%s=%f", Parameters::kVisInlierDistance().c_str(), _inlierDistance);
UDEBUG("%s=%d", Parameters::kVisIterations().c_str(), _iterations);
UDEBUG("%s=%d", Parameters::kVisForce2D().c_str(), _force2D?1:0);
UDEBUG("%s=%d", Parameters::kVisEstimationType().c_str(), _estimationType);
UDEBUG("%s=%d", Parameters::kVisForwardEstOnly().c_str(), _forwardEstimateOnly);
UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
@@ -337,7 +332,7 @@ Transform RegistrationVis::computeTransformationMod(
kptsFrom3DKept.resize(ki);
std::vector<cv::Point3f> kptsTo3D;
if(_estimationType == 0 || (_estimationType == 1 && !_varianceFromInliersCount) || !_forwardEstimateOnly)
if(_estimationType == 0 || (_estimationType == 1 && !varianceFromInliersCount()) || !_forwardEstimateOnly)
{
kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
}
@@ -433,7 +428,7 @@ Transform RegistrationVis::computeTransformationMod(
{
kptsFrom3D = uValues(fromSignature.getWords3());
}
if((_estimationType == 0 || (_estimationType == 1 && !_varianceFromInliersCount) || !_forwardEstimateOnly) &&
if((_estimationType == 0 || (_estimationType == 1 && !varianceFromInliersCount()) || !_forwardEstimateOnly) &&
toSignature.getWords3().empty() &&
!toSignature.sensorData().imageRaw().empty())
{
@@ -503,6 +498,7 @@ Transform RegistrationVis::computeTransformationMod(
/////////////////////
Transform transform;
float variance = 1.0f;
int inliersCount = 0;
if(toSignature.getWords().size() || !toSignature.sensorData().imageRaw().empty())
{
Transform transforms[2];
@@ -629,7 +625,7 @@ Transform RegistrationVis::computeTransformationMod(
_PnPRefineIterations,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
uMultimapToMapUnique(signatureA->getWords3()),
_varianceFromInliersCount?0:&variances[dir],
varianceFromInliersCount()?0:&variances[dir],
0,
&inliersV);
inliers[dir] = inliersV;
@@ -696,68 +692,27 @@ Transform RegistrationVis::computeTransformationMod(
if(transforms[0].isNull())
{
transform = transforms[1];
if(inliersOut)
{
*inliersOut = inliers[1];
}
info.inliersIndexes_ = inliers[1];
variance = variances[1];
if(_varianceFromInliersCount)
{
variance = inliers[1].size() > 0?1.0f/float(inliers[1].size()):1.0f;
}
inliersCount = (int)inliers[1].size();
}
else
{
/*if(!guess.isNull())
{
// use the transform nearest of the guess
int index = 0;
if(transforms[0].getDistance(guess) > transforms[1].getDistance(guess))
{
index = 1;
}
transform = transforms[index];
if(inliersOut)
{
*inliersOut = inliers[index];
}
transform = transforms[0].interpolate(0.5f, transforms[1]);
info.inliersIndexes_ = inliers[0];
variance = variances[index];
if(_varianceFromInliersCount)
{
variance = inliers[index].size() > 0?1.0f/float(inliers[index].size()):1.0f;
}
}
else*/
{
transform = transforms[0].interpolate(0.5f, transforms[1]);
if(inliersOut)
{
*inliersOut = inliers[0];
}
variance = (variances[0]+variances[1])/2.0f;
if(_varianceFromInliersCount)
{
int avg = (inliers[0].size()+inliers[1].size())/2;
variance = avg>0?1.0f/float(avg):1.0f;
}
}
variance = (variances[0]+variances[1])/2.0f;
inliersCount = (int)(inliers[0].size()+inliers[1].size())/2;
}
}
else
{
transform = transforms[0];
if(inliersOut)
{
*inliersOut = inliers[0];
}
info.inliersIndexes_ = inliers[0];
variance = variances[0];
if(_varianceFromInliersCount)
{
variance = inliers[0].size() > 0?1.0f/float(inliers[0].size()):1.0f;
}
inliersCount = (int)inliers[0].size();
}
}
@@ -776,21 +731,12 @@ Transform RegistrationVis::computeTransformationMod(
roll, pitch, yaw);
UWARN(msg.c_str());
}
else if(_force2D)
{
UDEBUG("Forcing 2D...");
transform = Transform(x,y,0, 0, 0, yaw);
}
}
if(rejectedMsg)
{
*rejectedMsg = msg;
}
if(varianceOut)
{
*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
}
info.inliers = inliersCount;
info.rejectedMsg_ = msg;
info.variance = variance>0.0f?variance:0.0001f; // epsilon if exact transform
UDEBUG("transform=%s", transform.prettyPrint().c_str());
return transform;
}

75
corelib/src/Rtabmap.cpp
View File

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/BayesFilter.h"
#include "rtabmap/core/Compression.h"
#include "rtabmap/core/RegistrationInfo.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UFile.h>
@@ -89,7 +90,6 @@ Rtabmap::Rtabmap() :
_rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()),
_rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
_newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
_loopClosureRefining(Parameters::defaultRGBDLoopClosureLinkRefining()),
_neighborLinkRefining(Parameters::defaultRGBDNeighborLinkRefining()),
_proximityByTime(Parameters::defaultRGBDProximityByTime()),
_proximityBySpace(Parameters::defaultRGBDProximityBySpace()),
@@ -411,7 +411,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
Parameters::parse(parameters, Parameters::kRGBDLoopClosureLinkRefining(), _loopClosureRefining);
UASSERT(_rgbdLinearUpdate >= 0.0f);
UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -1009,21 +1008,18 @@ bool Rtabmap::process(
{
UINFO("Odometry correction by scan matching");
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, &rejectedMsg, &inliers, &variance, &inliersRatio);
RegistrationInfo info;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &info);
if(!t.isNull())
{
UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
signature->id(),
oldId,
variance,
info.variance,
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str());
UASSERT(variance > 0.0);
_memory->updateLink(oldId, signature->id(), t, variance, variance);
UASSERT(info.variance > 0.0);
_memory->updateLink(oldId, signature->id(), t, info.variance, info.variance);
if(_optimizeFromGraphEnd)
{
@@ -1043,17 +1039,17 @@ bool Rtabmap::process(
}
else
{
UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
if(variance > 0)
UINFO("Scan matching rejected: %s", info.rejectedMsg_.c_str());
if(info.variance > 0)
{
double sqrtVar = sqrt(variance);
double sqrtVar = sqrt(info.variance);
_memory->updateLink(signature->id(), oldId, guess, sqrtVar, sqrtVar);
}
}
statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), inliers);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), inliersRatio);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), variance);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), info.inliersRatio);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), info.variance);
statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
}
}
@@ -1161,13 +1157,9 @@ bool Rtabmap::process(
{
std::string rejectedMsg;
UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
float variance = 1.0f;
int inliers = -1;
Transform transform = _memory->computeVisualTransform(signature->id(), *iter, &rejectedMsg, &inliers, &variance);
if(!transform.isNull() && _loopClosureRefining)
{
transform = _memory->computeIcpTransform(signature->id(), *iter, transform, &rejectedMsg, 0, &variance);
}
RegistrationInfo info;
Transform transform = _memory->computeTransform(signature->id(), *iter, &info);
if(!transform.isNull())
{
UDEBUG("Add local loop closure in TIME (%d->%d) %s",
@@ -1175,8 +1167,8 @@ bool Rtabmap::process(
*iter,
transform.prettyPrint().c_str());
// Add a loop constraint
UASSERT(variance > 0.0);
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, variance, variance)))
UASSERT(info.variance > 0.0);
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, info.variance, info.variance)))
{
++localLoopClosuresInTimeFound;
UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1718,19 +1710,14 @@ bool Rtabmap::process(
float variance = 1.0f;
if(_rgbdSlamMode)
{
std::string rejectedMsg;
transform = _memory->computeVisualTransform(signature->id(), _loopClosureHypothesis.first, &rejectedMsg, &loopClosureVisualInliers, &variance);
if(!transform.isNull() && _loopClosureRefining)
{
transform = _memory->computeIcpTransform(signature->id(), _loopClosureHypothesis.first, transform, &rejectedMsg, 0, &variance);
}
RegistrationInfo info;
transform = _memory->computeTransform(signature->id(), _loopClosureHypothesis.first, &info);
loopClosureVisualInliers = info.inliers;
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)
{
UWARN("Rejected loop closure %d -> %d: %s",
_loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str());
_loopClosureHypothesis.first, signature->id(), info.rejectedMsg_.c_str());
}
}
if(!rejectedHypothesis)
@@ -1822,12 +1809,8 @@ bool Rtabmap::process(
(_proximityFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
{
float variance = 1.0f;
Transform transform = _memory->computeVisualTransform(signature->id(), nearestId, 0, 0, &variance);
if(!transform.isNull() && _loopClosureRefining)
{
transform = _memory->computeIcpTransform(signature->id(), nearestId, transform, 0, 0, &variance);
}
RegistrationInfo info;
Transform transform = _memory->computeTransform(signature->id(), nearestId, &info);
if(!transform.isNull())
{
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
@@ -1836,8 +1819,8 @@ bool Rtabmap::process(
signature->id(),
nearestId,
transform.prettyPrint().c_str());
UASSERT(variance > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, variance, variance));
UASSERT(info.variance > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.variance, info.variance));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
if(_loopClosureHypothesis.first == 0)
@@ -1927,8 +1910,8 @@ bool Rtabmap::process(
//The nearest will be the reference for a loop closure transform
if(signature->getLinks().find(nearestId) == signature->getLinks().end())
{
float variance = 1.0f;
Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, path, 0, 0, &variance);
RegistrationInfo info;
Transform transform = _memory->computeIcpTransformMulti(signature->id(), nearestId, path, &info);
if(!transform.isNull())
{
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
@@ -1960,8 +1943,8 @@ bool Rtabmap::process(
}
// set Identify covariance for laser scan matching only
UASSERT(variance>0.0);
double sqrtVar = sqrt(variance);
UASSERT(info.variance>0.0);
double sqrtVar = sqrt(info.variance);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, sqrtVar, sqrtVar, scanMatchingIds));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));

7
corelib/src/Transform.cpp
View File

@@ -173,6 +173,13 @@ Transform Transform::translation() const
0,0,1, data()[11]);
}
Transform Transform::to3DoF() const
{
float x,y,z,roll,pitch,yaw;
this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
return Transform(x,y,0, 0,0,yaw);
}
void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
{
pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);