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

0.15.2: Fixed Transform's not orthogonal rotation matrix causing problems with GTSAM (normalize rotation each time transforms are multiplied, and for backward compatibility automatically normalize rotation of all transforms loaded from old databases prior to 0.15.2). Added Vis/CorGuessMatchToProjection, Icp/PMMatcherKnn and Icp/PMMatcherEpsilon parameters. CameraRGB: fixed max scans number when filtering. Link: transfering maximum covariance when merging. OptimizerGTSAM: updated how covariance are copied based on GTSAM official example. util3d::loadBINCloud(): fixed dim parameter not used. DatabaseViewer: Added option in graph view to ignore intermediate nodes when optimizing the graph.

Browse Source
This commit is contained in:
matlabbe
2017-12-09 21:44:53 -05:00
parent dafaac412f
commit 775b80eff5
25 changed files with 560 additions and 133 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
corelib/src/CameraRGB.cpp
View File

@@ -504,6 +504,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
Transform odometryPose;
Transform groundTruthPose;
cv::Mat depthFromScan;
int scanMaxPts = _scanMaxPts;
UDEBUG("");
if(_dir->isValid())
{
@@ -725,13 +726,18 @@ SensorData CameraImages::captureImage(CameraInfo * info)
if(_scanDownsampleStep > 1 && cloud->size())
{
cloud = util3d::downsample(cloud, _scanDownsampleStep);
UDEBUG("Downsampling scan (step=%d): %d -> %d", _scanDownsampleStep, previousSize, (int)cloud->size());
int scanMaxPtsTmp = scanMaxPts;
scanMaxPts/=_scanDownsampleStep;
UDEBUG("Downsampling scan (step=%d): %d -> %d (scanMaxPts=%d->%d)", _scanDownsampleStep, previousSize, (int)cloud->size(), scanMaxPtsTmp, scanMaxPts);
}
previousSize = (int)cloud->size();
if(_scanVoxelSize > 0.0f && cloud->size())
{
cloud = util3d::voxelize(cloud, _scanVoxelSize);
UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", _scanVoxelSize, previousSize, (int)cloud->size());
float ratio = float(cloud->size()) / previousSize;
int scanMaxPtsTmp = scanMaxPts;
scanMaxPts = int(float(scanMaxPts) * ratio);
UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", _scanVoxelSize, previousSize, (int)cloud->size(), scanMaxPtsTmp, scanMaxPts);
}
if((_scanNormalsK > 0 || _scanNormalsRadius) && cloud->size())
{
@@ -756,7 +762,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
_model.setImageSize(img.size());
}
SensorData data(scan, LaserScanInfo(scan.empty()?0:_scanMaxPts, 0, _scanLocalTransform), _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
SensorData data(scan, LaserScanInfo(scan.empty()?0:scanMaxPts, 0, _scanLocalTransform), _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
data.setGroundTruth(groundTruthPose);
if(info && !odometryPose.isNull())

89
corelib/src/DBDriverSqlite3.cpp
View File

@@ -1351,6 +1351,10 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
{
memcpy(localTransform.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
}
}
@@ -1375,6 +1379,10 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
@@ -1398,6 +1406,10 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
@@ -1410,6 +1422,10 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
@@ -1423,6 +1439,10 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
@@ -1482,6 +1502,10 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
{
float * dataFloat = (float*)data;
memcpy(scanLocalTransform.data(), dataFloat+2, scanLocalTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
scanLocalTransform.normalizeRotation();
}
laserScanMaxPts = (int)dataFloat[0];
laserScanMaxRange = dataFloat[1];
}
@@ -1666,6 +1690,7 @@ bool DBDriverSqlite3::getCalibrationQuery(
if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
{
memcpy(localTransform.data(), data, dataSize);
localTransform.normalizeRotation();
}
}
@@ -1689,6 +1714,10 @@ bool DBDriverSqlite3::getCalibrationQuery(
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
@@ -1712,6 +1741,10 @@ bool DBDriverSqlite3::getCalibrationQuery(
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
@@ -1724,6 +1757,10 @@ bool DBDriverSqlite3::getCalibrationQuery(
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
@@ -1737,6 +1774,10 @@ bool DBDriverSqlite3::getCalibrationQuery(
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
@@ -1839,6 +1880,10 @@ bool DBDriverSqlite3::getLaserScanInfoQuery(
{
float * dataFloat = (float*)data;
memcpy(localTransform.data(), dataFloat+2, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
maxPts = (int)dataFloat[0];
maxRange = dataFloat[1];
@@ -1926,6 +1971,10 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
{
memcpy(pose.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
pose.normalizeRotation();
}
}
mapId = sqlite3_column_int(ppStmt, index++); // map id
@@ -1947,6 +1996,10 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
{
memcpy(groundTruthPose.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
groundTruthPose.normalizeRotation();
}
}
if(uStrNumCmp(_version, "0.13.0") >= 0)
@@ -2094,6 +2147,10 @@ void DBDriverSqlite3::getAllLinksQuery(std::multimap<int, Link> & links, bool ig
if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
{
memcpy(transform.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
transform.normalizeRotation();
}
}
else if(dataSize)
{
@@ -2438,6 +2495,10 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
if((unsigned int)dataSize == pose.size()*sizeof(float) && data)
{
memcpy(pose.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
pose.normalizeRotation();
}
}
if(uStrNumCmp(_version, "0.8.5") >= 0)
@@ -2456,6 +2517,10 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
{
memcpy(groundTruthPose.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
groundTruthPose.normalizeRotation();
}
}
if(uStrNumCmp(_version, "0.13.0") >= 0)
@@ -2708,6 +2773,10 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
@@ -2732,6 +2801,10 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
localTransform = Transform::getIdentity();
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
@@ -2744,6 +2817,10 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+7, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
@@ -2757,6 +2834,10 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
if(uStrNumCmp(_version, "0.15.2") < 0)
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
@@ -3103,6 +3184,10 @@ void DBDriverSqlite3::loadLinksQuery(
if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
{
memcpy(transform.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
transform.normalizeRotation();
}
}
else if(dataSize)
{
@@ -3289,6 +3374,10 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
if((unsigned int)dataSize == transform.size()*sizeof(float) && data)
{
memcpy(transform.data(), data, dataSize);
if(uStrNumCmp(_version, "0.15.2") < 0)
{
transform.normalizeRotation();
}
}
else if(dataSize)
{

3
corelib/src/Link.cpp
View File

@@ -117,7 +117,8 @@ Link Link::merge(const Link & link, Type outputType) const
link.to(),
outputType,
transform_.isNull()?Transform():transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2)
transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):(infMatrix_.inv() + link.infMatrix().inv()).inv());
transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):(infMatrix_.at<double>(0,0)<link.infMatrix().at<double>(0,0)?infMatrix_:link.infMatrix()));
//transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):(infMatrix_.inv() + link.infMatrix().inv()).inv());
}
Link Link::inverse() const

40
corelib/src/OdometryF2M.cpp
View File

@@ -123,15 +123,17 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
uInsert(bundleParameters, ParametersPair(Parameters::kVisCorType(), uNumber2Str(corType)));
regPipeline_ = Registration::create(bundleParameters);
if(bundleAdjustment_>0 && !regPipeline_->isImageRequired())
if(bundleAdjustment_>0 && regPipeline_->isScanRequired())
{
UWARN("%s=%d cannot be used with registration not done with images (%s=%s), disabling bundle adjustment.",
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()
@@ -209,6 +211,25 @@ Transform OdometryF2M::computeTransform(
{
Signature tmpMap = *map_;
UDEBUG("guess=%s frames=%d image required=%d", guess.prettyPrint().c_str(), this->framesProcessed(), regPipeline_->isImageRequired()?1:0);
float maxCorrespondenceDistance = 0.0f;
float pmOutlierRatio = 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();
pmOutlierRatio = Parameters::defaultIcpPMOutlierRatio();
Parameters::parse(parameters_, Parameters::kIcpMaxCorrespondenceDistance(), maxCorrespondenceDistance);
Parameters::parse(parameters_, Parameters::kIcpPMOutlierRatio(), pmOutlierRatio);
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance*3.0f)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(0.95f)));
regPipeline_->parseParameters(params);
}
Transform transform = regPipeline_->computeTransformationMod(
tmpMap,
*lastFrame_,
@@ -216,6 +237,15 @@ Transform OdometryF2M::computeTransform(
!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::kIcpPMOutlierRatio(), uNumber2Str(pmOutlierRatio)));
regPipeline_->parseParameters(params);
}
if(transform.isNull() && !guess.isNull() && regPipeline_->isImageRequired())
{
tmpMap = *map_;
@@ -240,6 +270,7 @@ Transform OdometryF2M::computeTransform(
}
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
UDEBUG("Registration time = %fs", regInfo.totalTime);
std::map<int, cv::Point3f> points3DMap;
std::map<int, Transform> bundlePoses;
std::multimap<int, Link> bundleLinks;
@@ -340,7 +371,10 @@ Transform OdometryF2M::computeTransform(
//make sure the last reference is here
if(refIter->second.size() > 1)
{
references.insert(*refIter->second.rbegin());
if(references.insert(*refIter->second.rbegin()).second)
{
++totalBundleWordReferencesUsed;
}
}
if(iter2D!=lastFrame_->getWords().end())

36
corelib/src/OptimizerGTSAM.cpp
View File

@@ -119,12 +119,12 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
const Transform & initialPose = poses.at(rootId);
if(isSlam2d())
{
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.01, 0.01, 0.01));
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, 0.01));
graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
}
else
{
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
}
}
@@ -168,7 +168,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
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
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
@@ -180,13 +180,15 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
// 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);
Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
}
}
@@ -230,7 +232,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
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
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
@@ -254,13 +256,14 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
// 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);
Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
@@ -308,7 +311,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
UINFO("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
UTimer timer;
int it = 0;
double lastError = 0.0;
double lastError = optimizer->error();
for(int i=0; i<iterations(); ++i)
{
if(intermediateGraphes && i > 0)
@@ -375,7 +378,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
*iterationsDone = it;
}
UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)", optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)",
optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
{

3
corelib/src/Registration.cpp
View File

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/RegistrationVis.h>
#include <rtabmap/core/RegistrationIcp.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
namespace rtabmap {
@@ -183,6 +184,7 @@ Transform Registration::computeTransformationMod(
Transform guess,
RegistrationInfo * infoOut) const
{
UTimer time;
RegistrationInfo info;
if(infoOut)
{
@@ -239,6 +241,7 @@ Transform Registration::computeTransformationMod(
if(infoOut)
{
*infoOut = info;
infoOut->totalTime = time.ticks();
}
return t;
}

12
corelib/src/RegistrationIcp.cpp
View File

@@ -235,6 +235,8 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
_pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()),
_libpointmatcher(Parameters::defaultIcpPM()),
_libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
_libpointmatcherKnn(Parameters::defaultIcpPMMatcherKnn()),
_libpointmatcherEpsilon(Parameters::defaultIcpPMMatcherEpsilon()),
_libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()),
_libpointmatcherICP(0)
{
@@ -272,6 +274,8 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher);
Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig);
Parameters::parse(parameters, Parameters::kIcpPMOutlierRatio(), _libpointmatcherOutlierRatio);
Parameters::parse(parameters, Parameters::kIcpPMMatcherKnn(), _libpointmatcherKnn);
Parameters::parse(parameters, Parameters::kIcpPMMatcherEpsilon(), _libpointmatcherEpsilon);
#ifndef RTABMAP_POINTMATCHER
if(_libpointmatcher)
@@ -322,6 +326,8 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
PM::Parameters params;
params["maxDist"] = uNumber2Str(_maxCorrespondenceDistance);
params["knn"] = uNumber2Str(_libpointmatcherKnn);
params["epsilon"] = uNumber2Str(_libpointmatcherEpsilon);
icp->matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
params.clear();
@@ -389,7 +395,7 @@ Transform RegistrationIcp::computeTransformationImpl(
UDEBUG("Max translation=%f", _maxTranslation);
UDEBUG("Max rotation=%f", _maxRotation);
UDEBUG("Downsampling step=%d", _downsamplingStep);
UDEBUG("libpointmatcher=%d (outlier ratio=%f)", _libpointmatcher?1:0, _libpointmatcherOutlierRatio);
UDEBUG("libpointmatcher=%d (knn=%d, outlier ratio=%f)", _libpointmatcher?1:0, _libpointmatcherKnn, _libpointmatcherOutlierRatio);
UTimer timer;
std::string msg;
@@ -903,6 +909,8 @@ Transform RegistrationIcp::computeTransformationImpl(
correspondencesRatio = float(correspondences)/float(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols);
}
variance/=10.0;
UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%), from guess: trans=%f rot=%f",
dataTo.id(), dataFrom.id(),
hasConverged?"true":"false",
@@ -913,7 +921,7 @@ Transform RegistrationIcp::computeTransformationImpl(
info.icpTranslation,
info.icpRotation);
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*(variance>0.0001?variance:0.0001); // epsilon if exact transform
info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
info.icpInliersRatio = correspondencesRatio;
if(correspondencesRatio < _correspondenceRatio)

77
corelib/src/RegistrationVis.cpp
View File

@@ -66,6 +66,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
_nndr(Parameters::defaultVisCorNNDR()),
_guessWinSize(Parameters::defaultVisCorGuessWinSize()),
_guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
_bundleAdjustment(Parameters::defaultVisBundleAdjustment())
{
_featureParameters = Parameters::getDefaultParameters();
@@ -107,6 +108,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
Parameters::parse(parameters, Parameters::kVisCorGuessWinSize(), _guessWinSize);
Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
uInsert(_bundleParameters, parameters);
@@ -762,7 +764,7 @@ Transform RegistrationVis::computeTransformationImpl(
cv::Mat K = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].K():toSignature.sensorData().stereoCameraModel().left().K();
std::vector<cv::Point2f> projected;
cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
UDEBUG("Projected points=%d", (int)projected.size());
//remove projected points outside of the image
UASSERT((int)projected.size() == descriptorsFrom.rows);
std::vector<cv::Point2f> cornersProjected(projected.size());
@@ -780,19 +782,14 @@ Transform RegistrationVis::computeTransformationImpl(
}
projectedIndexToDescIndex.resize(oi);
cornersProjected.resize(oi);
UDEBUG("cornersProjected=%d", (int)cornersProjected.size());
UDEBUG("corners in frame=%d", (int)cornersProjected.size());
// For each projected feature guess of "from" in "to", find its matching feature in
// the radius around the projected guess.
// TODO: do cross-check?
if(cornersProjected.size())
{
bool matchToProjected = false;
if(matchToProjected)
if(_guessMatchToProjection)
{
// match frame to projected
// Create kd-tree for projected keypoints
@@ -820,6 +817,7 @@ Transform RegistrationVis::computeTransformationImpl(
std::map<int,int> addedWordsFrom; //<id, index>
std::map<int, int> duplicates; //<fromId, toId>
int newWords = 0;
cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
for(unsigned int i = 0; i < pointsToMat.rows; ++i)
{
if(kptsTo3D.empty() || util3d::isFinite(kptsTo3D[i]))
@@ -828,14 +826,17 @@ Transform RegistrationVis::computeTransformationImpl(
int matchedIndex = -1;
if(indices[i].size() >= 2)
{
cv::Mat descriptors;
std::vector<int> descriptorsIndices(indices[i].size());
int oi=0;
if((int)indices[i].size() > descriptors.rows)
{
descriptors.resize(indices[i].size());
}
for(unsigned int j=0; j<indices[i].size(); ++j)
{
if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave==octave)
{
descriptors.push_back(descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]));
descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]).copyTo(descriptors.row(oi));
descriptorsIndices[oi++] = indices[i].at(j);
}
}
@@ -895,11 +896,11 @@ Transform RegistrationVis::computeTransformationImpl(
else
{
// gen fake ids
wordsTo.insert(std::make_pair(newToId, kptsTo[i]));
wordsDescTo.insert(std::make_pair(newToId, descriptorsTo.row(i)));
wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(newToId, kptsTo3D[i]));
words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
}
++newToId;
@@ -918,9 +919,9 @@ Transform RegistrationVis::computeTransformationImpl(
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]));
wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[i]));
wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(i)));
words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[i]));
++addWordsFromNotMatched;
}
@@ -953,6 +954,10 @@ Transform RegistrationVis::computeTransformationImpl(
std::set<int> addedWordsTo;
std::set<int> addedWordsFrom;
std::set<int> indicesToIgnore;
double bruteForceTotalTime = 0.0;
double bruteForceDescCopy = 0.0;
UTimer bruteForceTimer;
cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
for(unsigned int i = 0; i < cornersProjectedMat.rows; ++i)
{
int matchedIndexFrom = projectedIndexToDescIndex[i];
@@ -961,15 +966,19 @@ Transform RegistrationVis::computeTransformationImpl(
int matchedIndexTo = -1;
if(indices[i].size() >= 2)
{
cv::Mat descriptors;
bruteForceTimer.restart();
std::vector<int> descriptorsIndices(indices[i].size());
int oi=0;
if((int)indices[i].size() > descriptors.rows)
{
descriptors.resize(indices[i].size());
}
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)));
descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(oi));
descriptorsIndices[oi++] = indices[i].at(j);
if(dists[i].at(j) < radius)
@@ -978,14 +987,15 @@ Transform RegistrationVis::computeTransformationImpl(
}
}
}
descriptorsIndices.resize(oi);
bruteForceDescCopy += bruteForceTimer.ticks();
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);
matcher.knnMatch(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
UASSERT(matches.size() == 1);
UASSERT(matches[0].size() == 2);
bruteForceTotalTime+=bruteForceTimer.elapsed();
if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
{
matchedIndexTo = descriptorsIndices.at(matches[0].at(0).trainIdx);
@@ -1006,14 +1016,14 @@ Transform RegistrationVis::computeTransformationImpl(
}
int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
addedWordsFrom.insert(matchedIndexFrom);
addedWordsFrom.insert(addedWordsFrom.end(), matchedIndexFrom);
if(kptsFrom.size())
{
wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndexFrom]));
wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[matchedIndexFrom]));
}
words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndexFrom]));
wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndexFrom)));
words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[matchedIndexFrom]));
wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(matchedIndexFrom)));
if((kptsTo3D.empty() || util3d::isFinite(kptsTo3D[matchedIndexTo])) &&
matchedIndexTo >= 0 &&
@@ -1021,15 +1031,16 @@ Transform RegistrationVis::computeTransformationImpl(
{
addedWordsTo.insert(matchedIndexTo);
wordsTo.insert(std::make_pair(id, kptsTo[matchedIndexTo]));
wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(matchedIndexTo)));
wordsTo.insert(wordsTo.end(), std::make_pair(id, kptsTo[matchedIndexTo]));
wordsDescTo.insert(wordsDescTo.end(), std::make_pair(id, descriptorsTo.row(matchedIndexTo)));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(id, kptsTo3D[matchedIndexTo]));
words3To.insert(words3To.end(), std::make_pair(id, kptsTo3D[matchedIndexTo]));
}
}
}
}
UDEBUG("bruteForceDescCopy=%fs, bruteForceTotalTime=%fs", bruteForceDescCopy, bruteForceTotalTime);
// create fake ids for not matched words from "from"
for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
@@ -1037,9 +1048,9 @@ Transform RegistrationVis::computeTransformationImpl(
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]));
wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[i]));
wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(i)));
words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[i]));
}
}
@@ -1048,11 +1059,11 @@ Transform RegistrationVis::computeTransformationImpl(
{
if(addedWordsTo.find(i) == addedWordsTo.end() && indicesToIgnore.find(i) == indicesToIgnore.end())
{
wordsTo.insert(std::make_pair(newToId, kptsTo[i]));
wordsDescTo.insert(std::make_pair(newToId, descriptorsTo.row(i)));
wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(newToId, kptsTo3D[i]));
words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
}
++newToId;
}

17
corelib/src/Transform.cpp
View File

@@ -71,7 +71,7 @@ Transform::Transform(float x, float y, float z, float roll, float pitch, float y
Transform::Transform(float x, float y, float z, float qx, float qy, float qz, float qw) :
data_(cv::Mat::zeros(3,4,CV_32FC1))
{
Eigen::Matrix3f rotation = Eigen::Quaternionf(qw, qx, qy, qz).toRotationMatrix();
Eigen::Matrix3f rotation = Eigen::Quaternionf(qw, qx, qy, qz).normalized().toRotationMatrix();
data()[0] = rotation(0,0);
data()[1] = rotation(0,1);
data()[2] = rotation(0,2);
@@ -249,6 +249,16 @@ Transform Transform::interpolate(float t, const Transform & other) const
return Transform(x,y,z, qres.x(), qres.y(), qres.z(), qres.w());
}
void Transform::normalizeRotation()
{
if(!this->isNull())
{
Eigen::Affine3f m = toEigen3f();
m.linear() = Eigen::Quaternionf(m.linear()).normalized().toRotationMatrix();
*this = fromEigen3f(m);
}
}
std::string Transform::prettyPrint() const
{
if(this->isNull())
@@ -265,7 +275,10 @@ std::string Transform::prettyPrint() const
Transform Transform::operator*(const Transform & t) const
{
return fromEigen4f(toEigen4f()*t.toEigen4f());
Eigen::Affine3f m = Eigen::Affine3f(toEigen4f()*t.toEigen4f());
// make sure rotation is always normalized!
m.linear() = Eigen::Quaternionf(m.linear()).normalized().toRotationMatrix();
return fromEigen3f(m);
}
Transform & Transform::operator*=(const Transform & t)

4
corelib/src/util3d.cpp
View File

@@ -2283,13 +2283,13 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, i
// load point cloud
FILE *stream;
stream = fopen (fileName.c_str(),"rb");
num = fread(data,sizeof(float),num,stream)/4;
num = fread(data,sizeof(float),num,stream)/dim;
cloud->resize(num);
for (int32_t i=0; i<num; i++) {
(*cloud)[i].x = *px;
(*cloud)[i].y = *py;
(*cloud)[i].z = *pz;
px+=4; py+=4; pz+=4; pr+=4;
px+=dim; py+=dim; pz+=dim; pr+=dim;
}
fclose(stream);