i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 18:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
efa5afeba77f441ecfc02e78c1ed793c9141be45
rtabmap/corelib/src/odometry/OdometryF2M.cpp
T

1618 lines
62 KiB
C++
Raw Blame History

/*
Copyright (c) 2010-2016, 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/OdometryInfo.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/RegistrationVis.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/Optimizer.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UMath.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UConversion.h"
#if CV_MAJOR_VERSION < 5
#include <opencv2/calib3d/calib3d.hpp>
#else
#include <opencv2/geometry.hpp>
#endif
#include <rtabmap/core/odometry/OdometryF2M.h>
#include <pcl/common/io.h>
#if _MSC_VER
#define ISFINITE(value) _finite(value)
#else
#define ISFINITE(value) std::isfinite(value)
#endif
namespace rtabmap {
OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
Odometry(parameters),
maximumMapSize_(Parameters::defaultOdomF2MMaxSize()),
keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
visKeyFrameThr_(Parameters::defaultOdomVisKeyFrameThr()),
maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
initDepthFactor_(Parameters::defaultOdomF2MInitDepthFactor()),
floorThreshold_(Parameters::defaultOdomF2MFloorThreshold()),
scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
scanSubtractAngle_(Parameters::defaultOdomF2MScanSubtractAngle()),
scanMapMaxRange_(Parameters::defaultOdomF2MScanRange()),
bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
bundleMinMotion_(Parameters::defaultOdomF2MBundleAdjustmentMinMotion()),
bundleMaxKeyFramesPerFeature_(Parameters::defaultOdomF2MBundleAdjustmentMaxKeyFramesPerFeature()),
bundleUpdateFeatureMapOnAllFrames_(Parameters::defaultOdomF2MBundleUpdateFeatureMapOnAllFrames()),
validDepthRatio_(Parameters::defaultOdomF2MValidDepthRatio()),
pointToPlaneK_(Parameters::defaultIcpPointToPlaneK()),
pointToPlaneRadius_(Parameters::defaultIcpPointToPlaneRadius()),
map_(new Signature(-1)),
lastFrame_(new Signature(1)),
lastFrameOldestNewId_(0),
bundleSeq_(0),
sba_(0)
{
UDEBUG("");
Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_);
Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), visKeyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
Parameters::parse(parameters, Parameters::kOdomF2MInitDepthFactor(), initDepthFactor_);
Parameters::parse(parameters, Parameters::kOdomF2MFloorThreshold(), floorThreshold_);
Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
if(Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractAngle(), scanSubtractAngle_))
{
scanSubtractAngle_ *= M_PI/180.0f;
}
Parameters::parse(parameters, Parameters::kOdomF2MScanRange(), scanMapMaxRange_);
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_);
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMinMotion(), bundleMinMotion_);
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxKeyFramesPerFeature(), bundleMaxKeyFramesPerFeature_);
Parameters::parse(parameters, Parameters::kOdomF2MBundleUpdateFeatureMapOnAllFrames(), bundleUpdateFeatureMapOnAllFrames_);
Parameters::parse(parameters, Parameters::kOdomF2MValidDepthRatio(), validDepthRatio_);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), pointToPlaneK_);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), pointToPlaneRadius_);
UASSERT(bundleMaxFrames_ >= 0);
ParametersMap bundleParameters = parameters;
if(bundleAdjustment_ > 0)
{
// The BundleAdjustment int matches the Optimizer/Strategy parameter
// 1:1 (g2o=1, GTSAM=2, Ceres=3, CVSBA=4). 0 = "disabled" -- it's
// the TORO slot, which isn't BA-capable.
const Optimizer::Type sbaType = static_cast<Optimizer::Type>(bundleAdjustment_);
if(Optimizer::isAvailable(sbaType))
{
// disable bundle in RegistrationVis as we do it already here
uInsert(bundleParameters, ParametersPair(Parameters::kVisBundleAdjustment(), "0"));
sba_ = Optimizer::create(sbaType, bundleParameters);
}
else
{
UWARN("Selected bundle adjustment approach (\"%s\"=\"%d\") is not available, "
"local bundle adjustment is then disabled.", Parameters::kOdomF2MBundleAdjustment().c_str(), bundleAdjustment_);
bundleAdjustment_ = 0;
}
}
UASSERT(maximumMapSize_ >= 0);
UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
UASSERT(visKeyFrameThr_>=0);
UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
UASSERT(maxNewFeatures_ >= 0);
UASSERT(initDepthFactor_>0.0f);
int corType = Parameters::defaultVisCorType();
Parameters::parse(parameters, Parameters::kVisCorType(), corType);
if(corType != 0)
{
UWARN("%s=%d is not supported by OdometryF2M, using Features matching approach instead (type=0).",
Parameters::kVisCorType().c_str(),
corType);
corType = 0;
}
uInsert(bundleParameters, ParametersPair(Parameters::kVisCorType(), uNumber2Str(corType)));
int estType = Parameters::defaultVisEstimationType();
Parameters::parse(parameters, Parameters::kVisEstimationType(), estType);
if(estType > 1)
{
UWARN("%s=%d is not supported by OdometryF2M, using 2D->3D approach instead (type=1).",
Parameters::kVisEstimationType().c_str(),
estType);
estType = 1;
}
uInsert(bundleParameters, ParametersPair(Parameters::kVisEstimationType(), uNumber2Str(estType)));
regPipeline_ = Registration::create(bundleParameters);
if(bundleAdjustment_>0 && regPipeline_->isScanRequired())
{
if(regPipeline_->isImageRequired())
{
UWARN("%s=%d cannot be used with registration not done only with images (%s=%s), disabling bundle adjustment.",
Parameters::kOdomF2MBundleAdjustment().c_str(),
bundleAdjustment_,
Parameters::kRegStrategy().c_str(),
uValue(bundleParameters, Parameters::kRegStrategy(), uNumber2Str(Parameters::defaultRegStrategy())).c_str());
}
bundleAdjustment_ = 0;
}
parameters_ = bundleParameters;
}
OdometryF2M::~OdometryF2M()
{
delete map_;
delete lastFrame_;
delete sba_;
delete regPipeline_;
UDEBUG("");
}
void OdometryF2M::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
UDEBUG("initialPose=%s", initialPose.prettyPrint().c_str());
Odometry::reset(initialPose);
*lastFrame_ = Signature(1);
*map_ = Signature(-1);
scansBuffer_.clear();
bundleWordReferences_.clear();
bundlePoses_.clear();
bundleLinks_.clear();
bundleModels_.clear();
bundlePoseReferences_.clear();
bundleSeq_ = 0;
lastFrameOldestNewId_ = 0;
}
// return not null transform if odometry is correctly computed
Transform OdometryF2M::computeTransform(
SensorData & data,
const Transform & guessIn,
OdometryInfo * info)
{
Transform guess = guessIn;
UTimer timer;
Transform output;
if(info)
{
info->type = 0;
}
Transform imuT;
if(sba_ && sba_->gravitySigma() > 0.0f && !imus().empty())
{
imuT = Transform::getTransform(imus(), data.stamp());
if(data.imu().empty())
{
Eigen::Quaternionf q = imuT.getQuaternionf();
data.setIMU(IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat(), cv::Vec3d(), cv::Mat(), cv::Vec3d(), cv::Mat()));
}
}
RegistrationInfo regInfo;
int nFeatures = 0;
delete lastFrame_;
int id = data.id();
data.setId(++bundleSeq_); // generate our own unique ids, to make sure they are correctly set
lastFrame_ = new Signature(data);
data.setId(id);
bool addKeyFrame = false;
int totalBundleWordReferencesUsed = 0;
int totalBundleOutliers = 0;
float bundleTime = 0.0f;
bool visDepthAsMask = Parameters::defaultVisDepthAsMask();
Parameters::parse(parameters_, Parameters::kVisDepthAsMask(), visDepthAsMask);
std::vector<CameraModel> lastFrameModels;
if(!lastFrame_->sensorData().cameraModels().empty() &&
lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
lastFrameModels = lastFrame_->sensorData().cameraModels();
}
else if(!lastFrame_->sensorData().stereoCameraModels().empty() &&
lastFrame_->sensorData().stereoCameraModels().at(0).isValidForProjection())
{
for(size_t i=0; i<lastFrame_->sensorData().stereoCameraModels().size(); ++i)
{
CameraModel model = lastFrame_->sensorData().stereoCameraModels()[i].left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-lastFrame_->sensorData().stereoCameraModels()[i].baseline()*model.fx(),
model.imageSize());
lastFrameModels.push_back(model);
}
}
UDEBUG("lastFrameModels=%ld", lastFrameModels.size());
// Generate keypoints from the new data
if(lastFrame_->sensorData().isValid())
{
if((map_->getWords3().size() || !map_->sensorData().laserScanRaw().isEmpty()) &&
lastFrame_->sensorData().isValid())
{
Signature tmpMap;
Transform transform;
UDEBUG("guess=%s frames=%d image required=%d", guess.prettyPrint().c_str(), this->framesProcessed(), regPipeline_->isImageRequired()?1:0);
// bundle adjustment stuff if used
std::map<int, cv::Point3f> points3DMap;
std::map<int, Transform> bundlePoses;
std::multimap<int, Link> bundleLinks;
std::map<int, std::vector<CameraModel> > bundleModels;
float bundleAvgInlierDistance = 0.0f;
for(int guessIteration=0;
guessIteration<(!guess.isNull()&&regPipeline_->isImageRequired()?2:1) && transform.isNull();
++guessIteration)
{
tmpMap = *map_;
// reset matches, but keep already extracted features in lastFrame_->sensorData()
lastFrame_->removeAllWords();
points3DMap.clear();
bundlePoses.clear();
bundleLinks.clear();
bundleModels.clear();
float maxCorrespondenceDistance = 0.0f;
float outlierRatio = 0.0f;
if(guess.isNull() &&
!regPipeline_->isImageRequired() &&
regPipeline_->isScanRequired() &&
this->framesProcessed() < 2)
{
// only on initialization (first frame to register), increase icp max correspondences in case the robot is already moving
maxCorrespondenceDistance = Parameters::defaultIcpMaxCorrespondenceDistance();
outlierRatio = Parameters::defaultIcpOutlierRatio();
Parameters::parse(parameters_, Parameters::kIcpMaxCorrespondenceDistance(), maxCorrespondenceDistance);
Parameters::parse(parameters_, Parameters::kIcpOutlierRatio(), outlierRatio);
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance*3.0f)));
params.insert(ParametersPair(Parameters::kIcpOutlierRatio(), uNumber2Str(0.95f)));
regPipeline_->parseParameters(params);
}
if(guessIteration == 1)
{
UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
}
transform = regPipeline_->computeTransformationMod(
tmpMap,
*lastFrame_,
// special case for ICP-only odom, set guess to identity if we just started or reset
guessIteration==0 && !guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->framesProcessed()<2?this->getPose():Transform(),
&regInfo);
if(maxCorrespondenceDistance>0.0f)
{
// set it back
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance)));
params.insert(ParametersPair(Parameters::kIcpOutlierRatio(), uNumber2Str(outlierRatio)));
regPipeline_->parseParameters(params);
}
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
data.setLaserScan(lastFrame_->sensorData().laserScanRaw());
UDEBUG("Registration time = %fs", regInfo.totalTime);
if(!transform.isNull())
{
// local bundle adjustment
if(bundleAdjustment_>0 && sba_ &&
regPipeline_->isImageRequired() &&
!lastFrameModels.empty() &&
regInfo.inliersIDs.size())
{
UDEBUG("Local Bundle Adjustment");
// make sure the IDs of words in the map are not modified (Optical Flow Registration issue)
UASSERT(map_->getWords().size() && tmpMap.getWords().size());
if(map_->getWords().size() != tmpMap.getWords().size() ||
map_->getWords().begin()->first != tmpMap.getWords().begin()->first ||
map_->getWords().rbegin()->first != tmpMap.getWords().rbegin()->first)
{
UERROR("Bundle Adjustment cannot be used with a registration approach recomputing "
"features from the \"from\" signature (e.g., Optical Flow) that would change "
"their ids (size=old=%ld new=%ld first/last: old=%d->%d new=%d->%d).",
map_->getWords().size(), tmpMap.getWords().size(),
map_->getWords().begin()->first, map_->getWords().rbegin()->first,
tmpMap.getWords().begin()->first, tmpMap.getWords().rbegin()->first);
bundleAdjustment_ = 0;
}
else
{
UASSERT(bundlePoses_.size());
UASSERT_MSG(bundlePoses_.size()-1 == bundleLinks_.size(), uFormat("poses=%d links=%d", (int)bundlePoses_.size(), (int)bundleLinks_.size()).c_str());
UASSERT(bundlePoses_.size() == bundleModels_.size());
bundlePoses = bundlePoses_;
bundleLinks = bundleLinks_;
bundleModels = bundleModels_;
bundleLinks.insert(bundleIMUOrientations_.begin(), bundleIMUOrientations_.end());
UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.covariance.inv())));
bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
if(!imuT.isNull())
{
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, imuT)));
}
bundleModels.insert(std::make_pair(lastFrame_->id(), lastFrameModels));
UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
std::map<int, std::map<int, FeatureBA> > wordReferences;
size_t maxKeyFramesForInlier = 0;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
int wordId =regInfo.inliersIDs[i];
// 3D point
std::multimap<int, int>::const_iterator iter3D = tmpMap.getWords().find(wordId);
UASSERT(iter3D!=tmpMap.getWords().end() && !tmpMap.getWords3().empty());
points3DMap.insert(std::make_pair(wordId, tmpMap.getWords3()[iter3D->second]));
// all other references
std::map<int, std::map<int, FeatureBA> >::iterator refIter = bundleWordReferences_.find(wordId);
UASSERT_MSG(refIter != bundleWordReferences_.end(), uFormat("wordId=%d", wordId).c_str());
if(info && refIter->second.size() > maxKeyFramesForInlier)
{
maxKeyFramesForInlier = refIter->second.size();
}
std::map<int, FeatureBA> references;
int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1;
if(step == 0)
{
step = 1;
}
int oi=0;
for(std::map<int, FeatureBA>::iterator jter=refIter->second.begin(); jter!=refIter->second.end(); ++jter)
{
if(oi++ % step == 0 && bundlePoses.find(jter->first)!=bundlePoses.end())
{
references.insert(*jter);
++totalBundleWordReferencesUsed;
}
}
//make sure the last reference is here
if(refIter->second.size() > 1)
{
if(references.insert(*refIter->second.rbegin()).second)
{
++totalBundleWordReferencesUsed;
}
}
std::multimap<int, int>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
if(iter2D!=lastFrame_->getWords().end())
{
UASSERT(!lastFrame_->getWordsKpts().empty());
cv::KeyPoint kpt = lastFrame_->getWordsKpts()[iter2D->second];
int cameraIndex = 0;
if(lastFrameModels.size()>1)
{
UASSERT(lastFrameModels[0].imageWidth()>0);
float subImageWidth = lastFrameModels[0].imageWidth();
cameraIndex = int(kpt.pt.x / subImageWidth);
UASSERT(cameraIndex < (int)lastFrameModels.size());
kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
}
//get depth
float d = 0.0f;
if( !lastFrame_->getWords3().empty() &&
util3d::isFinite(lastFrame_->getWords3()[iter2D->second]))
{
//move back point in camera frame (to get depth along z)
d = util3d::transformPoint(lastFrame_->getWords3()[iter2D->second], lastFrameModels[cameraIndex].localTransform().inverse()).z;
}
references.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, d, cv::Mat(), cameraIndex)));
}
wordReferences.insert(std::make_pair(wordId, references));
//UDEBUG("%d (%f,%f,%f)", iter3D->first, iter3D->second.x, iter3D->second.y, iter3D->second.z);
//for(std::map<int, cv::Point2f>::iterator iter=inserted.first->second.begin(); iter!=inserted.first->second.end(); ++iter)
//{
// UDEBUG("%d (%f,%f)", iter->first, iter->second.x, iter->second.y);
//}
}
UDEBUG("sba...start");
// set root negative to fix all other poses
BAOutliers sbaOutliers;
UTimer bundleTimer;
bundlePoses = sba_->optimizeBA(-lastFrame_->id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
bundleTime = bundleTimer.ticks();
UDEBUG("sba...end");
int sbaOutliersCount = 0;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
BAOutliers::const_iterator iter = sbaOutliers.find(regInfo.inliersIDs[i]);
if(iter != sbaOutliers.end() && iter->second.find(lastFrame_->id()) != iter->second.end())
{
++sbaOutliersCount;
}
}
totalBundleOutliers = sbaOutliersCount;
UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), sbaOutliersCount);
if(info)
{
info->localBundlePoses = bundlePoses;
info->localBundleModels = bundleModels;
info->localBundleMaxKeyFramesForInlier = maxKeyFramesForInlier;
}
UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
if(bundlePoses.size() == bundlePoses_.size()+1)
{
if(!bundlePoses.rbegin()->second.isNull())
{
if(info)
{
info->localBundleOutliersPerCam = std::vector<int>(lastFrameModels.size(),0);
}
if(sbaOutliersCount)
{
regInfo.inliersPerCam = std::vector<int>(lastFrameModels.size(),0);
std::vector<int> newInliers(regInfo.inliersIDs.size());
int oi=0;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
BAOutliers::const_iterator iter = sbaOutliers.find(regInfo.inliersIDs[i]);
if(iter == sbaOutliers.end() || iter->second.find(lastFrame_->id()) == iter->second.end())
{
newInliers[oi++] = regInfo.inliersIDs[i];
regInfo.inliersPerCam[wordReferences.at(regInfo.inliersIDs[i]).at(lastFrame_->id()).cameraIndex] += 1;
}
else if(info)
{
info->localBundleOutliersPerCam[wordReferences.at(regInfo.inliersIDs[i]).at(lastFrame_->id()).cameraIndex] += 1;
}
}
newInliers.resize(oi);
UDEBUG("BA outliers ratio %f", float(sbaOutliersCount)/float(regInfo.inliersIDs.size()));
regInfo.inliers = (int)newInliers.size();
regInfo.inliersIDs = newInliers;
}
if(regInfo.inliers < regPipeline_->getMinVisualCorrespondences())
{
regInfo.rejectedMsg = uFormat("Too low inliers after bundle adjustment: %d<%d", regInfo.inliers, regPipeline_->getMinVisualCorrespondences());
transform.setNull();
}
else
{
transform = bundlePoses.rbegin()->second;
std::multimap<int, Link>::iterator iter = graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false);
UASSERT(iter != bundleLinks.end());
iter->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
iter = graph::findLink(bundleLinks, lastFrame_->id(), lastFrame_->id(), false);
if(info && iter!=bundleLinks.end() && iter->second.type() == Link::kGravity)
{
float rollImu,pitchImu,yaw;
iter->second.transform().getEulerAngles(rollImu, pitchImu, yaw);
float roll,pitch;
transform.getEulerAngles(roll, pitch, yaw);
info->gravityRollError = fabs(rollImu - roll);
info->gravityPitchError = fabs(pitchImu - pitch);
}
// With bundle adjustment, scale down covariance by 10
UASSERT(regInfo.covariance.cols==6 && regInfo.covariance.rows == 6 && regInfo.covariance.type() == CV_64FC1);
double thrLin = Registration::COVARIANCE_LINEAR_EPSILON*10.0;
double thrAng = Registration::COVARIANCE_ANGULAR_EPSILON*10.0;
if(regInfo.covariance.at<double>(0,0)>thrLin)
regInfo.covariance.at<double>(0,0) *= 0.1;
if(regInfo.covariance.at<double>(1,1)>thrLin)
regInfo.covariance.at<double>(1,1) *= 0.1;
if(regInfo.covariance.at<double>(2,2)>thrLin)
regInfo.covariance.at<double>(2,2) *= 0.1;
if(regInfo.covariance.at<double>(3,3)>thrAng)
regInfo.covariance.at<double>(3,3) *= 0.1;
if(regInfo.covariance.at<double>(4,4)>thrAng)
regInfo.covariance.at<double>(4,4) *= 0.1;
if(regInfo.covariance.at<double>(5,5)>thrAng)
regInfo.covariance.at<double>(5,5) *= 0.1;
// Estimate how much the new frame moved from previous frame in term of pixels
if(bundleMinMotion_ > 0.0f)
{
UASSERT(!bundlePoses_.empty());
int count = 0;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
std::map<int, std::map<int, FeatureBA> >::iterator wter = wordReferences.find(regInfo.inliersIDs[i]);
if(wter != wordReferences.end())
{
std::map<int, FeatureBA>::iterator fter = wter->second.find(bundlePoses_.rbegin()->first);
if(fter != wter->second.end())
{
const FeatureBA & f1 = fter->second; // previous key-frame
const FeatureBA & f2 = wter->second.find(lastFrame_->id())->second; // current key-frame
float dx = f1.kpt.pt.x - f2.kpt.pt.x;
float dy = f1.kpt.pt.y - f2.kpt.pt.y;
bundleAvgInlierDistance += sqrt(dx*dx + dy*dy);
++count;
}
}
}
if(count)
{
bundleAvgInlierDistance /= count;
}
UDEBUG("Average pixel distance between %d inliers: %f", count, bundleAvgInlierDistance);
if(info)
{
info->localBundleAvgInlierDistance = bundleAvgInlierDistance;
}
}
}
UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
}
else
{
regInfo.rejectedMsg = "Last bundle pose is null?!";
transform.setNull();
}
}
else
{
regInfo.rejectedMsg = "Local bundle adjustment failed!";
transform.setNull();
}
}
}
if(!transform.isNull())
{
// make it incremental
transform = this->getPose().inverse() * transform;
}
}
if(transform.isNull())
{
if(guessIteration == 1)
{
UWARN("Trial with no guess still fail.");
}
if(!regInfo.rejectedMsg.empty())
{
if(guess.isNull())
{
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
}
else
{
UWARN("Registration failed: \"%s\" (guess=%s)", regInfo.rejectedMsg.c_str(), guess.prettyPrint().c_str());
}
}
else
{
UWARN("Unknown registration error");
}
}
else if(guessIteration == 1)
{
UWARN("Trial with no guess succeeded!");
}
}
if(!transform.isNull())
{
output = transform;
bool modified = false;
Transform newFramePose = this->getPose()*output;
// fields to update
LaserScan mapScan = tmpMap.sensorData().laserScanRaw();
std::multimap<int, int> mapWords = tmpMap.getWords();
std::vector<cv::KeyPoint> mapWordsKpts = tmpMap.getWordsKpts();
std::vector<cv::Point3f> mapPoints = tmpMap.getWords3();
cv::Mat mapDescriptors = tmpMap.getWordsDescriptors();
// update last frame features without depth (if bundle adjustment was done)
// Do this before adding bundle frames to keep mono observations without depth
bool lastFrameWords3Updated = false;
std::vector<cv::Point3f> lastFrameWords3;
if( regPipeline_->isImageRequired() &&
!visDepthAsMask &&
bundleAdjustment_>0 &&
!lastFrame_->getWords().empty() &&
lastFrame_->getWords().size() == lastFrame_->getWords3().size() &&
!points3DMap.empty())
{
lastFrameWords3 = lastFrame_->getWords3();
Transform newFramePoseInv = newFramePose.inverse();
for(std::multimap<int, int>::const_iterator iter=lastFrame_->getWords().begin();
iter!=lastFrame_->getWords().end();
++iter)
{
cv::Point3f & pt = lastFrameWords3.at(iter->second);
if(!util3d::isFinite(pt))
{
std::map<int, cv::Point3f>::iterator mapIter = points3DMap.find(iter->first);
if(mapIter != points3DMap.end())
{
// in base frame
pt = util3d::transformPoint(mapIter->second, newFramePoseInv);
lastFrameWords3Updated = true;
}
}
}
}
if( regPipeline_->isImageRequired() &&
bundleAdjustment_>0 &&
bundleUpdateFeatureMapOnAllFrames_ &&
!points3DMap.empty())
{
// update local map 3D points (if bundle adjustment was done)
for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
UASSERT(mapWords.count(iter->first) == 1);
mapPoints[mapWords.find(iter->first)->second] = iter->second;
}
modified = true;
}
bool addVisualKeyFrame = regPipeline_->isImageRequired() &&
(keyFrameThr_ == 0.0f ||
visKeyFrameThr_ == 0 ||
float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
regInfo.inliers <= visKeyFrameThr_) &&
(bundleAdjustment_==0 || bundleAvgInlierDistance >= bundleMinMotion_);
bool addGeometricKeyFrame = regPipeline_->isScanRequired() &&
(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);
addKeyFrame = addVisualKeyFrame || addGeometricKeyFrame;
UDEBUG("keyframeThr=%f visKeyFrameThr_=%d matches=%d inliers=%d (avg dist=%f, min=%f) features=%d mp=%d",
keyFrameThr_,
visKeyFrameThr_,
regInfo.matches,
regInfo.inliers,
bundleAvgInlierDistance,
bundleMinMotion_,
(int)lastFrame_->sensorData().keypoints().size(),
(int)mapPoints.size());
if(addKeyFrame)
{
//Visual
int added = 0;
int removed = 0;
UTimer tmpTimer;
UDEBUG("Update local map");
modified = bundleAdjustment_>0; // We always add new references even if we don't add/remove points
// update local map
UASSERT(mapWords.size() == mapPoints.size());
UASSERT(mapWords.size() == mapWordsKpts.size());
UASSERT((int)mapPoints.size() == mapDescriptors.rows);
UASSERT_MSG(lastFrame_->getWordsDescriptors().rows == (int)lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().rows, (int)lastFrame_->getWords3().size()).c_str());
std::map<int, int>::iterator iterBundlePosesRef = bundlePoseReferences_.end();
if(bundleAdjustment_>0)
{
bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), 0));
std::multimap<int, Link>::iterator iter = graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false);
UASSERT(iter != bundleLinks.end());
bundleLinks_.insert(*iter);
iter = graph::findLink(bundleLinks, lastFrame_->id(), lastFrame_->id(), false);
if(iter != bundleLinks.end())
{
bundleIMUOrientations_.insert(*iter);
}
uInsert(bundlePoses_, bundlePoses);
UASSERT(bundleModels.find(lastFrame_->id()) != bundleModels.end());
bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id());
if(!bundleUpdateFeatureMapOnAllFrames_)
{
// update local map 3D points (if bundle adjustment was done)
for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
UASSERT(mapWords.count(iter->first) == 1);
mapPoints[mapWords.find(iter->first)->second] = iter->second;
}
}
}
// sort by feature response
std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, std::pair<cv::Mat, int> > > > > newIds;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
UDEBUG("new frame words3=%d", (int)lastFrame_->getWords3().size());
std::set<int> seenStatusUpdated;
// add points without depth only if the local map has reached its maximum size
bool addPointsWithoutDepth = false;
if(!visDepthAsMask && validDepthRatio_ < 1.0f && !lastFrame_->getWords3().empty())
{
int ptsWithDepth = 0;
for (std::vector<cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
iter != lastFrame_->getWords3().end();
++iter)
{
if(util3d::isFinite(*iter))
{
++ptsWithDepth;
}
}
float r = float(ptsWithDepth) / float(lastFrame_->getWords3().size());
addPointsWithoutDepth = r > validDepthRatio_;
if(!addPointsWithoutDepth)
{
UWARN("Not enough points with valid depth in current frame (%d/%d=%f < %s=%f), points without depth are not added to map.",
ptsWithDepth, (int)lastFrame_->getWords3().size(), r, Parameters::kOdomF2MValidDepthRatio().c_str(), validDepthRatio_);
}
}
if(!lastFrameModels.empty())
{
UASSERT(lastFrame_->getWordsKpts().size() == lastFrame_->getWords().size());
for(std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
{
const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
cv::KeyPoint kpt = lastFrame_->getWordsKpts()[iter->second];
int cameraIndex = 0;
if(lastFrameModels.size()>1)
{
UASSERT(lastFrameModels[0].imageWidth()>0);
float subImageWidth = lastFrameModels[0].imageWidth();
cameraIndex = int(kpt.pt.x / subImageWidth);
UASSERT(cameraIndex < (int)lastFrameModels.size());
kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
}
if(mapWords.find(iter->first) == mapWords.end()) // Point not in map
{
if(util3d::isFinite(pt) || addPointsWithoutDepth)
{
newIds.insert(
std::make_pair(kpt.response>0?1.0f/kpt.response:0.0f,
std::make_pair(iter->first,
std::make_pair(kpt,
std::make_pair(pt,
std::make_pair(lastFrame_->getWordsDescriptors().row(iter->second), cameraIndex))))));
}
}
else if(bundleAdjustment_>0)
{
if(lastFrame_->getWords().count(iter->first) == 1)
{
std::multimap<int, int>::iterator iterKpts = mapWords.find(iter->first);
if(iterKpts!=mapWords.end() && !mapWordsKpts.empty())
{
mapWordsKpts[iterKpts->second].octave = kpt.octave;
}
UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
iterBundlePosesRef->second += 1;
//move back point in camera frame (to get depth along z)
float depth = 0.0f;
if(util3d::isFinite(pt))
{
depth = util3d::transformPoint(pt, lastFrameModels[cameraIndex].localTransform().inverse()).z;
}
if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
{
std::map<int, FeatureBA> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex)));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
else
{
std::map<int, rtabmap::FeatureBA> & keyframes = bundleWordReferences_.find(iter->first)->second;
if(bundleMaxKeyFramesPerFeature_ != 0 && (int)keyframes.size() > bundleMaxKeyFramesPerFeature_)
{
// To keep number of keyframes looking at same feature bounded
int frameId = keyframes.rbegin()->first;
UASSERT(bundlePoseReferences_.find(frameId) != bundlePoseReferences_.end());
bundlePoseReferences_.at(frameId) -= 1;
keyframes.erase(frameId);
}
keyframes.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex)));
}
}
}
}
UDEBUG("newIds=%d", (int)newIds.size());
}
int lastFrameOldestNewId = lastFrameOldestNewId_;
lastFrameOldestNewId_ = lastFrame_->getWords().size()?lastFrame_->getWords().rbegin()->first:0;
for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, std::pair<cv::Mat, int> > > > >::reverse_iterator iter=newIds.rbegin();
iter!=newIds.rend();
++iter)
{
if(maxNewFeatures_ == 0 || added < maxNewFeatures_)
{
int cameraIndex = iter->second.second.second.second.second;
cv::Point3f pt = iter->second.second.second.first;
if(!util3d::isFinite(pt))
{
// get the ray instead
float x = iter->second.second.first.pt.x; //subImageWidth should be already removed
float y = iter->second.second.first.pt.y;
Eigen::Vector3f ray = util3d::projectDepthTo3DRay(
lastFrameModels[cameraIndex].imageSize(),
x,
y,
lastFrameModels[cameraIndex].cx(),
lastFrameModels[cameraIndex].cy(),
lastFrameModels[cameraIndex].fx(),
lastFrameModels[cameraIndex].fy());
float scaleInf = initDepthFactor_ * lastFrameModels[cameraIndex].fx();
pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), lastFrameModels[cameraIndex].localTransform()); // in base_link frame
}
if(floorThreshold_ != 0.0f && pt.z < floorThreshold_)
{
continue;
}
if(bundleAdjustment_>0)
{
if(lastFrame_->getWords().count(iter->second.first) == 1)
{
UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
iterBundlePosesRef->second += 1;
//move back point in camera frame (to get depth along z)
float depth = 0.0f;
if(util3d::isFinite(iter->second.second.second.first))
{
depth = util3d::transformPoint(iter->second.second.second.first, lastFrameModels[cameraIndex].localTransform().inverse()).z;
}
if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
{
std::map<int, FeatureBA> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth, cv::Mat(), cameraIndex)));
bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
}
else
{
bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth, cv::Mat(), cameraIndex)));
}
}
}
mapWords.insert(mapWords.end(), std::make_pair(iter->second.first, mapWords.size()));
mapWordsKpts.push_back(iter->second.second.first);
mapPoints.push_back(util3d::transformPoint(pt, newFramePose));
mapDescriptors.push_back(iter->second.second.second.second.first);
if(lastFrameOldestNewId_ > iter->second.first)
{
lastFrameOldestNewId_ = iter->second.first;
}
++added;
}
else
{
break;
}
}
UDEBUG("");
// remove words in map if max size is reached
if((int)mapWords.size() > maximumMapSize_)
{
// remove oldest outliers first
std::set<int> inliers(regInfo.inliersIDs.begin(), regInfo.inliersIDs.end());
std::vector<int> ids = regInfo.matchesIDs;
if(regInfo.projectedIDs.size())
{
ids.resize(ids.size() + regInfo.projectedIDs.size());
int oi=0;
for(unsigned int i=0; i<regInfo.projectedIDs.size(); ++i)
{
if(regInfo.projectedIDs[i]>=lastFrameOldestNewId)
{
ids[regInfo.matchesIDs.size()+oi++] = regInfo.projectedIDs[i];
}
}
ids.resize(regInfo.matchesIDs.size()+oi);
UDEBUG("projected added=%d/%d minLastFrameId=%d", oi, (int)regInfo.projectedIDs.size(), lastFrameOldestNewId);
}
for(unsigned int i=0; i<ids.size() && (int)mapWords.size() > maximumMapSize_ && mapWords.size() >= newIds.size(); ++i)
{
int id = ids.at(i);
if(inliers.find(id) == inliers.end())
{
std::map<int, std::map<int, FeatureBA> >::iterator iterRef = bundleWordReferences_.find(id);
if(iterRef != bundleWordReferences_.end())
{
for(std::map<int, FeatureBA>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
{
if(bundlePoseReferences_.find(iterFrame->first) != bundlePoseReferences_.end())
{
bundlePoseReferences_.at(iterFrame->first) -= 1;
}
}
bundleWordReferences_.erase(iterRef);
}
mapWords.erase(id);
++removed;
}
}
// remove oldest first
for(std::multimap<int, int>::iterator iter = mapWords.begin();
iter!=mapWords.end() && (int)mapWords.size() > maximumMapSize_ && mapWords.size() >= newIds.size();)
{
if(inliers.find(iter->first) == inliers.end())
{
std::map<int, std::map<int, FeatureBA> >::iterator iterRef = bundleWordReferences_.find(iter->first);
if(iterRef != bundleWordReferences_.end())
{
for(std::map<int, FeatureBA>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
{
if(bundlePoseReferences_.find(iterFrame->first) != bundlePoseReferences_.end())
{
bundlePoseReferences_.at(iterFrame->first) -= 1;
}
}
bundleWordReferences_.erase(iterRef);
}
mapWords.erase(iter++);
++removed;
}
else
{
++iter;
}
}
if(mapWords.size() != mapPoints.size())
{
UDEBUG("Remove points");
std::vector<cv::KeyPoint> mapWordsKptsClean(mapWords.size());
std::vector<cv::Point3f> mapPointsClean(mapWords.size());
cv::Mat mapDescriptorsClean(mapWords.size(), mapDescriptors.cols, mapDescriptors.type());
int index = 0;
for(std::multimap<int, int>::iterator iter = mapWords.begin(); iter!=mapWords.end(); ++iter, ++index)
{
mapWordsKptsClean[index] = mapWordsKpts[iter->second];
mapPointsClean[index] = mapPoints[iter->second];
mapDescriptors.row(iter->second).copyTo(mapDescriptorsClean.row(index));
iter->second = index;
}
mapWordsKpts = mapWordsKptsClean;
mapWordsKptsClean.clear();
mapPoints = mapPointsClean;
mapPointsClean.clear();
mapDescriptors = mapDescriptorsClean;
}
Link * previousLink = 0;
for(std::map<int, int>::iterator iter=bundlePoseReferences_.begin(); iter!=bundlePoseReferences_.end();)
{
if(iter->second <= 0)
{
if(previousLink == 0 || bundleLinks_.find(iter->first) != bundleLinks_.end())
{
if(previousLink)
{
UASSERT(previousLink->to() == iter->first);
*previousLink = previousLink->merge(bundleLinks_.find(iter->first)->second, previousLink->type());
}
UASSERT(bundlePoses_.erase(iter->first) == 1);
bundleLinks_.erase(iter->first);
bundleModels_.erase(iter->first);
bundleIMUOrientations_.erase(iter->first);
bundlePoseReferences_.erase(iter++);
}
}
else
{
previousLink=0;
if(bundleLinks_.find(iter->first) != bundleLinks_.end())
{
previousLink = &bundleLinks_.find(iter->first)->second;
}
++iter;
}
}
}
if(added || removed)
{
modified = true;
}
UDEBUG("Update local features map = %fs", tmpTimer.ticks());
// Geometric
UDEBUG("scankeyframeThr=%f icpInliersRatio=%f", scanKeyFrameThr_, regInfo.icpInliersRatio);
UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);
if(lastFrame_->sensorData().laserScanRaw().size())
{
pcl::PointCloud<pcl::PointXYZINormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudINormal(mapScan, tmpMap.sensorData().laserScanRaw().localTransform());
Transform viewpoint = newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform();
pcl::PointCloud<pcl::PointXYZINormal>::Ptr frameCloudNormals (new pcl::PointCloud<pcl::PointXYZINormal>());
if(scanMapMaxRange_ > 0)
{
frameCloudNormals = util3d::laserScanToPointCloudINormal(lastFrame_->sensorData().laserScanRaw());
frameCloudNormals = util3d::cropBox(frameCloudNormals,
Eigen::Vector4f(-scanMapMaxRange_ / 2, -scanMapMaxRange_ / 2,-scanMapMaxRange_ / 2, 0),
Eigen::Vector4f(scanMapMaxRange_ / 2,scanMapMaxRange_ / 2,scanMapMaxRange_ / 2, 0)
);
frameCloudNormals = util3d::transformPointCloud(frameCloudNormals, viewpoint);
} else
{
frameCloudNormals = util3d::laserScanToPointCloudINormal(lastFrame_->sensorData().laserScanRaw(), viewpoint);
}
pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
int newPoints;
if(mapCloudNormals->size() && scanSubtractRadius_ > 0.0f)
{
// remove points that overlap (the ones found in both clouds)
frameCloudNormalsIndices = util3d::subtractFiltering(
frameCloudNormals,
pcl::IndicesPtr(new std::vector<int>),
mapCloudNormals,
pcl::IndicesPtr(new std::vector<int>),
scanSubtractRadius_,
lastFrame_->sensorData().laserScanRaw().hasNormals()&&mapScan.hasNormals()?scanSubtractAngle_:0.0f);
newPoints = frameCloudNormalsIndices->size();
}
else
{
newPoints = frameCloudNormals->size();
}
if(newPoints)
{
if (scanMapMaxRange_ > 0) {
// Copying new points to tmp cloud
// These are the points that have no overlap between mapScan and lastFrame
pcl::PointCloud<pcl::PointXYZINormal> tmp;
pcl::copyPointCloud(*frameCloudNormals, *frameCloudNormalsIndices, tmp);
if (int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_) // 20 000 points
{
// Print mapSize
UINFO("mapSize=%d newPoints=%d maxPoints=%d",
int(mapCloudNormals->size()),
newPoints,
scanMaximumMapSize_);
*mapCloudNormals += tmp;
cv::Point3f boxMin (-scanMapMaxRange_/2, -scanMapMaxRange_/2, -scanMapMaxRange_/2);
cv::Point3f boxMax (scanMapMaxRange_/2, scanMapMaxRange_/2, scanMapMaxRange_/2);
boxMin = util3d::transformPoint(boxMin, viewpoint.translation());
boxMax = util3d::transformPoint(boxMax, viewpoint.translation());
mapCloudNormals = util3d::cropBox(mapCloudNormals, Eigen::Vector4f(boxMin.x, boxMin.y, boxMin.z, 0 ), Eigen::Vector4f(boxMax.x, boxMax.y, boxMax.z, 0 ));
} else {
*mapCloudNormals += tmp;
}
mapCloudNormals = util3d::voxelize(mapCloudNormals, scanSubtractRadius_);
pcl::PointCloud<pcl::PointXYZI>::Ptr mapCloud (new pcl::PointCloud<pcl::PointXYZI> ());
copyPointCloud(*mapCloudNormals, *mapCloud);
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(mapCloud, pointToPlaneK_, pointToPlaneRadius_, Eigen::Vector3f(viewpoint.x(), viewpoint.y(), viewpoint.z()));
copyPointCloud(*normals, *mapCloudNormals);
} else {
scansBuffer_.push_back(std::make_pair(frameCloudNormals, frameCloudNormalsIndices));
//remove points if too big
UDEBUG("scansBuffer=%d, mapSize=%d newPoints=%d maxPoints=%d",
(int)scansBuffer_.size(),
int(mapCloudNormals->size()),
newPoints,
scanMaximumMapSize_);
if(scansBuffer_.size() > 1 &&
int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_)
{
//regenerate the local map
mapCloudNormals->clear();
std::list<int> toRemove;
int i = int(scansBuffer_.size())-1;
for(; i>=0; --i)
{
int pointsToAdd = scansBuffer_[i].second->size()?scansBuffer_[i].second->size():scansBuffer_[i].first->size();
if((int)mapCloudNormals->size() + pointsToAdd > scanMaximumMapSize_ ||
i == 0)
{
*mapCloudNormals += *scansBuffer_[i].first;
break;
}
else
{
if(scansBuffer_[i].second->size())
{
pcl::PointCloud<pcl::PointXYZINormal> tmp;
pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
*mapCloudNormals += tmp;
}
else
{
*mapCloudNormals += *scansBuffer_[i].first;
}
}
}
// remove old clouds
if(i > 0)
{
std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
int oi = 0;
for(; i<(int)scansBuffer_.size(); ++i)
{
UASSERT(oi < (int)scansTmp.size());
scansTmp[oi++] = scansBuffer_[i];
}
scansBuffer_ = scansTmp;
}
}
else
{
// just append the last cloud
if(scansBuffer_.back().second->size())
{
pcl::PointCloud<pcl::PointXYZINormal> tmp;
pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
*mapCloudNormals += tmp;
}
else
{
*mapCloudNormals += *scansBuffer_.back().first;
}
}
}
if(mapScan.is2d())
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
mapScan = LaserScan(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f);
}
else
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
mapScan = LaserScan(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f);
}
modified=true;
}
}
UDEBUG("Update local scan map = %fs", tmpTimer.ticks());
}
if(modified)
{
*map_ = tmpMap;
if(mapScan.is2d())
{
map_->sensorData().setLaserScan(
LaserScan(
mapScan.data(),
0,
0.0f,
mapScan.format(),
Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
}
else
{
map_->sensorData().setLaserScan(
LaserScan(
mapScan.data(),
0,
0.0f,
mapScan.format(),
newFramePose.translation()));
}
map_->setWords(mapWords, mapWordsKpts, mapPoints, mapDescriptors);
}
if(lastFrameWords3Updated)
{
// update output with refined 3d points from bundle adjustment
data.setFeatures(lastFrame_->getWordsKpts(), lastFrameWords3, lastFrame_->getWordsDescriptors());
}
}
if(info)
{
// use tmpMap instead of map_ to make sure that correspondences with the new frame matches
info->localMapSize = (int)tmpMap.getWords3().size();
info->localScanMapSize = tmpMap.sensorData().laserScanRaw().size();
if(this->isInfoDataFilled())
{
info->localMap.clear();
if(!tmpMap.getWords3().empty())
{
for(std::multimap<int, int>::const_iterator iter=tmpMap.getWords().begin(); iter!=tmpMap.getWords().end(); ++iter)
{
info->localMap.insert(std::make_pair(iter->first, tmpMap.getWords3()[iter->second]));
}
}
info->localScanMap = tmpMap.sensorData().laserScanRaw();
}
}
}
else
{
// Just generate keypoints for the new signature
// For scan, we want to use reading filters, so set dummy's scan and set back to reference afterwards
Signature dummy;
dummy.sensorData().setLaserScan(lastFrame_->sensorData().laserScanRaw());
lastFrame_->sensorData().setLaserScan(LaserScan());
regPipeline_->computeTransformationMod(
*lastFrame_,
dummy);
lastFrame_->sensorData().setLaserScan(dummy.sensorData().laserScanRaw());
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
data.setLaserScan(lastFrame_->sensorData().laserScanRaw());
// a very high variance tells that the new pose is not linked with the previous one
regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
bool frameValid = false;
Transform newFramePose = this->getPose(); // initial pose may be not identity...
if(regPipeline_->isImageRequired())
{
int ptsWithDepth = 0;
for (std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin();
iter != lastFrame_->getWords().end();
++iter)
{
if(!lastFrame_->getWords3().empty() &&
util3d::isFinite(lastFrame_->getWords3()[iter->second]))
{
++ptsWithDepth;
}
}
if (ptsWithDepth >= regPipeline_->getMinVisualCorrespondences())
{
frameValid = true;
// update local map
UASSERT_MSG(lastFrame_->getWordsDescriptors().rows == (int)lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().rows, (int)lastFrame_->getWords3().size()).c_str());
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
std::multimap<int, int> words;
std::vector<cv::KeyPoint> wordsKpts;
std::vector<cv::Point3f> transformedPoints;
std::multimap<int, int> mapPointWeights;
cv::Mat descriptors;
if(!lastFrame_->getWords3().empty() && !lastFrameModels.empty())
{
for (std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin();
iter != lastFrame_->getWords().end();
++iter)
{
const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
if (util3d::isFinite(pt))
{
words.insert(words.end(), std::make_pair(iter->first, words.size()));
wordsKpts.push_back(lastFrame_->getWordsKpts()[iter->second]);
transformedPoints.push_back(util3d::transformPoint(pt, newFramePose));
mapPointWeights.insert(std::make_pair(iter->first, 0));
descriptors.push_back(lastFrame_->getWordsDescriptors().row(iter->second));
}
}
}
if(bundleAdjustment_>0)
{
// update bundleWordReferences_: used for bundle adjustment
if(!wordsKpts.empty())
{
for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
if(words.count(iter->first) == 1)
{
UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
std::map<int, FeatureBA> framePt;
cv::KeyPoint kpt = wordsKpts[iter->second];
int cameraIndex = 0;
if(lastFrameModels.size()>1)
{
UASSERT(lastFrameModels[0].imageWidth()>0);
float subImageWidth = lastFrameModels[0].imageWidth();
cameraIndex = int(kpt.pt.x / subImageWidth);
kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
}
//get depth
float d = 0.0f;
if(lastFrame_->getWords().count(iter->first) == 1 &&
!lastFrame_->getWords3().empty() &&
util3d::isFinite(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second]))
{
//move back point in camera frame (to get depth along z)
d = util3d::transformPoint(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second], lastFrameModels[cameraIndex].localTransform().inverse()).z;
}
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, d, cv::Mat(), cameraIndex)));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
}
}
bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), (int)bundleWordReferences_.size()));
bundleModels_.insert(std::make_pair(lastFrame_->id(), lastFrameModels));
bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
if(!imuT.isNull())
{
bundleIMUOrientations_.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, newFramePose)));
}
}
map_->setWords(words, wordsKpts, transformedPoints, descriptors);
addKeyFrame = true;
}
else
{
UWARN("%d visual features required to initialize the odometry (only %d extracted).", regPipeline_->getMinVisualCorrespondences(), (int)lastFrame_->getWords3().size());
}
}
if(regPipeline_->isScanRequired())
{
if (lastFrame_->sensorData().laserScanRaw().size())
{
pcl::PointCloud<pcl::PointXYZINormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudINormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());
double complexity = 0.0;;
if(!frameValid)
{
float minComplexity = Parameters::defaultIcpPointToPlaneMinComplexity();
bool p2n = Parameters::defaultIcpPointToPlane();
Parameters::parse(parameters_, Parameters::kIcpPointToPlane(), p2n);
Parameters::parse(parameters_, Parameters::kIcpPointToPlaneMinComplexity(), minComplexity);
if(p2n && minComplexity>0.0f)
{
if(lastFrame_->sensorData().laserScanRaw().hasNormals())
{
complexity = util3d::computeNormalsComplexity(*mapCloudNormals, Transform::getIdentity(), lastFrame_->sensorData().laserScanRaw().is2d());
if(complexity > minComplexity)
{
frameValid = true;
}
else if(!guess.isNull() && !guess.isIdentity())
{
UWARN("Scan complexity too low (%f) to init robustly the first "
"keyframe. Make sure the lidar is seeing enough "
"geometry in all axes for good initialization. "
"Accepting as an initial guess (%s) is provided.",
complexity,
guess.prettyPrint().c_str());
frameValid = true;
}
}
else
{
UWARN("Input raw scan doesn't have normals, complexity check on first frame is not done.");
frameValid = true;
}
}
else
{
frameValid = true;
}
}
const int scanMaxPoints = lastFrame_->sensorData().laserScanRaw().maxPoints();
if(frameValid && scanMaxPoints > 0)
{
float correspondenceRatio = Parameters::defaultIcpCorrespondenceRatio();
Parameters::parse(parameters_, Parameters::kIcpCorrespondenceRatio(), correspondenceRatio);
if(float(lastFrame_->sensorData().laserScanRaw().size()) <
float(scanMaxPoints) * correspondenceRatio)
{
UWARN("Scan has %d points of the %d of a full sweep, under the %s=%f "
"that a registration against it would have to reach, so no "
"later scan could be matched to it. Not initializing on it.",
(int)lastFrame_->sensorData().laserScanRaw().size(),
scanMaxPoints,
Parameters::kIcpCorrespondenceRatio().c_str(),
correspondenceRatio);
frameValid = false;
}
}
if(frameValid)
{
if (scanMapMaxRange_ > 0 ){
UINFO("Local map will be updated using range instead of time with range threshold set at %f", scanMapMaxRange_);
} else {
scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
}
if(lastFrame_->sensorData().laserScanRaw().is2d())
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
map_->sensorData().setLaserScan(
LaserScan(
util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
0,
0.0f,
Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
}
else
{
Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
map_->sensorData().setLaserScan(
LaserScan(
util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
0,
0.0f,
newFramePose.translation()));
}
addKeyFrame = true;
}
else
{
UWARN("Scan complexity too low (%f) to init first keyframe.", complexity);
}
}
else
{
UWARN("Missing scan to initialize odometry.");
}
}
if (frameValid)
{
// We initialized the local map
output.setIdentity();
}
if(info)
{
info->localMapSize = (int)map_->getWords3().size();
info->localScanMapSize = map_->sensorData().laserScanRaw().size();
if(this->isInfoDataFilled())
{
info->localMap.clear();
if(!map_->getWords3().empty())
{
for(std::multimap<int, int>::const_iterator iter=map_->getWords().begin(); iter!=map_->getWords().end(); ++iter)
{
info->localMap.insert(std::make_pair(iter->first, map_->getWords3()[iter->second]));
}
}
info->localScanMap = map_->sensorData().laserScanRaw();
}
}
}
map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat()); // clear sensorData features
nFeatures = lastFrame_->getWords().size();
if(this->isInfoDataFilled() && info)
{
if(regPipeline_->isImageRequired())
{
info->words.clear();
if(!lastFrame_->getWordsKpts().empty())
{
for(std::multimap<int, int>::const_iterator iter=lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
{
info->words.insert(std::make_pair(iter->first, lastFrame_->getWordsKpts()[iter->second]));
}
}
}
}
}
else
{
UERROR("SensorData not valid!");
}
if(info)
{
info->features = nFeatures;
info->localKeyFrames = (int)bundlePoses_.size();
info->keyFrameAdded = addKeyFrame;
info->localBundleOutliers = totalBundleOutliers;
info->localBundleConstraints = totalBundleWordReferencesUsed;
info->localBundleTime = bundleTime;
if(this->isInfoDataFilled())
{
info->reg = regInfo;
}
else
{
info->reg = regInfo.copyWithoutData();
}
if(output.isNull())
{
info->reg.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0; // Lost
}
}
UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance:lin=%f, ang=%f local_map=%d local_scan_map=%d",
timer.elapsed(),
output.isNull()?"true":"false",
nFeatures,
regInfo.inliers,
regInfo.matches,
!regInfo.covariance.empty()?regInfo.covariance.at<double>(0,0):0,
!regInfo.covariance.empty()?regInfo.covariance.at<double>(5,5):0,
regPipeline_->isImageRequired()?(int)map_->getWords3().size():0,
regPipeline_->isScanRequired()?(int)map_->sensorData().laserScanRaw().size():0);
return output;
}
} // namespace rtabmap
Reference in New Issue View Git Blame Copy Permalink