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

Merge branch 'master' of github.com:introlab/rtabmap into gtest

Browse Source
This commit is contained in:
matlabbe
2025-05-18 16:14:48 -07:00
parent 8d3b78e760 a02621ef30
commit 879da2a0a7
17 changed files with 436 additions and 239 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Optimizer.h
+6 -3
View File
@@ -149,14 +149,16 @@ public:
const std::map<int, Signature> & signatures,
std::map<int, cv::Point3f> & points3DMap,
std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
bool rematchFeatures = false);
bool rematchFeatures = false,
const ParametersMap & registrationParameters = ParametersMap());
std::map<int, Transform> optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures,
bool rematchFeatures = false);
bool rematchFeatures = false,
const ParametersMap & registrationParameters = ParametersMap());
Transform optimizeBA(
const Link & link,
@@ -172,7 +174,8 @@ public:
std::map<int, cv::Point3f> & points3DMap,
std::map<int, std::map<int, FeatureBA > > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
bool rematchFeatures = false,
bool useLinkTransformAsGuess = false);
bool useLinkTransformAsGuess = false,
ParametersMap registrationParameters = ParametersMap());
protected:
Optimizer(
corelib/include/rtabmap/core/util3d.h
+2
View File
@@ -1060,6 +1060,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_CORE_EXPORT
const std::map<int, std::vector<CameraModel> > & cameraModels,
float maxDistance = 0.0f,
float maxAngle = 0.0f,
float maxDepthError = 0.0f,
const std::vector<float> & roiRatios = std::vector<float>(),
const cv::Mat & projMask = cv::Mat(),
bool distanceToCamPolicy = false,
@@ -1090,6 +1091,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_CORE_EXPORT
const std::map<int, std::vector<CameraModel> > & cameraModels,
float maxDistance = 0.0f,
float maxAngle = 0.0f,
float maxDepthError = 0.0f,
const std::vector<float> & roiRatios = std::vector<float>(),
const cv::Mat & projMask = cv::Mat(),
bool distanceToCamPolicy = false,
corelib/src/Optimizer.cpp
+16 -12
View File
@@ -447,7 +447,8 @@ std::map<int, Transform> Optimizer::optimizeBA(
const std::map<int, Signature> & signatures,
std::map<int, cv::Point3f> & points3DMap,
std::map<int, std::map<int, FeatureBA> > & wordReferences,
bool rematchFeatures)
bool rematchFeatures,
const ParametersMap & registrationParameters)
{
UDEBUG("");
std::map<int, std::vector<CameraModel> > multiModels;
@@ -497,7 +498,7 @@ std::map<int, Transform> Optimizer::optimizeBA(
}
// compute correspondences
this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures, false, registrationParameters);
return optimizeBA(rootId, poses, links, multiModels, points3DMap, wordReferences);
}
@@ -507,11 +508,12 @@ std::map<int, Transform> Optimizer::optimizeBA(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures,
bool rematchFeatures)
bool rematchFeatures,
const ParametersMap & registrationParameters)
{
std::map<int, cv::Point3f> points3DMap;
std::map<int, std::map<int, FeatureBA> > wordReferences;
return optimizeBA(rootId, poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
return optimizeBA(rootId, poses, links, signatures, points3DMap, wordReferences, rematchFeatures, registrationParameters);
}
Transform Optimizer::optimizeBA(
@@ -557,11 +559,20 @@ void Optimizer::computeBACorrespondences(
std::map<int, cv::Point3f> & points3DMap,
std::map<int, std::map<int, FeatureBA> > & wordReferences,
bool rematchFeatures,
bool useLinkTransformAsGuess)
bool useLinkTransformAsGuess,
ParametersMap registrationParameters)
{
UDEBUG("rematchFeatures=%d", rematchFeatures?1:0);
int wordCount = 0;
int edgeWithWordsAdded = 0;
// Some defaults if not provided
registrationParameters.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
registrationParameters.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
registrationParameters.insert(ParametersPair(Parameters::kVisMinInliers(), "6"));
registrationParameters.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
RegistrationVis reg(registrationParameters);
std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> > frameToWordMap; // <FrameId, <Keypoint, wordId> >
for(std::multimap<int, Link>::const_iterator iter=links.lower_bound(1); iter!=links.end(); ++iter)
{
@@ -601,13 +612,6 @@ void Optimizer::computeBACorrespondences(
sTo.getWords().size() &&
sFrom.getWords3().size())
{
ParametersMap regParam;
regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "6"));
regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
RegistrationVis reg(regParam);
if(!rematchFeatures)
{
sFrom.setWordsDescriptors(cv::Mat());
corelib/src/python/rtabmap_superpoint.py
+1 -1
View File
@@ -2,7 +2,7 @@
#
# Drop this file in the root folder of SuperPoint git: https://github.com/magicleap/SuperPointPretrainedNetwork
# To use with rtabmap:
# --Vis/FeatureType 15 --PyDetector/Path "~/SuperPointPretrainedNetwork/rtabmap_superpoint.py" --PyDetector/Model "~/SuperPointPretrainedNetwork/superpoint_v1.pth"
# --Vis/FeatureType 15 --Kp/DetectorStrategy 15 --PyDetector/Path "~/SuperPointPretrainedNetwork/rtabmap_superpoint.py"
#
import random
corelib/src/util3d.cpp
+92 -59
View File
@@ -3078,6 +3078,18 @@ public:
float distance;
};
class RegisteredPoints {
public:
class Point {
public:
Point(float distance_, int index_) : distance(distance_), index(index_) {}
float distance;
int index;
};
float minDistance;
std::vector<Point> points;
};
/**
* For each point, return pixel of the best camera (NodeID->CameraIndex)
* looking at it based on the policy and parameters
@@ -3089,6 +3101,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
const std::map<int, std::vector<CameraModel> > & cameraModels,
float maxDistance,
float maxAngle,
float maxDepthError,
const std::vector<float> & roiRatios,
const cv::Mat & projMask,
bool distanceToCamPolicy,
@@ -3099,6 +3112,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
UINFO("cameraModels=%d", (int)cameraModels.size());
UINFO("maxDistance=%f", maxDistance);
UINFO("maxAngle=%f", maxAngle);
UINFO("maxDepthError=%f", maxDepthError);
UINFO("distanceToCamPolicy=%s", distanceToCamPolicy?"true":"false");
UINFO("roiRatios=%s", roiRatios.size() == 4?uFormat("%f %f %f %f", roiRatios[0], roiRatios[1], roiRatios[2], roiRatios[3]).c_str():"");
UINFO("projMask=%dx%d", projMask.cols, projMask.rows);
@@ -3164,8 +3178,9 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
float cx = cameraMatrixK.at<double>(0,2);
float cy = cameraMatrixK.at<double>(1,2);
// depth: 2 channels UINT: [depthMM, indexPt]
cv::Mat registered = cv::Mat::zeros(imageSize, CV_32SC2);
// [rows][cols][depth, indexPt]
std::vector<std::vector<RegisteredPoints> > registered(
imageSize.height, std::vector<RegisteredPoints>(imageSize.width));
Transform t = cameraTransform.inverse();
cv::Rect roi(0,0,imageSize.width, imageSize.height);
@@ -3187,34 +3202,44 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
if(z > 0.0f && (maxDistance<=0 || z<maxDistance))
{
float invZ = 1.0f/z;
float dx = (fx*ptScan.x)*invZ + cx;
float dy = (fy*ptScan.y)*invZ + cy;
int dx_low = dx;
int dy_low = dy;
int dx_high = dx + 0.5f;
int dy_high = dy + 0.5f;
int zMM = z * 1000;
if(uIsInBounds(dx_low, roi.x, roi.x+roi.width) && uIsInBounds(dy_low, roi.y, roi.y+roi.height) &&
(validProjMask.empty() || validProjMask.at<unsigned char>(dy_low, imageSize.width*camIndex+dx_low) > 0))
{
set = true;
cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_low, dx_low);
if(zReg[0] == 0 || zMM < zReg[0])
{
zReg[0] = zMM;
zReg[1] = i;
float u = (fx*ptScan.x)*invZ + cx;
float v = (fy*ptScan.y)*invZ + cy;
int x = u + 0.5f;
int y = v + 0.5f;
if(uIsInBounds(x, roi.x, roi.x+roi.width) && uIsInBounds(y, roi.y, roi.y+roi.height) &&
(validProjMask.empty() || validProjMask.at<unsigned char>(y, imageSize.width*camIndex+x) > 0)) {
RegisteredPoints &zReg = registered[y][x];
if(zReg.points.empty()) {
zReg.minDistance = z;
zReg.points.push_back(RegisteredPoints::Point(z, i));
set = true;
}
}
if((dx_low != dx_high || dy_low != dy_high) &&
uIsInBounds(dx_high, roi.x, roi.x+roi.width) && uIsInBounds(dy_high, roi.y, roi.y+roi.height) &&
(validProjMask.empty() || validProjMask.at<unsigned char>(dy_high, imageSize.width*camIndex+dx_high) > 0))
{
set = true;
cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_high, dx_high);
if(zReg[0] == 0 || zMM < zReg[0])
{
zReg[0] = zMM;
zReg[1] = i;
else if(z < zReg.minDistance) {
zReg.minDistance = z;
if(maxDepthError<=0.0f) {
// keeping only closest point, just update it
zReg.points[0].distance = z;
zReg.points[0].index = i;
}
else {
// update the points attached to same pixel based on new closest distance
std::vector<RegisteredPoints::Point> reOrderedPts;
reOrderedPts.push_back(RegisteredPoints::Point(z, i));
for(size_t p=0; p<zReg.points.size(); ++p) {
if(zReg.points[p].distance - z < maxDepthError) {
reOrderedPts.push_back(zReg.points[p]);
}
}
zReg.points = reOrderedPts;
}
set = true;
}
else if(maxDepthError>=0.0f && z - zReg.minDistance < maxDepthError) {
// The point is closer than current closest one to camera,
// but still under max depth difference, just append
zReg.points.push_back(RegisteredPoints::Point(z, i));
set = true;
}
}
}
@@ -3225,19 +3250,19 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
}
if(count == 0)
{
registered = cv::Mat();
registered.clear();
UINFO("No points projected in camera %d/%d", pter->first, camIndex);
}
else
{
UDEBUG("%d points projected in camera %d/%d", count, pter->first, camIndex);
}
for(int u=0; u<registered.cols; ++u)
for(int u=0; u<imageSize.width; ++u)
{
for(int v=0; v<registered.rows; ++v)
for(int v=0; v<imageSize.height; ++v)
{
cv::Vec2i &zReg = registered.at<cv::Vec2i>(v, u);
if(zReg[0] > 0)
RegisteredPoints &zReg = registered[v][u];
if(!zReg.points.empty())
{
ProjectionInfo info;
info.nodeID = pter->first;
@@ -3245,36 +3270,40 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
info.uv.x = float(u)/float(imageSize.width);
info.uv.y = float(v)/float(imageSize.height);
const Transform & cam = cameraPoses.at(info.nodeID);
const PointT & pt = cloud.at(zReg[1]);
Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
float distanceToCam = zReg[0]/1000.0f;
if( (maxAngle<=0 || (camDir.dot(normal) > 0 && angleToCam < maxAngle)) && // is facing camera? is point normal perpendicular to camera?
(maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
for(size_t p=0; p<zReg.points.size(); ++p)
{
float vx = info.uv.x-0.5f;
float vy = info.uv.y-0.5f;
float distanceToCenter = vx*vx+vy*vy;
float distance = distanceToCenter;
if(distanceToCamPolicy)
int ptIdx = zReg.points[p].index;
const PointT & pt = cloud.at(ptIdx);
Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
float distanceToCam = zReg.points[p].distance;
if( (maxAngle<=0 || (camDir.dot(normal) > 0 && angleToCam < maxAngle)) && // is facing camera? is point normal perpendicular to camera?
(maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
{
distance = distanceToCam;
}
float vx = info.uv.x-0.5f;
float vy = info.uv.y-0.5f;
info.distance = distance;
if(invertedIndex[zReg[1]].distance != -1.0f)
{
if(distance <= invertedIndex[zReg[1]].distance)
float distanceToCenter = vx*vx+vy*vy;
float distance = distanceToCenter;
if(distanceToCamPolicy)
{
invertedIndex[zReg[1]] = info;
distance = distanceToCam;
}
info.distance = distance;
if(invertedIndex[ptIdx].distance != -1.0f)
{
if(distance <= invertedIndex[ptIdx].distance)
{
invertedIndex[ptIdx] = info;
}
}
else
{
invertedIndex[ptIdx] = info;
}
}
else
{
invertedIndex[zReg[1]] = info;
}
}
}
@@ -3344,6 +3373,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
const std::map<int, std::vector<CameraModel> > & cameraModels,
float maxDistance,
float maxAngle,
float maxDepthError,
const std::vector<float> & roiRatios,
const cv::Mat & projMask,
bool distanceToCamPolicy,
@@ -3354,6 +3384,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
cameraModels,
maxDistance,
maxAngle,
maxDepthError,
roiRatios,
projMask,
distanceToCamPolicy,
@@ -3366,6 +3397,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
const std::map<int, std::vector<CameraModel> > & cameraModels,
float maxDistance,
float maxAngle,
float maxDepthError,
const std::vector<float> & roiRatios,
const cv::Mat & projMask,
bool distanceToCamPolicy,
@@ -3376,6 +3408,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
cameraModels,
maxDistance,
maxAngle,
maxDepthError,
roiRatios,
projMask,
distanceToCamPolicy,
guilib/include/rtabmap/gui/DatabaseViewer.h
+4 -2
View File
@@ -65,6 +65,8 @@ class ExportCloudsDialog;
class EditDepthArea;
class EditMapArea;
class LinkRefiningDialog;
class Registration;
class RegistrationIcp;
class RTABMAP_GUI_EXPORT DatabaseViewer : public QMainWindow
{
@@ -202,8 +204,8 @@ private:
void updateLoopClosuresSlider(int from = 0, int to = 0);
void updateCovariances(const QList<Link> & links);
void refineLinks(const QList<Link> & links);
void refineConstraint(int from, int to, bool silent);
bool addConstraint(int from, int to, bool silent, bool silentlyUseOptimizedGraphAsGuess = false);
void refineConstraint(int from, int to, Registration * reg, RegistrationIcp * regIcp, bool silent);
bool addConstraint(int from, int to, Registration * reg, bool silent, bool silentlyUseOptimizedGraphAsGuess = false);
void exportPoses(int format);
void exportGPS(int format);
guilib/src/DataRecorder.cpp
+6 -2
View File
@@ -156,8 +156,12 @@ void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & p
void DataRecorder::showImage(const cv::Mat & image, const cv::Mat & depth)
{
processingImages_ = true;
imageView_->setImage(uCvMat2QImage(image));
imageView_->setImageDepth(depth);
if(!image.empty()) {
imageView_->setImage(uCvMat2QImage(image));
}
if(!depth.empty()) {
imageView_->setImageDepth(depth);
}
label_->setText(tr("Images=%1 (~%2 MB)").arg(count_).arg(totalSizeKB_/1000));
processingImages_ = false;
}
guilib/src/DatabaseViewer.cpp
+17 -13
View File
@@ -4263,6 +4263,8 @@ void DatabaseViewer::detectMoreLoopClosures()
return;
}
std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
for(int n=0; n<iterations; ++n)
{
UINFO("iteration %d/%d", n+1, iterations);
@@ -4310,7 +4312,7 @@ void DatabaseViewer::detectMoreLoopClosures()
delta.getNorm() >= ui_->doubleSpinBox_detectMore_radiusMin->value())
{
checkedLoopClosures.insert(std::make_pair(from, to));
if(addConstraint(from, to, true, useOptimizedGraphAsGuess))
if(addConstraint(from, to, reg.get(), true, useOptimizedGraphAsGuess))
{
UINFO("Added new loop closure between %d and %d.", from, to);
++added;
@@ -4568,13 +4570,16 @@ void DatabaseViewer::refineLinks(const QList<Link> & links)
progressDialog->setMinimumWidth(800);
progressDialog->show();
RegistrationIcp regProximity(ui_->parameters_toolbox->getParameters());
std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
for(int i=0; i<links.size(); ++i)
{
int from = links[i].from();
int to = links[i].to();
if(from > 0 && to > 0)
{
this->refineConstraint(links[i].from(), links[i].to(), true);
this->refineConstraint(links[i].from(), links[i].to(), reg.get(), &regProximity, true);
progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
}
else
@@ -8085,10 +8090,12 @@ void DatabaseViewer::refineConstraint()
{
int from = ids_.at(ui_->horizontalSlider_A->value());
int to = ids_.at(ui_->horizontalSlider_B->value());
refineConstraint(from, to, false);
RegistrationIcp regProximity(ui_->parameters_toolbox->getParameters());
std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
refineConstraint(from, to, reg.get(), &regProximity, false);
}
void DatabaseViewer::refineConstraint(int from, int to, bool silent)
void DatabaseViewer::refineConstraint(int from, int to, Registration * reg, RegistrationIcp * regProximity, bool silent)
{
UDEBUG("%d -> %d", from, to);
bool switchedIds = false;
@@ -8361,8 +8368,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
toS = new Signature(assembledData);
RegistrationIcp registrationIcp(parameters);
transform = registrationIcp.computeTransformationMod(*fromS, *toS, currentLink.transform(), &info);
transform = regProximity->computeTransformationMod(*fromS, *toS, currentLink.transform(), &info);
if(!transform.isNull())
{
// local scan matching proximity detection should have higher variance (see Rtabmap::process())
@@ -8380,7 +8386,6 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
}
bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
Registration * reg = Registration::create(parameters);
if( reg->isScanRequired() ||
reg->isUserDataRequired() ||
reextractVisualFeatures ||
@@ -8477,8 +8482,6 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
transform = reg->computeTransformationMod(*toS, *fromS, t.isNull()?t:t.inverse(), &info);
switchedIds = true;
}
delete reg;
}
UINFO("(%d ->%d) Registration time: %f s", currentLink.from(), currentLink.to(), timer.ticks());
@@ -8610,11 +8613,14 @@ void DatabaseViewer::addConstraint()
{
int from = ids_.at(ui_->horizontalSlider_A->value());
int to = ids_.at(ui_->horizontalSlider_B->value());
addConstraint(from, to, false);
std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
addConstraint(from, to, reg.get(), false);
}
bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyUseOptimizedGraphAsGuess)
bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool silent, bool silentlyUseOptimizedGraphAsGuess)
{
UASSERT(reg);
bool switchedIds = false;
if(from == to)
{
@@ -8641,7 +8647,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyU
UASSERT(!containsLink(linksRefined_, from, to));
ParametersMap parameters = ui_->parameters_toolbox->getParameters();
Registration * reg = Registration::create(parameters);
bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
@@ -8823,7 +8828,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyU
{
t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
}
delete reg;
UDEBUG("");
if(!t.isNull())
guilib/src/ExportBundlerDialog.cpp
+27 -7
View File
@@ -194,17 +194,37 @@ void ExportBundlerDialog::exportBundler(
ParametersMap parametersSBA = parameters;
uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(_ui->sba_iterations->value())));
uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(_ui->sba_variance->value())));
Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
std::shared_ptr<Optimizer> sba(Optimizer::create(sbaType, parametersSBA));
sba->getConnectedGraph(poses.begin()->first, poses, links, posesOut, linksOut);
poses = sba->optimizeBA(
posesOut.begin()->first,
posesOut,
linksOut,
// set input poses as initial optimization guess
for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
{
iter->second = poses.at(iter->first);
}
if(_ui->sba_iterations->value() > 0)
{
poses = sba->optimizeBA(
posesOut.begin()->first,
posesOut,
linksOut,
signatures.toStdMap(),
points3DMap,
wordReferences,
_ui->sba_rematchFeatures->isChecked());
}
else
{
// do not optimize, just compute 3D features and word correspondences
poses = posesOut;
sba->computeBACorrespondences(poses,
linksOut,
signatures.toStdMap(),
points3DMap,
wordReferences,
_ui->sba_rematchFeatures->isChecked());
delete sba;
_ui->sba_rematchFeatures->isChecked(),
false,
parametersSBA);
}
if(poses.empty())
{
guilib/src/ExportCloudsDialog.cpp
+5
View File
@@ -209,6 +209,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
connect(_ui->spinBox_camProjDecimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_camProjMaxDistance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_camProjMaxAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->doubleSpinBox_camProjMaxDepthError, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_camProjDistanceToCamPolicy, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_camProjKeepPointsNotSeenByCameras, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->checkBox_camProjRecolorPoints, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
@@ -443,6 +444,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
settings.setValue("cam_proj_decimation", _ui->spinBox_camProjDecimation->value());
settings.setValue("cam_proj_max_distance", _ui->doubleSpinBox_camProjMaxDistance->value());
settings.setValue("cam_proj_max_angle", _ui->doubleSpinBox_camProjMaxAngle->value());
settings.setValue("cam_proj_max_depth_error", _ui->doubleSpinBox_camProjMaxDepthError->value());
settings.setValue("cam_proj_distance_policy", _ui->checkBox_camProjDistanceToCamPolicy->isChecked());
settings.setValue("cam_proj_keep_points", _ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked());
settings.setValue("cam_proj_recolor_points", _ui->checkBox_camProjRecolorPoints->isChecked());
@@ -628,6 +630,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
_ui->spinBox_camProjDecimation->setValue(settings.value("cam_proj_decimation", _ui->spinBox_camProjDecimation->value()).toInt());
_ui->doubleSpinBox_camProjMaxDistance->setValue(settings.value("cam_proj_max_distance", _ui->doubleSpinBox_camProjMaxDistance->value()).toDouble());
_ui->doubleSpinBox_camProjMaxAngle->setValue(settings.value("cam_proj_max_angle", _ui->doubleSpinBox_camProjMaxAngle->value()).toDouble());
_ui->doubleSpinBox_camProjMaxDepthError->setValue(settings.value("cam_proj_max_depth_error", _ui->doubleSpinBox_camProjMaxDepthError->value()).toDouble());
_ui->checkBox_camProjDistanceToCamPolicy->setChecked(settings.value("cam_proj_distance_policy", _ui->checkBox_camProjDistanceToCamPolicy->isChecked()).toBool());
_ui->checkBox_camProjKeepPointsNotSeenByCameras->setChecked(settings.value("cam_proj_keep_points", _ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked()).toBool());
_ui->checkBox_camProjRecolorPoints->setChecked(settings.value("cam_proj_recolor_points", _ui->checkBox_camProjRecolorPoints->isChecked()).toBool());
@@ -810,6 +813,7 @@ void ExportCloudsDialog::restoreDefaults()
_ui->spinBox_camProjDecimation->setValue(1);
_ui->doubleSpinBox_camProjMaxDistance->setValue(0);
_ui->doubleSpinBox_camProjMaxAngle->setValue(0);
_ui->doubleSpinBox_camProjMaxDepthError->setValue(0);
_ui->checkBox_camProjDistanceToCamPolicy->setChecked(true);
_ui->checkBox_camProjKeepPointsNotSeenByCameras->setChecked(false);
_ui->checkBox_camProjRecolorPoints->setChecked(true);
@@ -2911,6 +2915,7 @@ bool ExportCloudsDialog::getExportedClouds(
cameraModelsProj,
_ui->doubleSpinBox_camProjMaxDistance->value(),
_ui->doubleSpinBox_camProjMaxAngle->value()*M_PI/180.0,
_ui->doubleSpinBox_camProjMaxDepthError->value(),
roiRatios,
projMask,
_ui->checkBox_camProjDistanceToCamPolicy->isChecked(),
guilib/src/MainWindow.cpp
+9 -3
View File
@@ -6623,6 +6623,8 @@ void MainWindow::postProcessing(
odomMaxInf = graph::getMaxOdomInf(_currentLinksMap);
}
std::shared_ptr<Registration> registration(Registration::create(parameters));
UASSERT(iterations>0);
for(int n=0; n<iterations && !_progressCanceled; ++n)
{
@@ -6703,7 +6705,6 @@ void MainWindow::postProcessing(
{
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
}
Registration * registration = Registration::create(parameters);
if(reextractFeatures)
{
@@ -6735,7 +6736,6 @@ void MainWindow::postProcessing(
Parameters::kRGBDLoopClosureReextractFeatures().c_str());
}
transform = registration->computeTransformation(signatureFrom, signatureTo, Transform(), &info);
delete registration;
if(!transform.isNull())
{
//optimize the graph to see if the new constraint is globally valid
@@ -7023,7 +7023,13 @@ void MainWindow::postProcessing(
uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations)));
uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(sbaVariance)));
Optimizer * sbaOptimizer = Optimizer::create(sbaType, parametersSBA);
std::map<int, Transform> newPoses = sbaOptimizer->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap(), sbaRematchFeatures);
std::map<int, Transform> newPoses = sbaOptimizer->optimizeBA(
optimizedPoses.begin()->first,
optimizedPoses,
linksOut,
_cachedSignatures.toStdMap(),
sbaRematchFeatures,
parametersSBA);
delete sbaOptimizer;
if(newPoses.size())
{
guilib/src/ui/exportBundlerDialog.ui
+1 -1
View File
@@ -141,7 +141,7 @@
<item row="1" column="0">
<widget class="QSpinBox" name="sba_iterations">
<property name="minimum">
<number>1</number>
<number>0</number>
</property>
<property name="maximum">
<number>999999</number>
guilib/src/ui/exportCloudsDialog.ui
+149 -117
View File
@@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>919</width>
<width>895</width>
<height>869</height>
</rect>
</property>
@@ -23,9 +23,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-3199</y>
<width>885</width>
<height>6183</height>
<y>-2495</y>
<width>861</width>
<height>6426</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_13">
@@ -2012,100 +2012,30 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
<bool>false</bool>
</property>
<layout class="QGridLayout" name="gridLayout_20" columnstretch="0,0,0,1">
<item row="7" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_14">
<property name="text">
<string>Distance to camera policy. The closest camera from a point is used to colorize the point. If disabled, the camera for which the point projection is the closest of the image center is used to colorize the point.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="9" column="2">
<widget class="QLabel" name="label_meshingTextureSize_18">
<property name="text">
<string>Recolor points from camera projection. This would be used to color laser scans with the cameras.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QCheckBox" name="checkBox_camProjDistanceToCamPolicy">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_16">
<property name="text">
<string>Decimation of camera resolution before projection. This can help to correctly estimate points hidden by other points, in case the point cloud is sparse.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="9" column="2" colspan="2">
<widget class="QLabel" name="label_camProjExportCamera">
<property name="text">
<string>ID format of the camera selected for each point.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_13">
<property name="text">
<string>Maximum distance from the camera for points to be colorized by this camera (0 means infinite).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QSpinBox" name="spinBox_camProjDecimation">
<property name="suffix">
<string/>
</property>
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>9999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
<item row="1" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_17">
<property name="text">
<string>ROI ratios [left right top bottom] between 0 and 1. This can be used to ignore black borders of RGB images caused by calibration. </string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_12">
<property name="text">
<string>Maximum angle between camera and point's normal to apply color (0 means disabled).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QCheckBox" name="checkBox_camProjRecolorPoints">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLineEdit" name="lineEdit_camProjMaskFilePath"/>
</item>
<item row="7" column="1">
<widget class="QCheckBox" name="checkBox_camProjKeepPointsNotSeenByCameras">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="9" column="1">
<item row="10" column="1">
<widget class="QComboBox" name="comboBox_camProjExportCamera">
<property name="toolTip">
<string>By Node ID: cameras of same node have same ID
@@ -2134,18 +2064,56 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
</item>
</widget>
</item>
<item row="6" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_14">
<item row="9" column="1">
<widget class="QCheckBox" name="checkBox_camProjRecolorPoints">
<property name="text">
<string>Distance to camera policy. The closest camera from a point is used to colorize the point. If disabled, the camera for which the point projection is the closest of the image center is used to colorize the point.</string>
<string/>
</property>
</widget>
</item>
<item row="2" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_13">
<property name="text">
<string>Maximum distance from the camera for points to be colorized by this camera (0 means infinite).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLineEdit" name="lineEdit_camProjRoiRatios"/>
<item row="8" column="1">
<widget class="QCheckBox" name="checkBox_camProjKeepPointsNotSeenByCameras">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_17">
<property name="text">
<string>ROI ratios [left right top bottom] between 0 and 1. This can be used to ignore black borders of RGB images caused by calibration. </string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QToolButton" name="toolButton_camProjMaskFilePath">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="5" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_16">
<property name="text">
<string>Decimation of camera resolution before projection. This can help to correctly estimate points hidden by other points, in case the point cloud is sparse.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxAngle">
@@ -2169,6 +2137,65 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLineEdit" name="lineEdit_camProjRoiRatios"/>
</item>
<item row="5" column="1">
<widget class="QSpinBox" name="spinBox_camProjDecimation">
<property name="suffix">
<string/>
</property>
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>9999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QCheckBox" name="checkBox_camProjDistanceToCamPolicy">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_19">
<property name="text">
<string>File path for a mask. Format should be 8-bits grayscale. The mask should cover all cameras in case multi-camera is used and have the same resolution.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_12">
<property name="text">
<string>Maximum angle between camera and point's normal to apply color (0 means disabled).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="10" column="2" colspan="2">
<widget class="QLabel" name="label_camProjExportCamera">
<property name="text">
<string>ID format of the camera selected for each point.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxDistance">
<property name="suffix">
@@ -2191,7 +2218,7 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
</property>
</widget>
</item>
<item row="7" column="2" colspan="2">
<item row="8" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_15">
<property name="text">
<string>Keep points not seen by the cameras. These points will be set with a pure red color (255,0,0) if the cloud was created from laser scans.</string>
@@ -2201,33 +2228,38 @@ By Node ID and Camera Index: NodeID*10+CameraIndex</string>
</property>
</widget>
</item>
<item row="8" column="2">
<widget class="QLabel" name="label_meshingTextureSize_18">
<property name="text">
<string>Recolor points from camera projection. This would be used to color laser scans with the cameras.</string>
<item row="4" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxDepthError">
<property name="suffix">
<string> m</string>
</property>
<property name="wordWrap">
<bool>true</bool>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>9.990000000000000</double>
</property>
<property name="singleStep">
<double>0.050000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="5" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_19">
<item row="4" column="2" colspan="2">
<widget class="QLabel" name="label_meshingTextureSize_20">
<property name="text">
<string>File path for a mask. Format should be 8-bits grayscale. The mask should cover all cameras in case multi-camera is used and have the same resolution.</string>
<string>For all points registered to same pixel, only those close enough to the closest point will be colored with same color (0 means only the closest point of all points registered in same pixel is colored).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QToolButton" name="toolButton_camProjMaskFilePath">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
tools/DataRecorder/main.cpp
+30
View File
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/CameraRGBD.h>
#include <rtabmap/core/CameraStereo.h>
#include <rtabmap/core/Camera.h>
#include <rtabmap/core/SensorEvent.h>
#include <rtabmap/core/DBReader.h>
#include <rtabmap/core/SensorCaptureThread.h>
#include <rtabmap/core/SensorCaptureThread.h>
@@ -71,6 +72,31 @@ void sighandler(int sig)
}
}
// Detect when we reached end-of-files
class StatusHandler: public UEventsHandler{
protected:
virtual bool handleEvent(UEvent * event)
{
if(event->getClassName().compare("SensorEvent") == 0)
{
SensorEvent * camEvent = (SensorEvent*)event;
if(camEvent->getCode() == SensorEvent::kCodeNoMoreImages)
{
printf("End of stream reached...\n");
if(cam)
{
cam->join(true);
}
if(app)
{
QMetaObject::invokeMethod(app, "quit");
}
}
}
return false;
}
};
int main (int argc, char * argv[])
{
ULogger::setType(ULogger::kTypeConsole);
@@ -138,6 +164,10 @@ int main (int argc, char * argv[])
signal(SIGTERM, &sighandler);
signal(SIGINT, &sighandler);
// Catch end of stream to close the gui
StatusHandler statusHandler;
statusHandler.registerToEventsManager();
rtabmap::Camera * camera = dialog.createCamera();
if(camera == 0)
{
tools/Export/main.cpp
+56 -11
View File
@@ -106,9 +106,10 @@ void showUsage()
" 2=Use optimized poses already computed in the database instead\n"
" of re-computing them (fallback to default if optimized poses don't exist).\n"
" 3=No optimization, use odometry poses directly.\n"
" --poses Export optimized poses of the robot frame (e.g., base_link).\n"
" --poses Export optimized poses of the robot frame (e.g., base_link), including landmarks.\n"
" --poses_camera Export optimized poses of the camera frame (e.g., optical frame).\n"
" --poses_scan Export optimized poses of the scan frame.\n"
" --poses_landmark Export optimized poses of landmarks.\n"
" --poses_gt Export ground truth poses of the robot frame (e.g., base_link).\n"
" --poses_gps Export GPS poses of the GPS frame in local coordinates.\n"
" --poses_format # Format used for exported poses (default is 11):\n"
@@ -252,6 +253,7 @@ int main(int argc, char * argv[])
bool exportPoses = false;
bool exportPosesCamera = false;
bool exportPosesScan = false;
bool exportPosesLandmarks = false;
bool exportPosesGt = false;
bool exportPosesGps = false;
int exportPosesFormat = 11;
@@ -498,6 +500,10 @@ int main(int argc, char * argv[])
{
exportPosesScan = true;
}
else if(std::strcmp(argv[i], "--poses_landmark") == 0)
{
exportPosesLandmarks = true;
}
else if(std::strcmp(argv[i], "--poses_gt") == 0)
{
exportPosesGt = true;
@@ -1079,6 +1085,7 @@ int main(int argc, char * argv[])
exportPoses ||
exportPosesScan ||
exportPosesCamera ||
exportPosesLandmarks ||
exportPosesGt ||
exportPosesGps ||
exportGps>=0 ||
@@ -1142,7 +1149,7 @@ int main(int argc, char * argv[])
std::multimap<int, Link> links;
dbDriver->getAllOdomPoses(odomPoses, true);
dbDriver->getAllLinks(links, true, true);
if(optimizationApproach == 3 || !(exportCloud || exportMesh || exportPoses || exportPosesCamera || exportPosesScan))
if(optimizationApproach == 3 || !(exportCloud || exportMesh || exportPoses || exportPosesCamera || exportPosesScan || exportPosesLandmarks))
{
// Just use odometry poses when exporting only images
optimizedPoses = odomPoses;
@@ -1346,13 +1353,18 @@ int main(int argc, char * argv[])
pcl::PointCloud<pcl::PointXYZI>::Ptr assembledCloudI(new pcl::PointCloud<pcl::PointXYZI>);
std::map<int, rtabmap::Transform> robotPoses;
std::vector<std::map<int, rtabmap::Transform> > cameraPoses;
std::vector<std::map<int, double> > cameraStamps;
std::map<int, rtabmap::Transform> scanPoses;
std::map<int, double> scanStamps;
std::map<int, rtabmap::Transform> landmarkPoses;
std::map<int, double> landmarkStamps;
std::map<int, rtabmap::Transform> gtPoses;
std::map<int, double> gtStamps;
std::map<int, rtabmap::Transform> gpsPoses;
std::map<int, double> gpsStamps;
GPS gpsOrigin;
std::map<int, rtabmap::GPS> gpsValues;
std::map<int, double> cameraStamps;
std::map<int, double> robotStamps;
std::map<int, std::vector<rtabmap::CameraModel> > cameraModels;
std::map<int, cv::Mat> cameraDepths;
int imagesExported = 0;
@@ -1376,7 +1388,10 @@ int main(int argc, char * argv[])
{
// landmark, just add to list of poses
robotPoses.insert(*iter);
cameraStamps.insert(std::make_pair(iter->first, 0));
robotStamps.insert(std::make_pair(iter->first, 0));
landmarkPoses.insert(*iter);
landmarkStamps.insert(std::make_pair(iter->first, 0));
continue;
}
@@ -1628,7 +1643,7 @@ int main(int argc, char * argv[])
}
robotPoses.insert(std::make_pair(iter->first, iter->second));
cameraStamps.insert(std::make_pair(iter->first, stamp));
robotStamps.insert(std::make_pair(iter->first, stamp));
if(models.empty() && weight == -1 && !cameraModels.empty())
{
// For intermediate nodes, use latest models
@@ -1645,17 +1660,20 @@ int main(int argc, char * argv[])
if(cameraPoses.empty())
{
cameraPoses.resize(models.size());
cameraStamps.resize(models.size());
}
UASSERT_MSG(models.size() == cameraPoses.size(), "Not all nodes have same number of cameras to export camera poses.");
for(size_t i=0; i<models.size(); ++i)
{
cameraPoses[i].insert(std::make_pair(iter->first, iter->second*models[i].localTransform()));
cameraStamps[i].insert(std::make_pair(iter->first, stamp));
}
}
}
if(exportPosesScan && !data.laserScanCompressed().empty())
{
scanPoses.insert(std::make_pair(iter->first, iter->second*data.laserScanCompressed().localTransform()));
scanStamps.insert(std::make_pair(iter->first, stamp));
}
if(exportPosesGps || exportGps>=0)
@@ -1688,6 +1706,7 @@ int main(int argc, char * argv[])
if(exportPosesGt && !gt.isNull())
{
gtPoses.insert(std::make_pair(iter->first, gt));
gtStamps.insert(std::make_pair(iter->first, stamp));
}
if(optimizedPoses.size() >= 500)
@@ -1750,11 +1769,11 @@ int main(int argc, char * argv[])
exportPosesFormat,
std::map<int, Transform>(robotPoses.lower_bound(1), robotPoses.end()),
links,
std::map<int, double>(cameraStamps.lower_bound(1), cameraStamps.end()));
std::map<int, double>(robotStamps.lower_bound(1), robotStamps.end()));
}
else
{
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, robotPoses, links, cameraStamps);
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, robotPoses, links, robotStamps);
}
cv::Vec3f vmin, vmax;
graph::computeMinMax(robotPoses, vmin, vmax);
@@ -1773,7 +1792,7 @@ int main(int argc, char * argv[])
outputPath = outputDirectory+"/"+baseName+"_camera_poses." + posesExt;
else
outputPath = outputDirectory+"/"+baseName+"_camera_poses_"+uNumber2Str((int)i)+"." + posesExt;
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, cameraPoses[i], std::multimap<int, Link>(), cameraStamps);
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, cameraPoses[i], std::multimap<int, Link>(), cameraStamps[i]);
cv::Vec3f vmin, vmax;
graph::computeMinMax(cameraPoses[i], vmin, vmax);
printf("%d camera poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
@@ -1786,7 +1805,7 @@ int main(int argc, char * argv[])
if(exportPosesScan)
{
std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt;
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), cameraStamps);
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), scanStamps);
cv::Vec3f min, max;
graph::computeMinMax(scanPoses, min, max);
printf("%d scan poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
@@ -1795,6 +1814,30 @@ int main(int argc, char * argv[])
min[0], min[1], min[2],
max[0], max[1], max[2]);
}
if(exportPosesScan)
{
std::string outputPath=outputDirectory+"/"+baseName+"_scan_poses." + posesExt;
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, scanPoses, std::multimap<int, Link>(), scanStamps);
cv::Vec3f min, max;
graph::computeMinMax(scanPoses, min, max);
printf("%d scan poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
(int)scanPoses.size(),
outputPath.c_str(),
min[0], min[1], min[2],
max[0], max[1], max[2]);
}
if(exportPosesLandmarks)
{
std::string outputPath=outputDirectory+"/"+baseName+"_landmark_poses." + posesExt;
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, landmarkPoses, std::multimap<int, Link>(), landmarkStamps);
cv::Vec3f min, max;
graph::computeMinMax(landmarkPoses, min, max);
printf("%d landmark poses exported to \"%s\". (min=[%f,%f,%f] max=[%f,%f,%f])\n",
(int)landmarkPoses.size(),
outputPath.c_str(),
min[0], min[1], min[2],
max[0], max[1], max[2]);
}
if(exportPosesGps)
{
std::string outputPath=outputDirectory+"/"+baseName+"_gps_poses." + posesExt;
@@ -1806,7 +1849,7 @@ int main(int argc, char * argv[])
if(exportPosesGt)
{
std::string outputPath=outputDirectory+"/"+baseName+"_gt_poses." + posesExt;
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gtPoses, std::multimap<int, Link>(), cameraStamps);
rtabmap::graph::exportPoses(outputPath, exportPosesFormat, gtPoses, std::multimap<int, Link>(), gtStamps);
printf("%d scan poses exported to \"%s\".\n",
(int)gtPoses.size(),
outputPath.c_str());
@@ -1999,7 +2042,7 @@ int main(int argc, char * argv[])
}
depth = rtabmap::util2d::cvtDepthFromFloat(depth);
std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f",cameraStamps.at(iter->first)))+".png";
std::string outputPath=dir+"/"+(exportImagesId?uNumber2Str(iter->first):uFormat("%f",robotStamps.at(iter->first)))+".png";
cv::imwrite(outputPath, depth);
}
}
@@ -2027,6 +2070,7 @@ int main(int argc, char * argv[])
cameraModelsProj,
textureRange,
textureAngle,
textureDepthError,
textureRoiRatios,
projMask,
distanceToCamPolicy,
@@ -2040,6 +2084,7 @@ int main(int argc, char * argv[])
cameraModelsProj,
textureRange,
textureAngle,
textureDepthError,
textureRoiRatios,
projMask,
distanceToCamPolicy,
tools/GlobalBundleAdjustment/main.cpp
+1 -1
View File
@@ -119,7 +119,7 @@ int main(int argc, char * argv[])
printf("Global bundle adjustment...\n");
Optimizer * optimizer = Optimizer::create(Optimizer::kTypeG2O, parameters);
optimizedPoses = optimizer->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links, nodes, true);
optimizedPoses = optimizer->optimizeBA(optimizedPoses.lower_bound(1)->first, optimizedPoses, links, nodes, true, parameters);
delete optimizer;
printf("Global bundle adjustment... done (%fs).\n", timer.ticks());
tools/Reprocess/main.cpp
+14 -7
View File
@@ -1024,7 +1024,7 @@ int main(int argc, char * argv[])
UTimer iterationTime;
std::string status;
if(!odometryIgnored && info.odomPose.isNull())
if(!odometryIgnored && info.odomPose.isNull() && incrementalMemory)
{
printf("Skipping node %d as it doesn't have odometry pose set.\n", data.id());
}
@@ -1235,15 +1235,22 @@ int main(int argc, char * argv[])
localizationAngleVariations.push_back(stats.data().at(Statistics::kLoopOdom_correction_angle()));
}
if(exportPoses && !info.odomPose.isNull())
if(exportPoses)
{
if(!odomTrajectoryPoses.empty())
if(!info.odomPose.isNull())
{
int previousId = odomTrajectoryPoses.rbegin()->first;
odomTrajectoryLinks.insert(std::make_pair(previousId, Link(previousId, refId, Link::kNeighbor, odomTrajectoryPoses.rbegin()->second.inverse()*info.odomPose, info.odomCovariance)));
if(!odomTrajectoryPoses.empty())
{
int previousId = odomTrajectoryPoses.rbegin()->first;
odomTrajectoryLinks.insert(std::make_pair(previousId, Link(previousId, refId, Link::kNeighbor, odomTrajectoryPoses.rbegin()->second.inverse()*info.odomPose, info.odomCovariance)));
}
odomTrajectoryPoses.insert(std::make_pair(refId, info.odomPose));
localizationPoses.insert(std::make_pair(refId, stats.mapCorrection()*info.odomPose));
}
else
{
localizationPoses.insert(std::make_pair(refId, rtabmap.getLastLocalizationPose()));
}
odomTrajectoryPoses.insert(std::make_pair(refId, info.odomPose));
localizationPoses.insert(std::make_pair(refId, stats.mapCorrection()*info.odomPose));
}
}