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

0.15.1: CameraImages: added ground truth time diff, fixed memory leak when loading binary scans. CameraRGBDImages and CameraStereoImages: fixed start id. Feature2D: added grid rows and cols parameters (Kp/GridRows, Kp/GridCols, Vis/GridRows, Vis/GridCols). OdomInfo: publish bundle frames. OdometryORBSLAM2: added OdomORBSLAM2/Fps and OdomORBSLAM2/MaxFeatures parameters. Registration: added Reg/RepeatOnce parameter and removed variance normalization. For util2d::getDepth() and util3d::projectDepthTo3D(), maxZError parameter is now depthErrorRatio to be dependent of the sensor range. Database: save image width and height from stereo calibration. OptimizerG2O: fixed SBA optimization when using g2o built from ORBSLAM2 library. OptimizerGTSAM: to increase optimization stability, all rotations in information matrix are divided by 100000. Added rtabmap-report tool.

Browse Source
This commit is contained in:
matlabbe
2017-11-30 16:52:03 -05:00
parent 821c1c938e
commit 4452e637ad
46 changed files with 1892 additions and 802 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
corelib/src/Camera.cpp
View File

@@ -59,6 +59,11 @@ Camera::~Camera()
UDEBUG("");
}
void Camera::resetTimer()
{
_frameRateTimer->start();
}
SensorData Camera::takeImage(CameraInfo * info)
{
bool warnFrameRateTooHigh = false;

102
corelib/src/CameraRGB.cpp
View File

@@ -78,6 +78,7 @@ CameraImages::CameraImages() :
_syncImageRateWithStamps(true),
_odometryFormat(0),
_groundTruthFormat(0),
_maxPoseTimeDiff(0.02),
_captureDelay(0.0)
{}
CameraImages::CameraImages(const std::string & path,
@@ -108,6 +109,7 @@ CameraImages::CameraImages(const std::string & path,
_syncImageRateWithStamps(true),
_odometryFormat(0),
_groundTruthFormat(0),
_maxPoseTimeDiff(0.02),
_captureDelay(0.0)
{
@@ -312,12 +314,12 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
if(success && _odometryPath.size())
{
success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat);
success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat, _maxPoseTimeDiff);
}
if(success && _groundTruthPath.size())
{
success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat);
success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat, _maxPoseTimeDiff);
}
}
@@ -326,7 +328,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
return success;
}
bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<double> & inOutStamps, const std::string & filePath, int format) const
bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<double> & inOutStamps, const std::string & filePath, int format, double maxTimeDiff) const
{
outputPoses.clear();
std::map<int, Transform> poses;
@@ -380,16 +382,21 @@ bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<doubl
double stampBeg = beginIter->first;
double stampEnd = endIter->first;
UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
if(stampEnd - stampBeg > 10.0)
if(fabs(*ster-stampEnd) > maxTimeDiff || fabs(*ster-stampBeg) > maxTimeDiff)
{
warned = true;
UDEBUG("Cannot interpolate pose for stamp %f between %f and %f (>10 sec)",
*ster,
stampBeg,
stampEnd);
if(!warned)
{
UWARN("Cannot interpolate pose for stamp %f between %f and %f (> maximum time diff of %f sec)",
*ster,
stampBeg,
stampEnd,
maxTimeDiff);
}
}
else
{
warned=false;
float t = (*ster - stampBeg) / (stampEnd-stampBeg);
Transform & ta = poses.at(beginIter->second);
Transform & tb = poses.at(endIter->second);
@@ -533,6 +540,26 @@ SensorData CameraImages::captureImage(CameraInfo * info)
}
}
}
if(_stamps.size())
{
stamp = _stamps.front();
_stamps.pop_front();
if(_stamps.size())
{
_captureDelay = _stamps.front() - stamp;
}
if(odometry_.size())
{
odometryPose = odometry_.front();
odometry_.pop_front();
}
if(groundTruth_.size())
{
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
}
}
}
else
{
@@ -541,9 +568,48 @@ SensorData CameraImages::captureImage(CameraInfo * info)
if(!fileName.empty())
{
imageFilePath = _path + fileName;
if(_stamps.size())
{
stamp = _stamps.front();
_stamps.pop_front();
if(_stamps.size())
{
_captureDelay = _stamps.front() - stamp;
}
if(odometry_.size())
{
odometryPose = odometry_.front();
odometry_.pop_front();
}
if(groundTruth_.size())
{
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
}
}
while(_count++ < _startAt && (fileName = _dir->getNextFileName()).size())
{
imageFilePath = _path + fileName;
if(_stamps.size())
{
stamp = _stamps.front();
_stamps.pop_front();
if(_stamps.size())
{
_captureDelay = _stamps.front() - stamp;
}
if(odometry_.size())
{
odometryPose = odometry_.front();
odometry_.pop_front();
}
if(groundTruth_.size())
{
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
}
}
}
}
if(_scanDir)
@@ -560,26 +626,6 @@ SensorData CameraImages::captureImage(CameraInfo * info)
}
}
if(_stamps.size())
{
stamp = _stamps.front();
_stamps.pop_front();
if(_stamps.size())
{
_captureDelay = _stamps.front() - stamp;
}
if(odometry_.size())
{
odometryPose = odometry_.front();
odometry_.pop_front();
}
if(groundTruth_.size())
{
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
}
}
if(!imageFilePath.empty())
{
ULOGGER_DEBUG("Loading image : %s", imageFilePath.c_str());

1
corelib/src/CameraThread.cpp
View File

@@ -122,6 +122,7 @@ void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
void CameraThread::mainLoopBegin()
{
ULogger::registerCurrentThread("Camera");
_camera->resetTimer();
}
void CameraThread::mainLoop()

52
corelib/src/DBDriverSqlite3.cpp
View File

@@ -1406,6 +1406,19 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
localTransform));
}
}
else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform,
cv::Size(dataFloat[5],dataFloat[6]));
}
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
@@ -1707,6 +1720,19 @@ bool DBDriverSqlite3::getCalibrationQuery(
localTransform));
}
}
else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform,
cv::Size(dataFloat[5],dataFloat[6]));
}
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
@@ -2667,7 +2693,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
// multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
// stereo [fx, fy, cx, cy, baseline, [width,height], local_transform] (5or7+12)*float
if(dataSize > 0 && data)
{
float * dataFloat = (float*)data;
@@ -2687,8 +2713,9 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
localTransform,
0,
cv::Size(dataFloat[i+4], dataFloat[i+5])));
UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
localTransform.prettyPrint().c_str());
@@ -2713,6 +2740,19 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
localTransform));
}
}
else if((unsigned int)dataSize == (7+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform,
cv::Size(dataFloat[5], dataFloat[6]));
}
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
@@ -4805,13 +4845,15 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
else if(sensorData.stereoCameraModel().isValidForProjection())
{
const Transform & localTransform = sensorData.stereoCameraModel().left().localTransform();
calibration.resize(5+localTransform.size());
calibration.resize(7+localTransform.size());
calibration[0] = sensorData.stereoCameraModel().left().fx();
calibration[1] = sensorData.stereoCameraModel().left().fy();
calibration[2] = sensorData.stereoCameraModel().left().cx();
calibration[3] = sensorData.stereoCameraModel().left().cy();
calibration[4] = sensorData.stereoCameraModel().baseline();
memcpy(calibration.data()+5, localTransform.data(), localTransform.size()*sizeof(float));
calibration[5] = sensorData.stereoCameraModel().left().imageWidth();
calibration[6] = sensorData.stereoCameraModel().left().imageHeight();
memcpy(calibration.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
}
if(calibration.size())

47
corelib/src/Features2d.cpp
View File

@@ -327,7 +327,9 @@ Feature2D::Feature2D(const ParametersMap & parameters) :
_roiRatios(std::vector<float>(4, 0.0f)),
_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
_subPixIterations(Parameters::defaultKpSubPixIterations()),
_subPixEps(Parameters::defaultKpSubPixEps())
_subPixEps(Parameters::defaultKpSubPixEps()),
gridRows_(Parameters::defaultKpGridRows()),
gridCols_(Parameters::defaultKpGridCols())
{
_stereo = new Stereo(parameters);
this->parseParameters(parameters);
@@ -346,6 +348,14 @@ void Feature2D::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
Parameters::parse(parameters, Parameters::kKpGridRows(), gridRows_);
Parameters::parse(parameters, Parameters::kKpGridCols(), gridCols_);
UASSERT(gridRows_ >= 1 && gridCols_>=1);
if(maxFeatures_ > 0)
{
maxFeatures_ = maxFeatures_ / (gridRows_ * gridCols_);
}
// convert ROI from string to vector
ParametersMap::const_iterator iter;
@@ -533,23 +543,36 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
std::vector<cv::KeyPoint> keypoints;
UTimer timer;
cv::Rect globalRoi = Feature2D::computeRoi(image, _roiRatios);
if(!(globalRoi.width && globalRoi.height))
{
globalRoi = cv::Rect(0,0,image.cols, image.rows);
}
// Get keypoints
cv::Rect roi = Feature2D::computeRoi(image, _roiRatios);
keypoints = this->generateKeypointsImpl(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows), mask);
UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (mask empty=%d)", timer.ticks(), keypoints.size(), mask.empty()?1:0);
limitKeypoints(keypoints, maxFeatures_);
if(roi.x || roi.y)
int rowSize = globalRoi.height / gridRows_;
int colSize = globalRoi.width / gridCols_;
for (int i = 0; i<gridRows_; ++i)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
for (int j = 0; j<gridCols_; ++j)
{
iter->pt.x += roi.x;
iter->pt.y += roi.y;
cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize);
std::vector<cv::KeyPoint> sub_keypoints;
sub_keypoints = this->generateKeypointsImpl(image, roi, mask);
limitKeypoints(sub_keypoints, maxFeatures_);
if(roi.x || roi.y)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=sub_keypoints.begin(); iter!=sub_keypoints.end(); ++iter)
{
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
}
keypoints.insert( keypoints.end(), sub_keypoints.begin(), sub_keypoints.end() );
}
}
UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (mask empty=%d)", timer.ticks(), keypoints.size(), mask.empty()?1:0);
if(keypoints.size() && _subPixWinSize > 0 && _subPixIterations > 0)
{

9
corelib/src/Memory.cpp
View File

@@ -2251,7 +2251,7 @@ Transform Memory::computeTransform(
if(fromS && toS)
{
return computeTransform(*fromS, *toS, guess, info, useKnownCorrespondencesIfPossible);
transform = computeTransform(*fromS, *toS, guess, info, useKnownCorrespondencesIfPossible);
}
else
{
@@ -3515,7 +3515,12 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
else if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode)
{
UINFO("Use odometry features");
UINFO("Use odometry features: kpts=%d 3d=%d desc=%d (dim=%d, type=%d)",
(int)data.keypoints().size(),
(int)data.keypoints3D().size(),
data.descriptors().rows,
data.descriptors().cols,
data.descriptors().type());
keypoints = data.keypoints();
keypoints3D = data.keypoints3D();
descriptors = data.descriptors().clone();

5
corelib/src/Odometry.cpp
View File

@@ -286,7 +286,8 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
}
double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
// KITTI datasets start with stamp=0
double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
Transform guess = dt>0.0 && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform();
if(!(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull())))
{
@@ -528,7 +529,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
}
if(data.stamp() == 0)
if(data.stamp() == 0 && framesProcessed_ != 0)
{
UWARN("Null stamp detected");
}

6
corelib/src/OdometryDVO.cpp
View File

@@ -216,8 +216,10 @@ Transform OdometryDVO::computeTransform(
lost_ = false;
cv::Mat information = cv::Mat::eye(6,6, CV_64FC1);
memcpy(information.data, result.Information.data(), 36*sizeof(double));
covariance = information.inv();
covariance *= 100.0; // to be in the same scale than loop closure detection
//copy only diagonal to avoid g2o/gtsam errors on graph optimization
covariance = cv::Mat::eye(6,6,CV_64FC1);
covariance = information.inv().mul(covariance);
//covariance *= 100.0; // to be in the same scale than loop closure detection
Transform currentMotion = t;
t = motionFromKeyFrame_.inverse() * t;

158
corelib/src/OdometryF2M.cpp
View File

@@ -111,7 +111,27 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
UASSERT(maxNewFeatures_ >= 0);
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)));
regPipeline_ = Registration::create(bundleParameters);
if(bundleAdjustment_>0 && !regPipeline_->isImageRequired())
{
UWARN("%s=%d cannot be used with registration not done 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;
}
}
OdometryF2M::~OdometryF2M()
@@ -188,6 +208,7 @@ Transform OdometryF2M::computeTransform(
lastFrame_->sensorData().isValid())
{
Signature tmpMap = *map_;
UDEBUG("guess=%s frames=%d image required=%d", guess.prettyPrint().c_str(), this->framesProcessed(), regPipeline_->isImageRequired()?1:0);
Transform transform = regPipeline_->computeTransformationMod(
tmpMap,
*lastFrame_,
@@ -255,10 +276,35 @@ Transform OdometryF2M::computeTransform(
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())));
cv::Mat var = regInfo.covariance;//cv::Mat::eye(6,6,CV_64FC1); //regInfo.covariance.inv()
//var(cv::Range(0,3), cv::Range(0,3)) *= 0.001;
//var(cv::Range(3,6), cv::Range(3,6)) *= 0.001;
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, var.inv())));
bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
CameraModel model;
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
model = lastFrame_->sensorData().cameraModels()[0];
}
else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
{
model = lastFrame_->sensorData().stereoCameraModel().left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
}
else
{
UFATAL("no valid camera model!");
}
bundleModels.insert(std::make_pair(lastFrame_->id(), model));
Transform invLocalTransform = model.localTransform().inverse();
UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
std::map<int, std::map<int, cv::Point3f> > wordReferences;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
@@ -300,7 +346,9 @@ Transform OdometryF2M::computeTransform(
if(iter2D!=lastFrame_->getWords().end())
{
UASSERT(lastFrame_->getWords3().find(wordId) != lastFrame_->getWords3().end());
references.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, lastFrame_->getWords3().find(wordId)->second.x)));
//move back point in camera frame (to get depth along z)
cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(wordId)->second, invLocalTransform);
references.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
}
wordReferences.insert(std::make_pair(wordId, references));
@@ -311,28 +359,6 @@ Transform OdometryF2M::computeTransform(
//}
}
CameraModel model;
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
model = lastFrame_->sensorData().cameraModels()[0];
}
else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
{
model = lastFrame_->sensorData().stereoCameraModel().left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
}
else
{
UFATAL("no valid camera model!");
}
bundleModels.insert(std::make_pair(lastFrame_->id(), model));
UDEBUG("sba...start");
// set root negative to fix all other poses
std::set<int> sbaOutliers;
@@ -343,6 +369,11 @@ Transform OdometryF2M::computeTransform(
totalBundleOutliers = (int)sbaOutliers.size();
UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), (int)sbaOutliers.size());
if(info)
{
info->localBundlePoses = bundlePoses;
info->localBundleModels = bundleModels;
}
UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
if(bundlePoses.size() == bundlePoses_.size()+1)
@@ -406,7 +437,7 @@ Transform OdometryF2M::computeTransform(
bool addVisualKeyFrame = regPipeline_->isImageRequired() &&
(keyFrameThr_ == 0.0f ||
visKeyFrameThr_ == 0 ||
float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())) ||
float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
regInfo.inliers <= visKeyFrameThr_);
bool addGeometricKeyFrame = regPipeline_->isScanRequired() && (scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);
@@ -455,6 +486,22 @@ Transform OdometryF2M::computeTransform(
std::multimap<int, cv::Mat>::const_iterator iterDesc = lastFrame_->getWordsDescriptors().begin();
UDEBUG("new frame words3=%d", (int)lastFrame_->getWords3().size());
std::set<int> seenStatusUpdated;
Transform invLocalTransform;
if(bundleAdjustment_>0)
{
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
invLocalTransform = lastFrame_->sensorData().cameraModels()[0].localTransform().inverse();
}
else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
{
invLocalTransform = lastFrame_->sensorData().stereoCameraModel().left().localTransform().inverse();
}
else
{
UFATAL("no valid camera model!");
}
}
for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
{
if(util3d::isFinite(iter->second))
@@ -480,16 +527,17 @@ Transform OdometryF2M::computeTransform(
UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
iterBundlePosesRef->second += 1;
//move back point in camera frame (to get depth along z)
cv::Point3f pt3d = util3d::transformPoint(iter->second, invLocalTransform);
if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
{
std::map<int, cv::Point3f> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, iter->second.x)));
framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
else
{
bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, iter->second.x)));
bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
}
}
}
@@ -510,15 +558,17 @@ Transform OdometryF2M::computeTransform(
UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
iterBundlePosesRef->second += 1;
//move back point in camera frame (to get depth along z)
cv::Point3f pt3d = util3d::transformPoint(iter->second.second.second.first, invLocalTransform);
if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
{
std::map<int, cv::Point3f> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, iter->second.second.second.first.x)));
framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, pt3d.z)));
bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
}
else
{
bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, iter->second.second.second.first.x)));
bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, pt3d.z)));
}
}
}
@@ -568,18 +618,31 @@ Transform OdometryF2M::computeTransform(
}
}
Link * previousLink = 0;
for(std::map<int, int>::iterator iter=bundlePoseReferences_.begin(); iter!=bundlePoseReferences_.end();)
{
if((iter->second <= 0 && // <= regPipeline_->getMinVisualCorrespondences() &&
bundlePoses_.begin()->first == iter->first)) // remove oldest pose first
if(iter->second <= 0)
{
UASSERT(bundlePoses_.erase(iter->first) == 1);
bundleLinks_.erase(iter->first);
bundleModels_.erase(iter->first);
bundlePoseReferences_.erase(iter++);
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);
bundlePoseReferences_.erase(iter++);
}
}
else
{
previousLink=0;
if(bundleLinks_.find(iter->first) != bundleLinks_.end())
{
previousLink = &bundleLinks_.find(iter->first)->second;
}
++iter;
}
}
@@ -765,6 +828,7 @@ Transform OdometryF2M::computeTransform(
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, cv::Point3f> transformedPoints;
std::multimap<int, int> mapPointWeights;
std::multimap<int, cv::Mat> descriptors;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
@@ -777,12 +841,27 @@ Transform OdometryF2M::computeTransform(
{
words.insert(*wordsIter);
transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
mapPointWeights.insert(std::make_pair(iter->first, 0));
descriptors.insert(*descIter);
}
}
if(bundleAdjustment_>0)
{
Transform invLocalTransform;
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
invLocalTransform = lastFrame_->sensorData().cameraModels()[0].localTransform().inverse();
}
else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
{
invLocalTransform = lastFrame_->sensorData().stereoCameraModel().left().localTransform().inverse();
}
else
{
UFATAL("no valid camera model!");
}
// update bundleWordReferences_: used for bundle adjustment
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
@@ -795,9 +874,12 @@ Transform OdometryF2M::computeTransform(
float d = 0.0f;
if(lastFrame_->getWords3().count(iter->first) == 1)
{
d = lastFrame_->getWords3().find(iter->first)->second.x;
//move back point in camera frame (to get depth along z)
cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(iter->first)->second, invLocalTransform);
d = pt3d.z;
}
framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.pt.x, iter->second.pt.y, d)));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}

4
corelib/src/OdometryFovis.cpp
View File

@@ -123,6 +123,7 @@ Transform OdometryFovis::computeTransform(
const Transform & guess,
OdometryInfo * info)
{
UDEBUG("");
Transform t;
#ifdef RTABMAP_FOVIS
@@ -205,6 +206,7 @@ Transform OdometryFovis::computeTransform(
Transform localTransform = Transform::getIdentity();
if(data.cameraModels().size() == 1) //depth
{
UDEBUG("");
fovis::CameraIntrinsicsParameters rgb_params;
memset(&rgb_params, 0, sizeof(fovis::CameraIntrinsicsParameters));
rgb_params.width = data.cameraModels()[0].imageWidth();
@@ -263,6 +265,7 @@ Transform OdometryFovis::computeTransform(
}
else // stereo
{
UDEBUG("");
// initialize left camera parameters
fovis::CameraIntrinsicsParameters left_parameters;
left_parameters.width = data.stereoCameraModel().left().imageWidth();
@@ -331,6 +334,7 @@ Transform OdometryFovis::computeTransform(
fovis_ = new fovis::VisualOdometry(rect_, options);
}
UDEBUG("");
fovis_->processFrame(gray.data, depthSource);
// get the motion estimate for this frame to the previous frame.

35
corelib/src/OdometryORBSLAM2.cpp
View File

@@ -591,6 +591,11 @@ public:
ofs << "Camera.RGB: 1" << std::endl;
ofs << std::endl;
float fps = rtabmap::Parameters::defaultOdomORBSLAM2Fps();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2Fps(), fps);
ofs << "Camera.fps: " << fps << std::endl;
ofs << std::endl;
//# Close/Far threshold. Baseline times.
double thDepth = rtabmap::Parameters::defaultOdomORBSLAM2ThDepth();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2ThDepth(), thDepth);
@@ -605,8 +610,8 @@ public:
//# ORB Parameters
//#--------------------------------------------------------------------------------------------
//# ORB Extractor: Number of features per image
int features = rtabmap::Parameters::defaultVisMaxFeatures();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kVisMaxFeatures(), features);
int features = rtabmap::Parameters::defaultOdomORBSLAM2MaxFeatures();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2MaxFeatures(), features);
ofs << "ORBextractor.nFeatures: " << features << std::endl;
ofs << std::endl;
@@ -869,14 +874,26 @@ Transform OdometryORBSLAM2::computeTransform(
}
else
{
//based on values set in viso2_ros
float baseline = data.stereoCameraModel().baseline();
if(baseline <= 0.0f)
{
baseline = rtabmap::Parameters::defaultOdomORBSLAM2Bf();
rtabmap::Parameters::parse(orbslam2_->parameters_, rtabmap::Parameters::kOdomORBSLAM2Bf(), baseline);
}
double linearVar = 0.0001;
if(baseline > 0.0f)
{
linearVar = baseline/8.0;
linearVar *= linearVar;
}
covariance = cv::Mat::eye(6,6, CV_64FC1);
covariance.at<double>(0,0) = 0.002;
covariance.at<double>(1,1) = 0.002;
covariance.at<double>(2,2) = 0.05;
covariance.at<double>(3,3) = 0.09;
covariance.at<double>(4,4) = 0.09;
covariance.at<double>(5,5) = 0.09;
covariance.at<double>(0,0) = linearVar;
covariance.at<double>(1,1) = linearVar;
covariance.at<double>(2,2) = linearVar;
covariance.at<double>(3,3) = 0.01;
covariance.at<double>(4,4) = 0.01;
covariance.at<double>(5,5) = 0.01;
}
}

1
corelib/src/OdometryThread.cpp
View File

@@ -107,6 +107,7 @@ void OdometryThread::mainLoop()
if(getData(data))
{
OdometryInfo info;
UDEBUG("Processing data...");
Transform pose = _odometry->process(data, &info);
// a null pose notify that odometry could not be computed
UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());

131
corelib/src/OptimizerG2O.cpp
View File

@@ -646,6 +646,81 @@ std::map<int, Transform> OptimizerG2O::optimize(
return optimizedPoses;
}
#ifdef RTABMAP_ORB_SLAM2
/**
* \brief 3D edge between two SBAcam
*/
class EdgeSE3Expmap : public g2o::BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeSE3Expmap(): BaseBinaryEdge<6, g2o::SE3Quat, g2o::VertexSE3Expmap, g2o::VertexSE3Expmap>(){}
bool read(std::istream& is)
{
return false;
}
bool write(std::ostream& os) const
{
return false;
}
void computeError()
{
const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
g2o::SE3Quat delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
_error[0]=delta.translation().x();
_error[1]=delta.translation().y();
_error[2]=delta.translation().z();
_error[3]=delta.rotation().x();
_error[4]=delta.rotation().y();
_error[5]=delta.rotation().z();
}
virtual void setMeasurement(const g2o::SE3Quat& meas){
_measurement=meas;
_inverseMeasurement=meas.inverse();
}
virtual double initialEstimatePossible(const g2o::OptimizableGraph::VertexSet& , g2o::OptimizableGraph::Vertex* ) { return 1.;}
virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* ){
g2o::VertexSE3Expmap* from = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
g2o::VertexSE3Expmap* to = static_cast<g2o::VertexSE3Expmap*>(_vertices[1]);
if (from_.count(from) > 0)
to->setEstimate((g2o::SE3Quat) from->estimate() * _measurement);
else
from->setEstimate((g2o::SE3Quat) to->estimate() * _inverseMeasurement);
}
virtual bool setMeasurementData(const double* d){
Eigen::Map<const g2o::Vector7d> v(d);
_measurement.fromVector(v);
_inverseMeasurement = _measurement.inverse();
return true;
}
virtual bool getMeasurementData(double* d) const{
Eigen::Map<g2o::Vector7d> v(d);
v = _measurement.toVector();
return true;
}
virtual int measurementDimension() const {return 7;}
virtual bool setMeasurementFromState() {
const g2o::VertexSE3Expmap* v1 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
const g2o::VertexSE3Expmap* v2 = dynamic_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
_measurement = (v1->estimate().inverse()*v2->estimate());
_inverseMeasurement = _measurement.inverse();
return true;
}
protected:
g2o::SE3Quat _inverseMeasurement;
};
#endif
std::map<int, Transform> OptimizerG2O::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
@@ -761,7 +836,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
++iter;
}
#ifndef RTABMAP_ORB_SLAM2
UDEBUG("fill edges to g2o...");
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
@@ -777,23 +851,36 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
UASSERT(!iter->second.transform().isNull());
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
// between cameras, not base_link
Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
UDEBUG("added edge %d->%d (in cam frame=%s)",
id1,
id2,
camLink.prettyPrint().c_str());
Eigen::Affine3d a = camLink.toEigen3d();
//UDEBUG("added edge %d->%d (in cam frame=%s)",
// id1,
// id2,
// camLink.prettyPrint().c_str());
#ifdef RTABMAP_ORB_SLAM2
EdgeSE3Expmap * e = new EdgeSE3Expmap();
g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id1);
g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id2);
Transform camPose1 = Transform::fromEigen3d(v1->estimate()).inverse();
Transform camPose2Inv = Transform::fromEigen3d(v2->estimate());
camLink = camPose1 * camPose1 * camLink * camPose2Inv * camPose2Inv;
#else
g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2);
#endif
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
Eigen::Affine3d a = camLink.toEigen3d();
e->setMeasurement(g2o::SE3Quat(a.linear(), a.translation()));
e->setInformation(information);
@@ -806,7 +893,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
}
}
}
#endif
UDEBUG("fill 3D points to g2o...");
const int stepVertexId = poses.rbegin()->first+1;
@@ -815,7 +901,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
{
if(points3DMap.find(iter->first) != points3DMap.end())
{
const cv::Point3f & pt3d = points3DMap.at(iter->first);
cv::Point3f pt3d = points3DMap.at(iter->first);
g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
@@ -823,7 +909,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
vpt3d->setMarginalized(true);
optimizer.addVertex(vpt3d);
UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);
//UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);
// set observations
for(std::map<int, cv::Point3f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
@@ -834,7 +920,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
const cv::Point3f & pt = jter->second;
double depth = pt.z;
UDEBUG("Added observation pt=%d to cam=%d (%f,%f) d=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);
//UDEBUG("Added observation pt=%d to cam=%d (%f,%f) depth=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);
g2o::OptimizableGraph::Edge * e;
double baseline = 0.0;
@@ -842,18 +928,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
g2o::VertexSE3Expmap* vcam = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
std::map<int, CameraModel>::const_iterator iterModel = models.find(camId);
cv::Point3f t = util3d::transformPoint(pt3d, Transform::fromEigen3d(vcam->estimate()).inverse());
UDEBUG("in cam %d frame=(%f,%f,%f)", camId, t.x, t.y, t.z);
cv::Point3f t2 = util3d::transformPoint(pt3d, (poses.at(camId)*iterModel->second.localTransform()).inverse());
UDEBUG("in cam2 %d frame=(%f,%f,%f)",camId, t2.x, t2.y, t2.z);
g2o::Vector3d t3 = vcam->estimate().map(g2o::Vector3d(pt3d.x, pt3d.y, pt3d.z));
UDEBUG("in cam3 %d frame=(%f,%f,%f)",camId, t3[0], t3[1], t3[2]);
cv::Point3f t4 = util3d::transformPoint(pt3d, (poses.at(camId)*iterModel->second.localTransform()));
UDEBUG("in cam4 %d frame=(%f,%f,%f)",camId, t4.x, t4.y, t4.z);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
baseline = iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_;
#else
@@ -918,7 +992,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
}
e->setVertex(0, vpt3d);
e->setVertex(1, vcam);
UDEBUG("");
if(robustKernelDelta_ > 0.0)
{
g2o::RobustKernelHuber* kernel = new g2o::RobustKernelHuber;
@@ -947,11 +1021,12 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
for(int i=0; i<(robustKernelDelta_>0.0?2:1); ++i)
{
it += optimizer.optimize(i==0&&robustKernelDelta_>0.0?3:iterations());
it += optimizer.optimize(i==0&&robustKernelDelta_>0.0?5:iterations());
// early stop condition
optimizer.computeActiveErrors();
double chi2 = optimizer.activeRobustChi2();
if(uIsNan(chi2))
{
UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current=%d).", optimizer_);
@@ -988,7 +1063,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
#endif
const cv::Point3f & pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
cv::Point3f pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
if(outliers)
@@ -1007,12 +1082,11 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
UDEBUG("outliers=%d outliersCountFar=%d", outliersCount, outliersCountFar);
}
}
UINFO("g2o optimizing end (%d iterations done, error=%f, outliers=%d/%d (delta=%f) time = %f s)", it, optimizer.activeRobustChi2(), outliersCount, (int)edges.size(), robustKernelDelta_, timer.ticks());
if(optimizer.activeRobustChi2() > 1000000000000.0)
{
UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
return optimizedPoses;
}
@@ -1034,6 +1108,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
// remove model local transform
t *= models.at(iter->first).localTransform().inverse();
UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
if(t.isNull())
{

50
corelib/src/OptimizerGTSAM.cpp
View File

@@ -160,16 +160,15 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
information(0,0) = iter->second.infMatrix().at<double>(0,0)/1000.0; // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1)/1000.0; // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5)/1000.0; // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0)/1000.0; // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1)/1000.0; // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5)/1000.0; // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0)/1000.0; // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1)/1000.0; // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5)/1000.0; // theta-theta
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5)/100000.0; // theta-theta
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
@@ -181,8 +180,10 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
information = information / 1000.0;
// Without the following, graph optimization on KITTI06 is very unstable
information(3,3) /= 100000.0;
information(4,4) /= 100000.0;
information(5,5) /= 100000.0;
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
@@ -221,16 +222,15 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
information(0,0) = iter->second.infMatrix().at<double>(0,0)/1000.0; // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1)/1000.0; // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5)/1000.0; // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0)/1000.0; // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1)/1000.0; // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5)/1000.0; // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0)/1000.0; // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1)/1000.0; // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5)/1000.0; // theta-theta
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5)/100000.0; // theta-theta
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
@@ -254,8 +254,10 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
information = information / 1000.0;
// Without the following, graph optimization on KITTI06 is very unstable
information(3,3) /= 100000.0;
information(4,4) /= 100000.0;
information(5,5) /= 100000.0;
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);

14
corelib/src/Parameters.cpp
View File

@@ -224,6 +224,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
if(removedParameters_.empty())
{
// removed parameters
// 0.15.1
removedParameters_.insert(std::make_pair("Reg/VarianceFromInliersCount", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("Reg/VarianceNormalized", std::make_pair(false, "")));
// 0.13.3
removedParameters_.insert(std::make_pair("Icp/PointToPlaneNormalNeighbors", std::make_pair(true, Parameters::kIcpPointToPlaneK())));
@@ -284,7 +287,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
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::kRegForce3DoF())));
removedParameters_.insert(std::make_pair("Odom/VarianceFromInliersCount", std::make_pair(true, Parameters::kRegVarianceFromInliersCount())));
removedParameters_.insert(std::make_pair("Odom/VarianceFromInliersCount", std::make_pair(false, "")));
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())));
@@ -314,7 +317,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
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::kRegForce3DoF())));
removedParameters_.insert(std::make_pair("LccBow/VarianceFromInliersCount", std::make_pair(false, Parameters::kRegVarianceFromInliersCount())));
removedParameters_.insert(std::make_pair("LccBow/VarianceFromInliersCount", std::make_pair(false, "")));
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())));
@@ -732,7 +735,7 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
}
void Parameters::readINI(const std::string & configFile, ParametersMap & parameters)
void Parameters::readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly)
{
CSimpleIniA ini;
ini.LoadFile(configFile.c_str());
@@ -805,7 +808,10 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
if(Parameters::getDefaultParameters().find(key) != Parameters::getDefaultParameters().end())
{
uInsert(parameters, ParametersPair(key, iter->second));
if(!modifiedOnly || std::string(iter->second).compare(Parameters::getDefaultParameters().find(key)->second) != 0)
{
uInsert(parameters, ParametersPair(key, iter->second));
}
}
}
}

72
corelib/src/Registration.cpp
View File

@@ -32,6 +32,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
double Registration::COVARIANCE_EPSILON = 0.000000001;
Registration * Registration::create(const ParametersMap & parameters)
{
int regTypeInt = Parameters::defaultRegStrategy();
@@ -61,8 +63,7 @@ Registration * Registration::create(Registration::Type & type, const ParametersM
}
Registration::Registration(const ParametersMap & parameters, Registration * child) :
varianceFromInliersCount_(Parameters::defaultRegVarianceFromInliersCount()),
covarianceNormalized_(Parameters::defaultRegVarianceNormalized()),
repeatOnce_(Parameters::defaultRegRepeatOnce()),
force3DoF_(Parameters::defaultRegForce3DoF()),
child_(child)
{
@@ -78,9 +79,9 @@ Registration::~Registration()
}
void Registration::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), varianceFromInliersCount_);
Parameters::parse(parameters, Parameters::kRegVarianceNormalized(), covarianceNormalized_);
Parameters::parse(parameters, Parameters::kRegRepeatOnce(), repeatOnce_);
Parameters::parse(parameters, Parameters::kRegForce3DoF(), force3DoF_);
if(child_)
{
child_->parseParameters(parameters);
@@ -194,26 +195,29 @@ Transform Registration::computeTransformationMod(
}
Transform t = computeTransformationImpl(from, to, guess, info);
if(repeatOnce_ && guess.isNull() && !t.isNull())
{
// redo with guess to get a more accurate transform
t = computeTransformationImpl(from, to, t, info);
}
if(info.covariance.empty())
{
info.covariance = cv::Mat::eye(6,6,CV_64FC1);
}
if(varianceFromInliersCount_)
{
if(info.icpInliersRatio)
{
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*(info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0);
}
else
{
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*(info.inliers > 0?1.0/double(info.inliers):1.0);
}
}
normalizeCovariance(info.covariance, t);
if(info.covariance.at<double>(0,0)<=COVARIANCE_EPSILON)
info.covariance.at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(1,1)<=COVARIANCE_EPSILON)
info.covariance.at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(2,2)<=COVARIANCE_EPSILON)
info.covariance.at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(3,3)<=COVARIANCE_EPSILON)
info.covariance.at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(4,4)<=COVARIANCE_EPSILON)
info.covariance.at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(5,5)<=COVARIANCE_EPSILON)
info.covariance.at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform
if(child_)
{
@@ -239,36 +243,4 @@ Transform Registration::computeTransformationMod(
return t;
}
void Registration::normalizeCovariance(cv::Mat & covariance, const Transform & transform) const
{
UASSERT(covariance.cols == 6 && covariance.rows == 6);
if(covarianceNormalized_)
{
// normalize variance
float norm = transform.getNorm();
covariance.at<double>(0,0) *= norm;
covariance.at<double>(1,1) *= norm;
covariance.at<double>(2,2) *= norm;
float angle = transform.getAngle()/10.0;
covariance.at<double>(3,3) *= angle;
covariance.at<double>(4,4) *= angle;
covariance.at<double>(5,5) *= angle;
}
double epsilon = 0.000001;
if(covariance.at<double>(0,0)<=epsilon)
covariance.at<double>(0,0) = epsilon; // epsilon if exact transform
if(covariance.at<double>(1,1)<=epsilon)
covariance.at<double>(1,1) = epsilon; // epsilon if exact transform
if(covariance.at<double>(2,2)<=epsilon)
covariance.at<double>(2,2) = epsilon; // epsilon if exact transform
if(covariance.at<double>(3,3)<=epsilon)
covariance.at<double>(3,3) = epsilon; // epsilon if exact transform
if(covariance.at<double>(4,4)<=epsilon)
covariance.at<double>(4,4) = epsilon; // epsilon if exact transform
if(covariance.at<double>(5,5)<=epsilon)
covariance.at<double>(5,5) = epsilon; // epsilon if exact transform
}
}

428
corelib/src/RegistrationVis.cpp
View File

@@ -79,6 +79,8 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixEps(), _featureParameters.at(Parameters::kVisSubPixWinSize())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixIterations(), _featureParameters.at(Parameters::kVisSubPixIterations())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), _featureParameters.at(Parameters::kVisSubPixEps())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpGridRows(), _featureParameters.at(Parameters::kVisGridRows())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), _featureParameters.at(Parameters::kVisGridCols())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
this->parseParameters(parameters);
@@ -162,6 +164,14 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
}
if(uContains(parameters, Parameters::kVisGridRows()))
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpGridRows(), parameters.at(Parameters::kVisGridRows())));
}
if(uContains(parameters, Parameters::kVisGridCols()))
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), parameters.at(Parameters::kVisGridCols())));
}
}
RegistrationVis::~RegistrationVis()
@@ -425,7 +435,7 @@ Transform RegistrationVis::computeTransformationImpl(
kptsFrom3D = kptsFrom3DKept;
std::vector<cv::Point3f> kptsTo3D;
if(_estimationType == 0 || (_estimationType == 1 && !varianceFromInliersCount()) || !_forwardEstimateOnly)
if(_estimationType == 0 || _estimationType == 1 || !_forwardEstimateOnly)
{
kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
}
@@ -721,7 +731,9 @@ Transform RegistrationVis::computeTransformationImpl(
{
imageSize = toSignature.sensorData().cameraModels().size() == 1?toSignature.sensorData().cameraModels()[0].imageSize():toSignature.sensorData().stereoCameraModel().left().imageSize();
}
isCalibrated = imageSize.height != 0 && imageSize.width != 0 && toSignature.sensorData().cameraModels().size()==1?toSignature.sensorData().cameraModels()[0].isValidForProjection():toSignature.sensorData().stereoCameraModel().isValidForProjection();
isCalibrated = imageSize.height != 0 && imageSize.width != 0 &&
(toSignature.sensorData().cameraModels().size()==1?toSignature.sensorData().cameraModels()[0].isValidForProjection():toSignature.sensorData().stereoCameraModel().isValidForProjection());
// If guess is set, limit the search of matches using optical flow window size
bool guessSet = !guess.isIdentity() && !guess.isNull();
@@ -756,12 +768,11 @@ Transform RegistrationVis::computeTransformationImpl(
std::vector<cv::Point2f> cornersProjected(projected.size());
std::vector<int> projectedIndexToDescIndex(projected.size());
int oi=0;
Transform guessInv = guess.inverse();
for(unsigned int i=0; i<projected.size(); ++i)
{
if(uIsInBounds(projected[i].x, 0.0f, float(imageSize.width-1)) &&
uIsInBounds(projected[i].y, 0.0f, float(imageSize.height-1)) &&
util3d::transformPoint(kptsFrom3D[i], guessInv).x > 0.0)
util3d::transformPoint(kptsFrom3D[i], guessCameraRef).z > 0.0)
{
projectedIndexToDescIndex[oi] = i;
cornersProjected[oi++] = projected[i];
@@ -780,159 +791,273 @@ Transform RegistrationVis::computeTransformationImpl(
if(cornersProjected.size())
{
// Create kd-tree for projected keypoints
rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
rtflann::Index<rtflann::L2_Simple<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
index.buildIndex();
std::vector< std::vector<size_t> > indices;
std::vector<std::vector<float> > dists;
float radius = (float)_guessWinSize; // pixels
std::vector<cv::Point2f> pointsTo;
cv::KeyPoint::convert(kptsTo, pointsTo);
rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
index.radiusSearch(pointsToMat, indices, dists, radius*radius, rtflann::SearchParams());
UASSERT(indices.size() == pointsToMat.rows);
UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
UASSERT(pointsToMat.rows == kptsTo.size());
UDEBUG("radius search done for guess");
// Process results (Nearest Neighbor Distance Ratio)
int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
std::map<int,int> addedWordsFrom; //<id, index>
std::map<int, int> duplicates; //<fromId, toId>
int newWords = 0;
for(unsigned int i = 0; i < pointsToMat.rows; ++i)
bool matchToProjected = false;
if(matchToProjected)
{
if(kptsTo3D.empty() || util3d::isFinite(kptsTo3D[i]))
// match frame to projected
// Create kd-tree for projected keypoints
rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
rtflann::Index<rtflann::L2_Simple<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
index.buildIndex();
std::vector< std::vector<size_t> > indices;
std::vector<std::vector<float> > dists;
float radius = (float)_guessWinSize; // pixels
std::vector<cv::Point2f> pointsTo;
cv::KeyPoint::convert(kptsTo, pointsTo);
rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
index.radiusSearch(pointsToMat, indices, dists, radius*radius, rtflann::SearchParams());
UASSERT(indices.size() == pointsToMat.rows);
UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
UASSERT(pointsToMat.rows == kptsTo.size());
UDEBUG("radius search done for guess");
// Process results (Nearest Neighbor Distance Ratio)
int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
std::map<int,int> addedWordsFrom; //<id, index>
std::map<int, int> duplicates; //<fromId, toId>
int newWords = 0;
for(unsigned int i = 0; i < pointsToMat.rows; ++i)
{
int octave = kptsTo[i].octave;
int matchedIndex = -1;
if(indices[i].size() >= 2)
if(kptsTo3D.empty() || util3d::isFinite(kptsTo3D[i]))
{
cv::Mat descriptors;
std::vector<int> descriptorsIndices(indices[i].size());
int oi=0;
for(unsigned int j=0; j<indices[i].size(); ++j)
int octave = kptsTo[i].octave;
int matchedIndex = -1;
if(indices[i].size() >= 2)
{
if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave==octave)
cv::Mat descriptors;
std::vector<int> descriptorsIndices(indices[i].size());
int oi=0;
for(unsigned int j=0; j<indices[i].size(); ++j)
{
descriptors.push_back(descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]));
descriptorsIndices[oi++] = indices[i].at(j);
if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave==octave)
{
descriptors.push_back(descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]));
descriptorsIndices[oi++] = indices[i].at(j);
}
}
descriptorsIndices.resize(oi);
if(oi >=2)
{
std::vector<std::vector<cv::DMatch> > matches;
cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
matcher.knnMatch(descriptorsTo.row(i), descriptors, matches, 2);
UASSERT(matches.size() == 1);
UASSERT(matches[0].size() == 2);
if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
{
matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
}
}
else if(oi == 1)
{
matchedIndex = descriptorsIndices[0];
}
}
descriptorsIndices.resize(oi);
if(oi >=2)
else if(indices[i].size() == 1 &&
kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave == octave)
{
std::vector<std::vector<cv::DMatch> > matches;
cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
matcher.knnMatch(descriptorsTo.row(i), descriptors, matches, 2);
UASSERT(matches.size() == 1);
UASSERT(matches[0].size() == 2);
if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
matchedIndex = indices[i].at(0);
}
if(matchedIndex >= 0)
{
matchedIndex = projectedIndexToDescIndex[matchedIndex];
int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
{
matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
id = addedWordsFrom.at(matchedIndex);
duplicates.insert(std::make_pair(matchedIndex, id));
}
}
else if(oi == 1)
{
matchedIndex = descriptorsIndices[0];
}
}
else if(indices[i].size() == 1 &&
kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave == octave)
{
matchedIndex = indices[i].at(0);
}
else
{
addedWordsFrom.insert(std::make_pair(matchedIndex, id));
if(matchedIndex >= 0)
{
matchedIndex = projectedIndexToDescIndex[matchedIndex];
int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
if(kptsFrom.size())
{
wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndex]));
}
words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndex]));
wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndex)));
}
if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
{
id = addedWordsFrom.at(matchedIndex);
duplicates.insert(std::make_pair(matchedIndex, id));
wordsTo.insert(std::make_pair(id, kptsTo[i]));
wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(i)));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(id, kptsTo3D[i]));
}
}
else
{
addedWordsFrom.insert(std::make_pair(matchedIndex, id));
if(kptsFrom.size())
// gen fake ids
wordsTo.insert(std::make_pair(newToId, kptsTo[i]));
wordsDescTo.insert(std::make_pair(newToId, descriptorsTo.row(i)));
if(kptsTo3D.size())
{
wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndex]));
words3To.insert(std::make_pair(newToId, kptsTo3D[i]));
}
words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndex]));
wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndex)));
}
wordsTo.insert(std::make_pair(id, kptsTo[i]));
wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(i)));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(id, kptsTo3D[i]));
++newToId;
++newWords;
}
}
else
}
UDEBUG("addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d",
(int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords,
(int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size());
// create fake ids for not matched words from "from"
int addWordsFromNotMatched = 0;
for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
{
if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
{
int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(i)));
words3From.insert(std::make_pair(id, kptsFrom3D[i]));
++addWordsFromNotMatched;
}
}
UDEBUG("addWordsFromNotMatched=%d -> words3From=%d", addWordsFromNotMatched, (int)words3From.size());
}
else
{
// match projected to frame
std::vector<cv::Point2f> pointsTo;
cv::KeyPoint::convert(kptsTo, pointsTo);
rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
rtflann::Index<rtflann::L2_Simple<float> > index(pointsToMat, rtflann::KDTreeIndexParams());
index.buildIndex();
std::vector< std::vector<size_t> > indices;
std::vector<std::vector<float> > dists;
float radius = (float)_guessWinSize; // pixels
rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams());
UASSERT(indices.size() == cornersProjectedMat.rows);
UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
UASSERT(pointsToMat.rows == kptsTo.size());
UDEBUG("radius search done for guess");
// Process results (Nearest Neighbor Distance Ratio)
std::set<int> addedWordsTo;
std::set<int> addedWordsFrom;
std::set<int> indicesToIgnore;
for(unsigned int i = 0; i < cornersProjectedMat.rows; ++i)
{
int matchedIndexFrom = projectedIndexToDescIndex[i];
if(util3d::isFinite(kptsFrom3D[matchedIndexFrom]))
{
int matchedIndexTo = -1;
if(indices[i].size() >= 2)
{
cv::Mat descriptors;
std::vector<int> descriptorsIndices(indices[i].size());
int oi=0;
for(unsigned int j=0; j<indices[i].size(); ++j)
{
int octave = kptsTo[indices[i].at(j)].octave;
if(kptsFrom.at(matchedIndexFrom).octave==octave)
{
descriptors.push_back(descriptorsTo.row(indices[i].at(j)));
descriptorsIndices[oi++] = indices[i].at(j);
if(dists[i].at(j) < radius)
{
indicesToIgnore.insert(indices[i].at(j));
}
}
}
descriptorsIndices.resize(oi);
if(oi >=2)
{
std::vector<std::vector<cv::DMatch> > matches;
cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
matcher.knnMatch(descriptorsFrom.row(matchedIndexFrom), descriptors, matches, 2);
UASSERT(matches.size() == 1);
UASSERT(matches[0].size() == 2);
if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
{
matchedIndexTo = descriptorsIndices.at(matches[0].at(0).trainIdx);
}
}
else if(oi == 1)
{
matchedIndexTo = descriptorsIndices[0];
}
}
else if(indices[i].size() == 1)
{
int octave = kptsTo[indices[i].at(0)].octave;
if(kptsFrom.at(matchedIndexFrom).octave == octave)
{
matchedIndexTo = indices[i].at(0);
}
}
int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
addedWordsFrom.insert(matchedIndexFrom);
if(kptsFrom.size())
{
wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndexFrom]));
}
words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndexFrom]));
wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndexFrom)));
if((kptsTo3D.empty() || util3d::isFinite(kptsTo3D[matchedIndexTo])) &&
matchedIndexTo >= 0 &&
addedWordsTo.find(matchedIndexTo) == addedWordsTo.end())
{
addedWordsTo.insert(matchedIndexTo);
wordsTo.insert(std::make_pair(id, kptsTo[matchedIndexTo]));
wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(matchedIndexTo)));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(id, kptsTo3D[matchedIndexTo]));
}
}
}
}
// create fake ids for not matched words from "from"
for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
{
if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
{
int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(i)));
words3From.insert(std::make_pair(id, kptsFrom3D[i]));
}
}
int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
for(unsigned int i = 0; i < kptsTo.size(); ++i)
{
if(addedWordsTo.find(i) == addedWordsTo.end() && indicesToIgnore.find(i) == indicesToIgnore.end())
{
// gen fake ids
wordsTo.insert(std::make_pair(newToId, kptsTo[i]));
wordsDescTo.insert(std::make_pair(newToId, descriptorsTo.row(i)));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(newToId, kptsTo3D[i]));
}
++newToId;
++newWords;
}
}
}
UDEBUG("addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d",
(int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords,
(int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size());
// create fake ids for not matched words from "from"
int addWordsFromNotMatched = 0;
for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
{
if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
{
int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(i)));
words3From.insert(std::make_pair(id, kptsFrom3D[i]));
++addWordsFromNotMatched;
}
}
UDEBUG("addWordsFromNotMatched=%d -> words3From=%d", addWordsFromNotMatched, (int)words3From.size());
/*std::vector<cv::KeyPoint> matches(wordsTo.size());
int oi=0;
for(std::multimap<int, cv::KeyPoint>::iterator iter = wordsTo.begin(); iter!=wordsTo.end(); ++iter)
{
if(iter->first < (orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows) && wordsTo.count(iter->first) <= 1)
{
matches[oi++] = iter->second;
}
}
matches.resize(oi);
UDEBUG("guess=%s", guess.prettyPrint().c_str());
std::vector<cv::KeyPoint> projectedKpts;
cv::KeyPoint::convert(cornersProjected, projectedKpts);
cv::Mat image = toSignature.sensorData().imageRaw().clone();
drawKeypoints(image, kptsTo, image, cv::Scalar(0,0,255));
drawKeypoints(image, projectedKpts, image, cv::Scalar(0,255,255)); // BGR
drawKeypoints(image, matches, image, cv::Scalar(0,255,0));
cv::imwrite("projected.bmp", image);
UWARN("saved projected.bmp");*/
}
else
{
@@ -1180,7 +1305,7 @@ Transform RegistrationVis::computeTransformationImpl(
_PnPRefineIterations,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
uMultimapToMapUnique(signatureB->getWords3()),
varianceFromInliersCount()?0:&covariances[dir],
&covariances[dir],
&matchesV,
&inliersV);
inliers[dir] = inliersV;
@@ -1236,20 +1361,6 @@ Transform RegistrationVis::computeTransformationImpl(
UINFO(msg.c_str());
}
}
double epsilon = 0.000001;
if(covariances[dir].at<double>(0,0)<=epsilon)
covariances[dir].at<double>(0,0) = epsilon; // epsilon if exact transform
if(covariances[dir].at<double>(1,1)<=epsilon)
covariances[dir].at<double>(1,1) = epsilon; // epsilon if exact transform
if(covariances[dir].at<double>(2,2)<=epsilon)
covariances[dir].at<double>(2,2) = epsilon; // epsilon if exact transform
if(covariances[dir].at<double>(3,3)<=epsilon)
covariances[dir].at<double>(3,3) = epsilon; // epsilon if exact transform
if(covariances[dir].at<double>(4,4)<=epsilon)
covariances[dir].at<double>(4,4) = epsilon; // epsilon if exact transform
if(covariances[dir].at<double>(5,5)<=epsilon)
covariances[dir].at<double>(5,5) = epsilon; // epsilon if exact transform
}
if(!_forwardEstimateOnly)
@@ -1307,14 +1418,29 @@ Transform RegistrationVis::computeTransformationImpl(
poses.insert(std::make_pair(1, Transform::getIdentity()));
poses.insert(std::make_pair(2, transforms[0]));
for(int i=0;i<2;++i)
{
UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1);
if(covariances[i].at<double>(0,0)<=COVARIANCE_EPSILON)
covariances[i].at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(1,1)<=COVARIANCE_EPSILON)
covariances[i].at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(2,2)<=COVARIANCE_EPSILON)
covariances[i].at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(3,3)<=COVARIANCE_EPSILON)
covariances[i].at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(4,4)<=COVARIANCE_EPSILON)
covariances[i].at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(5,5)<=COVARIANCE_EPSILON)
covariances[i].at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform
}
cv::Mat cov = covariances[0].clone();
normalizeCovariance(cov, transforms[0]);
links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
if(!transforms[1].isNull() && inliers[1].size())
{
cov = covariances[1].clone();
normalizeCovariance(cov, transforms[1]);
links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
}
@@ -1325,6 +1451,7 @@ Transform RegistrationVis::computeTransformationImpl(
std::map<int, CameraModel> models;
Transform invLocalTransformFrom;
CameraModel cameraModelFrom;
if(fromSignature.sensorData().stereoCameraModel().isValidForProjection())
{
@@ -1336,12 +1463,15 @@ Transform RegistrationVis::computeTransformationImpl(
cameraModelFrom.cy(),
cameraModelFrom.localTransform(),
-fromSignature.sensorData().stereoCameraModel().baseline()*cameraModelFrom.fy());
invLocalTransformFrom = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
}
else if(fromSignature.sensorData().cameraModels().size() == 1)
{
cameraModelFrom = fromSignature.sensorData().cameraModels()[0];
invLocalTransformFrom = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
}
Transform invLocalTransformTo = Transform::getIdentity();
CameraModel cameraModelTo;
if(toSignature.sensorData().stereoCameraModel().isValidForProjection())
{
@@ -1353,10 +1483,16 @@ Transform RegistrationVis::computeTransformationImpl(
cameraModelTo.cy(),
cameraModelTo.localTransform(),
-toSignature.sensorData().stereoCameraModel().baseline()*cameraModelTo.fy());
invLocalTransformTo = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
}
else if(toSignature.sensorData().cameraModels().size() == 1)
{
cameraModelTo = toSignature.sensorData().cameraModels()[0];
invLocalTransformTo = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
}
if(invLocalTransformFrom.isNull())
{
invLocalTransformFrom = invLocalTransformTo;
}
models.insert(std::make_pair(1, cameraModelFrom.isValidForProjection()?cameraModelFrom:cameraModelTo));
@@ -1372,14 +1508,16 @@ Transform RegistrationVis::computeTransformationImpl(
std::map<int, cv::Point3f> ptMap;
if(fromSignature.getWords().size() && cameraModelFrom.isValidForProjection())
{
float depthFrom = util3d::transformPoint(pt3D, invLocalTransformFrom).z;
const cv::Point2f & kpt = fromSignature.getWords().find(wordId)->second.pt;
ptMap.insert(std::make_pair(1,cv::Point3f(kpt.x, kpt.y, pt3D.x)));
ptMap.insert(std::make_pair(1,cv::Point3f(kpt.x, kpt.y, depthFrom)));
}
if(toSignature.getWords().size() && cameraModelTo.isValidForProjection())
{
float depthTo = util3d::transformPoint(toSignature.getWords3().find(wordId)->second, invLocalTransformTo).z;
const cv::Point2f & kpt = toSignature.getWords().find(wordId)->second.pt;
UASSERT(toSignature.getWords3().find(wordId) != toSignature.getWords3().end());
ptMap.insert(std::make_pair(2,cv::Point3f(kpt.x, kpt.y, toSignature.getWords3().find(wordId)->second.x)));
ptMap.insert(std::make_pair(2,cv::Point3f(kpt.x, kpt.y, depthTo)));
}
wordReferences.insert(std::make_pair(wordId, ptMap));

16
corelib/src/Rtabmap.cpp
View File

@@ -2170,6 +2170,7 @@ bool Rtabmap::process(
// Optimize map graph
//============================================================
float maxLinearError = 0.0f;
float maxLinearErrorRatio = 0.0f;
double optimizationError = 0.0;
int optimizationIterations = 0;
if(_rgbdSlamMode &&
@@ -2281,21 +2282,25 @@ bool Rtabmap::process(
}
if(maxLinearLink)
{
UINFO("Max optimization error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
UINFO("Max optimization error = %f m (link %d->%d, var=%f, %f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
}
if(maxLinearError > _optimizationMaxLinearError)
float stddev = sqrt(maxLinearLink->transVariance());
maxLinearErrorRatio = maxLinearError/stddev;
if(maxLinearErrorRatio > _optimizationMaxLinearError)
{
UWARN("Rejecting all added loop closures (%d) in this "
"iteration because a wrong loop closure has been "
"detected after graph optimization, resulting in "
"a maximum graph error of %f m (edge %d->%d, type=%d). The "
"maximum error parameter is %f m.",
"a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f, stddev=%f). The "
"maximum error ratio parameter is %f of std deviation.",
(int)loopClosureLinksAdded.size(),
maxLinearError,
maxLinearErrorRatio,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearLink->type(),
maxLinearError,
stddev,
_optimizationMaxLinearError);
for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
{
@@ -2384,6 +2389,7 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
statistics_.addStatistic(Statistics::kLoopLast_id(), _memory->getLastGlobalLoopClosureId());
statistics_.addStatistic(Statistics::kLoopOptimization_max_error(), maxLinearError);
statistics_.addStatistic(Statistics::kLoopOptimization_max_error_ratio(), maxLinearErrorRatio);
statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError);
statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations);

2
corelib/src/RtabmapThread.cpp
View File

@@ -560,7 +560,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
bool ignoreFrame = false;
if(_rate>0.0f)
{
if((_previousStamp>0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate) ||
if((_previousStamp>=0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate) ||
((_previousStamp<=0.0 || odomEvent.data().stamp()<=_previousStamp) && _frameRateTimer->getElapsedTime() < 1.0f/_rate))
{
ignoreFrame = true;

42
corelib/src/SensorData.cpp
View File

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/SensorData.h"
#include "rtabmap/core/Compression.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/utilite/ULogger.h"
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
@@ -793,5 +794,46 @@ long SensorData::getMemoryUsed() const // Return memory usage in Bytes
_descriptors.total()*_descriptors.elemSize();
}
bool SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
{
if(_cameraModels.size() >= 1)
{
for(unsigned int i=0; i<_cameraModels.size(); ++i)
{
if(_cameraModels[i].isValidForProjection() && !_cameraModels[i].localTransform().isNull())
{
cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _cameraModels[i].localTransform().inverse());
if(ptInCameraFrame.z > 0.0f)
{
int borderWidth = int(float(_cameraModels[i].imageWidth())* 0.2);
int u, v;
_cameraModels[i].reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
if(uIsInBounds(u, borderWidth, _cameraModels[i].imageWidth()-2*borderWidth) &&
uIsInBounds(v, borderWidth, _cameraModels[i].imageHeight()-2*borderWidth))
{
return true;
}
}
}
}
}
else if(_stereoCameraModel.isValidForProjection())
{
cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _stereoCameraModel.localTransform().inverse());
if(ptInCameraFrame.z > 0.0f)
{
int u, v;
_stereoCameraModel.left().reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
return uIsInBounds(u, 0, _stereoCameraModel.left().imageWidth()) &&
uIsInBounds(v, 0, _stereoCameraModel.left().imageHeight());
}
}
else
{
UERROR("no valid camera model!");
}
return false;
}
} // namespace rtabmap

17
corelib/src/util2d.cpp
View File

@@ -942,7 +942,7 @@ float getDepth(
const cv::Mat & depthImage,
float x, float y,
bool smoothing,
float maxZError,
float depthErrorRatio,
bool estWithNeighborsIfNull)
{
UASSERT(!depthImage.empty());
@@ -1024,10 +1024,15 @@ float getDepth(
tmp = d;
++count;
}
else if(fabs(d - tmp/float(count)) < maxZError)
else
{
tmp += d;
++count;
float depthError = depthErrorRatio * tmp;
if(fabs(d - tmp/float(count)) < depthError)
{
tmp += d;
++count;
}
}
}
}
@@ -1065,8 +1070,10 @@ float getDepth(
d = depthImage.at<float>(vv,uu);
}
float depthError = depthErrorRatio * depth;
// ignore if not valid or depth difference is too high
if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < maxZError)
if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < depthError)
{
if(uu == u || vv == v)
{

8
corelib/src/util3d.cpp
View File

@@ -215,13 +215,13 @@ pcl::PointXYZ projectDepthTo3D(
float cx, float cy,
float fx, float fy,
bool smoothing,
float maxZError)
float depthErrorRatio)
{
UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
pcl::PointXYZ pt;
float depth = util2d::getDepth(depthImage, x, y, smoothing, maxZError);
float depth = util2d::getDepth(depthImage, x, y, smoothing, depthErrorRatio);
if(depth > 0.0f)
{
// Use correct principal point from calibration
@@ -2272,7 +2272,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, i
UASSERT(bytes % sizeof(float) == 0);
int32_t num = bytes/sizeof(float);
UASSERT(num % dim == 0);
float *data = (float*)malloc(num*sizeof(float));
float *data = new float[num];
// pointers
float *px = data+0;
@@ -2292,6 +2292,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, i
px+=4; py+=4; pz+=4; pr+=4;
}
fclose(stream);
delete[] data;
}
return cloud;

68
corelib/src/util3d_filtering.cpp
View File

@@ -142,17 +142,18 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
float voxelSize)
{
UASSERT(voxelSize > 0.0f);
UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
pcl::VoxelGrid<pcl::PointXYZ> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
if(indices->size())
if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()))
{
filter.setIndices(indices);
pcl::VoxelGrid<pcl::PointXYZ> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
if(indices->size())
{
filter.setIndices(indices);
}
filter.filter(*output);
}
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(
@@ -161,17 +162,18 @@ pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(
float voxelSize)
{
UASSERT(voxelSize > 0.0f);
UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
pcl::VoxelGrid<pcl::PointNormal> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
if(indices->size())
if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()))
{
filter.setIndices(indices);
pcl::VoxelGrid<pcl::PointNormal> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
if(indices->size())
{
filter.setIndices(indices);
}
filter.filter(*output);
}
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
@@ -180,17 +182,18 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
float voxelSize)
{
UASSERT(voxelSize > 0.0f);
UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::VoxelGrid<pcl::PointXYZRGB> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
if(indices->size())
if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()))
{
filter.setIndices(indices);
pcl::VoxelGrid<pcl::PointXYZRGB> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
if(indices->size())
{
filter.setIndices(indices);
}
filter.filter(*output);
}
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
@@ -199,17 +202,18 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
float voxelSize)
{
UASSERT(voxelSize > 0.0f);
UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::VoxelGrid<pcl::PointXYZRGBNormal> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
if(indices->size())
if((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()))
{
filter.setIndices(indices);
pcl::VoxelGrid<pcl::PointXYZRGBNormal> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize);
filter.setInputCloud(cloud);
if(indices->size())
{
filter.setIndices(indices);
}
filter.filter(*output);
}
filter.filter(*output);
return output;
}

27
corelib/src/util3d_motion_estimation.cpp
View File

@@ -35,6 +35,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/util3d.h"
#include <pcl/common/common.h>
#if CV_MAJOR_VERSION < 3
#include "opencv/solvepnp.h"
#endif
@@ -94,8 +96,8 @@ Transform estimateMotion3DTo2D(
imagePoints.resize(oi);
matches.resize(oi);
UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d",
(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size());
UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s",
(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(), guess.prettyPrint().c_str());
if((int)matches.size() >= minInliers)
{
@@ -141,6 +143,7 @@ Transform estimateMotion3DTo2D(
if(covariance && words3B.size())
{
std::vector<float> errorSqrdDists(inliers.size());
std::vector<float> errorSqrdAngles(inliers.size());
oi = 0;
for(unsigned int i=0; i<inliers.size(); ++i)
{
@@ -150,19 +153,25 @@ Transform estimateMotion3DTo2D(
const cv::Point3f & objPt = objectPoints[inliers[i]];
cv::Point3f newPt = util3d::transformPoint(iter->second, transform);
errorSqrdDists[oi] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
//ignore very very far features (stereo)
if(errorSqrdDists[oi] < iter->second.x/100.0f)
{
++oi;
}
Eigen::Vector4f v1(objPt.x - transform.x(), objPt.y - transform.y(), objPt.z - transform.z(), 0);
Eigen::Vector4f v2(newPt.x - transform.x(), newPt.y - transform.y(), newPt.z - transform.z(), 0);
errorSqrdAngles[oi++] = pcl::getAngle3D(v1, v2)*10.0f;
}
}
errorSqrdDists.resize(oi);
errorSqrdAngles.resize(oi);
if(errorSqrdDists.size())
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
*covariance *= 2.1981 * median_error_sqr;
//divide by 4 instead of 2 to ignore very very far features (stereo)
double median_error_sqr = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2];
(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr;
std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
median_error_sqr = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2];
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr;
}
else
{