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

fixed compilation error on Windows (on util3d::removeNaNFromPointCloud() method)

Browse Source 
increased stability of new actions added (New/Open/Close database).

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1933 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-28 04:16:36 +00:00
parent d950c88373
commit 4340748fec
10 changed files with 60 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/impl/util3d.hpp
+3 -3
View File
@@ -96,7 +96,7 @@ typename pcl::PointCloud<PointT>::Ptr passThrough(
}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
typename pcl::PointCloud<PointT>::Ptr removeNaNFromPointCloud(
const typename pcl::PointCloud<PointT>::Ptr & cloud)
{
typedef typename pcl::PointCloud<PointT> PointCloud;
@@ -108,7 +108,7 @@ typename pcl::PointCloud<PointT>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
typename pcl::PointCloud<PointT>::Ptr removeNaNNormalsFromPointCloud(
const typename pcl::PointCloud<PointT>::Ptr & cloud)
{
typedef typename pcl::PointCloud<PointT> PointCloud;
@@ -120,7 +120,7 @@ typename pcl::PointCloud<PointT>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud
}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr RTABMAP_EXP transformPointCloud(
typename pcl::PointCloud<PointT>::Ptr transformPointCloud(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const Transform & transform)
{
corelib/src/DBDriverSqlite3.cpp
+3 -3
View File
@@ -1964,7 +1964,7 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
if(image.size())
{
rc = sqlite3_bind_blob(ppStmt, index++, image.data(), image.size(), SQLITE_STATIC);
rc = sqlite3_bind_blob(ppStmt, index++, image.data(), (int)image.size(), SQLITE_STATIC);
}
else
{
@@ -2015,7 +2015,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
if(depth.size())
{
rc = sqlite3_bind_blob(ppStmt, index++, depth.data(), depth.size(), SQLITE_STATIC);
rc = sqlite3_bind_blob(ppStmt, index++, depth.data(), (int)depth.size(), SQLITE_STATIC);
}
else
{
@@ -2045,7 +2045,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
if(depth2d.size())
{
rc = sqlite3_bind_blob(ppStmt, index++, depth2d.data(), depth2d.size(), SQLITE_STATIC);
rc = sqlite3_bind_blob(ppStmt, index++, depth2d.data(), (int)depth2d.size(), SQLITE_STATIC);
}
else
{
corelib/src/Features2d.cpp
+4 -4
View File
@@ -92,7 +92,7 @@ void Feature2D::filterKeypointsByDepth(
}
else
{
cv::Mat newDescriptors(keypoints.size(), descriptors.cols, descriptors.type());
cv::Mat newDescriptors((int)keypoints.size(), descriptors.cols, descriptors.type());
int di = 0;
for(unsigned int i=0; i<indexes.size(); ++i)
{
@@ -159,7 +159,7 @@ void Feature2D::filterKeypointsByDisparity(
}
else
{
cv::Mat newDescriptors(keypoints.size(), descriptors.cols, descriptors.type());
cv::Mat newDescriptors((int)keypoints.size(), descriptors.cols, descriptors.type());
int di = 0;
for(unsigned int i=0; i<indexes.size(); ++i)
{
@@ -205,7 +205,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
}
// Remove them from the signature
int removed = hessianMap.size()-maxKeypoints;
int removed = (int)hessianMap.size()-maxKeypoints;
std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
cv::Mat descriptorsTmp;
@@ -228,7 +228,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
}
}
}
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
keypoints = kptsTmp;
if(descriptors.rows)
corelib/src/Memory.cpp
+3 -3
View File
@@ -1033,7 +1033,7 @@ void Memory::clear()
_lastSignature?_lastSignature->id():0,
UProcessInfo::getMemoryUsage(),
_dbDriver->getMemoryUsed(),
_vwd->getVisualWords().size());
(int)_vwd->getVisualWords().size());
}
UDEBUG("");
@@ -2936,7 +2936,7 @@ int Memory::getNi(int signatureId) const
const Signature * s = this->getSignature(signatureId);
if(s)
{
ni = ((Signature *)s)->getWords().size();
ni = (int)((Signature *)s)->getWords().size();
}
else
{
@@ -3055,7 +3055,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S
}
}
int treeSize= _workingMem.size() + _stMem.size();
int treeSize= int(_workingMem.size() + _stMem.size());
int meanWordsPerLocation = 0;
if(treeSize > 0)
{
corelib/src/Odometry.cpp
+3 -3
View File
@@ -228,7 +228,7 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
const Signature * newSignature = _memory->getLastWorkingSignature();
if(newSignature)
{
nFeatures = newSignature->getWords().size();
nFeatures = (int)newSignature->getWords().size();
}
if(previousSignature && newSignature)
@@ -260,7 +260,7 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
if((int)inliers1->size() >= this->getMinInliers())
{
correspondences = inliers1->size();
correspondences = (int)inliers1->size();
// the transform returned is global odometry pose, not incremental one
std::vector<int> inliersV;
@@ -272,7 +272,7 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
&inliersV);
inliers = inliersV.size();
inliers = (int)inliersV.size();
if(!transform.isNull())
{
// make it incremental
corelib/src/Rtabmap.cpp
+9
View File
@@ -280,6 +280,15 @@ void Rtabmap::init(const std::string & configFile, const std::string & databaseP
void Rtabmap::close()
{
_retrievedId = 0;
_lcHypothesisValue = 0;
_lcHypothesisId = 0;
_lastProcessTime = 0.0;
_optimizedPoses.clear();
_constraints.clear();
_mapCorrection.setIdentity();
_mapTransform.setIdentity();
flushStatisticLogs();
if(_foutFloat)
{
corelib/src/Signature.cpp
+6 -21
View File
@@ -108,7 +108,7 @@ void Signature::addNeighbor(int neighbor, const Transform & transform)
void Signature::removeNeighbor(int neighborId)
{
int count = _neighbors.erase(neighborId);
int count = (int)_neighbors.erase(neighborId);
if(count)
{
_neighborsModified = true;
@@ -172,7 +172,7 @@ float Signature::compareTo(const Signature * s) const
if(words.size() != 0 && _words.size() != 0)
{
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
int totalWords = _words.size()>words.size()?_words.size():words.size();
unsigned int totalWords = _words.size()>words.size()?_words.size():words.size();
EpipolarGeometry::findPairs(words, _words, pairs);
similarity = float(pairs.size()) / float(totalWords);
@@ -244,22 +244,7 @@ SensorData Signature::toSensorData()
void Signature::uncompressData()
{
if(_imageRaw.empty() && _image.size())
{
//uncompress data
util3d::CompressionThread ctImage(&_image, true);
util3d::CompressionThread ctDepth(&_depth, true);
util3d::CompressionThread ctDepth2D(&_depth2D, false);
ctImage.start();
ctDepth.start();
ctDepth2D.start();
ctImage.join();
ctDepth.join();
ctDepth2D.join();
_imageRaw = ctImage.getUncompressedData();
_depthRaw = ctDepth.getUncompressedData();
_depth2DRaw = ctDepth2D.getUncompressedData();
}
uncompressData(&_imageRaw, &_depthRaw, &_depth2DRaw);
}
void Signature::uncompressData(cv::Mat * image, cv::Mat * depth, cv::Mat * depth2D) const
@@ -298,15 +283,15 @@ void Signature::uncompressData(cv::Mat * image, cv::Mat * depth, cv::Mat * depth
ctImage.join();
ctDepth.join();
ctDepth2D.join();
if(image)
if(image && image->empty())
{
*image = ctImage.getUncompressedData();
}
if(depth)
if(depth && depth->empty())
{
*depth = ctDepth.getUncompressedData();
}
if(depth2D)
if(depth2D && depth2D->empty())
{
*depth2D = ctDepth2D.getUncompressedData();
}
corelib/src/util3d.cpp
+5 -5
View File
@@ -1443,7 +1443,7 @@ Transform transformFromXYZCorrespondences(
std::vector<int> target_indices (cloud1->size());
// Copy the query-match indices
for (size_t i = 0; i < cloud1->size(); ++i)
for (int i = 0; i < (int)cloud1->size(); ++i)
{
source_indices[i] = i;
target_indices[i] = i;
@@ -1486,8 +1486,8 @@ Transform transformFromXYZCorrespondences(
// Select the new inliers based on the optimized coefficients and new threshold
model->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers);
UDEBUG("RANSAC refineModel: Number of inliers found (before/after): %zu/%zu, with an error threshold of %g.",
prev_inliers.size (), new_inliers.size (), error_threshold);
UDEBUG("RANSAC refineModel: Number of inliers found (before/after): %d/%d, with an error threshold of %f.",
(int)prev_inliers.size (), (int)new_inliers.size (), error_threshold);
if (new_inliers.empty ())
{
@@ -1503,7 +1503,7 @@ Transform transformFromXYZCorrespondences(
double variance = model->computeVariance ();
error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
UDEBUG ("RANSAC refineModel: New estimated error threshold: %g on iteration %d out of %d.",
UDEBUG ("RANSAC refineModel: New estimated error threshold: %f on iteration %d out of %d.",
error_threshold, refine_iterations, refineModelIterations);
inlier_changed = false;
std::swap (prev_inliers, new_inliers);
@@ -1568,7 +1568,7 @@ Transform transformFromXYZCorrespondences(
bestTransformation.row (3) = model_coefficients.segment<4>(12);
transform = util3d::transformFromEigen4f(bestTransformation);
UDEBUG("RANSAC inliers=%zu/%zu tf=%s", inliers.size(), cloud1->size(), transform.prettyPrint().c_str());
UDEBUG("RANSAC inliers=%d/%d tf=%s", (int)inliers.size(), (int)cloud1->size(), transform.prettyPrint().c_str());
return transform.inverse(); // inverse to get actual pose transform (not correspondences transform)
}
guilib/src/DatabaseViewer.cpp
+2 -2
View File
@@ -521,7 +521,7 @@ void DatabaseViewer::generateTOROGraph()
return;
}
bool ok = false;
int id = QInputDialog::getInt(this, tr("Which iteration?"), tr("Iteration (0 -> %1)").arg(graphes_.size()-1), graphes_.size()-1, 0, graphes_.size()-1, 1, &ok);
int id = QInputDialog::getInt(this, tr("Which iteration?"), tr("Iteration (0 -> %1)").arg((int)graphes_.size()-1), (int)graphes_.size()-1, 0, graphes_.size()-1, 1, &ok);
if(ok)
{
@@ -842,7 +842,7 @@ void DatabaseViewer::generate3DMap()
if(ids.size() > 0)
{
rtabmap::DetailedProgressDialog progressDialog;
progressDialog.setMaximumSteps(ids.size()+2);
progressDialog.setMaximumSteps((int)ids.size()+2);
progressDialog.show();
progressDialog.appendText("Graph generation...");
guilib/src/MainWindow.cpp
+22 -45
View File
@@ -728,7 +728,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
Signature signature = stat.getSignature();
signature.uncompressData(); // make sure data are uncompressed
_cachedSignatures.insert(stat.getSignature().id(), signature);
UDEBUG("");
// map ids
for(std::map<int, int>::const_iterator iter = stat.getMapIds().begin();
@@ -762,9 +761,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
int matchId = 0;
cv::Mat loopImage;
cv::Mat loopDepth;
cv::Mat loopDepth2D;
Signature loopSignature;
int shownLoopId = 0;
if(highestHypothesisId > 0 || stat.localLoopClosureId()>0)
{
@@ -811,7 +808,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
if(iter != _cachedSignatures.end())
{
iter.value().uncompressData(&loopImage, &loopDepth, &loopDepth2D);
iter.value().uncompressData();
loopSignature = iter.value();
}
}
}
@@ -821,9 +819,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
//update image views
{
UCvMat2QImageThread qimageThread(signature.getImageRaw());
UCvMat2QImageThread qimageLoopThread(loopImage);
UCvMat2QImageThread qimageLoopThread(loopSignature.getImageRaw());
UCvMat2QImageThread qdepthThread(signature.getDepthRaw());
UCvMat2QImageThread qdepthLoopThread(loopDepth);
UCvMat2QImageThread qdepthLoopThread(loopSignature.getDepthRaw());
qimageThread.start();
qdepthThread.start();
qimageLoopThread.start();
@@ -873,16 +871,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_ui->imageView_loopClosure->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio);
// do it after scaling
std::multimap<int, cv::KeyPoint> loopWords;
if(shownLoopId)
{
QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
if(iter!=_cachedSignatures.end())
{
loopWords = iter->getWords();
}
}
this->drawKeypoints(signature.getWords(), loopWords);
this->drawKeypoints(signature.getWords(), loopSignature.getWords());
if(_preferencesDialog->isImageFlipped())
{
@@ -893,7 +882,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
UDEBUG("time= %d ms", time.restart());
_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the last signature/", stat.refImageId(), signature.getWords().size());
_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the loop signature/", stat.refImageId(), loopWords.size());
_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the loop signature/", stat.refImageId(), loopSignature.getWords().size());
// PDF AND LIKELIHOOD
if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
@@ -952,36 +941,19 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
// loop closure view
if((stat.loopClosureId() > 0 || stat.localLoopClosureId() > 0) &&
!stat.loopClosureTransform().isNull())
!stat.loopClosureTransform().isNull() &&
!loopSignature.getImageRaw().empty())
{
// the last loop closure data
Transform loopClosureTransform = stat.loopClosureTransform();
int loopOldId = stat.loopClosureId();
if(!loopOldId)
signature.setPose(loopClosureTransform);
_ui->widget_loopClosureViewer->setData(loopSignature, signature);
if(_ui->dockWidget_loopClosureViewer->isVisible())
{
loopOldId = stat.localLoopClosureId();
}
QMap<int, Signature>::iterator oldIter = _cachedSignatures.find(loopOldId);
if(oldIter!=_cachedSignatures.end())
{
Signature old = oldIter.value();
if(old.getImageRaw().empty())
{
old.setImageRaw(loopImage);
old.setDepthRaw(loopDepth);
old.setDepth2DRaw(loopDepth2D);
}
signature.setPose(loopClosureTransform);
_ui->widget_loopClosureViewer->setData(*oldIter, signature);
if(_ui->dockWidget_loopClosureViewer->isVisible())
{
UTimer loopTimer;
_ui->widget_loopClosureViewer->updateView();
UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
_ui->statsToolBox->updateStat("/Gui RGB-D closure view/ms", stat.refImageId(), int(loopTimer.elapsed()*1000.0f));
}
UTimer loopTimer;
_ui->widget_loopClosureViewer->updateView();
UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
_ui->statsToolBox->updateStat("/Gui RGB-D closure view/ms", stat.refImageId(), int(loopTimer.elapsed()*1000.0f));
}
UDEBUG("time= %d ms", time.restart());
@@ -1533,7 +1505,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
UINFO(" poses = %d", event.getPoses().size());
UINFO(" constraints = %d", event.getConstraints().size());
_initProgressDialog->setMaximumSteps(event.getSignatures().size()+event.getPoses().size()+1);
_initProgressDialog->setMaximumSteps(int(event.getSignatures().size()+event.getPoses().size()+1));
_initProgressDialog->appendText(QString("Inserting data in the cache (%1 signatures downloaded)...").arg(event.getSignatures().size()));
QApplication::processEvents();
@@ -4060,6 +4032,7 @@ void MainWindow::changeState(MainWindow::State newState)
case kInitializing:
_ui->actionNew_database->setEnabled(false);
_ui->actionOpen_database->setEnabled(false);
_ui->actionClose_database->setEnabled(false);
_ui->actionEdit_database->setEnabled(false);
_state = newState;
break;
@@ -4101,6 +4074,10 @@ void MainWindow::changeState(MainWindow::State newState)
break;
case kDetecting:
_ui->actionNew_database->setEnabled(false);
_ui->actionOpen_database->setEnabled(false);
_ui->actionClose_database->setEnabled(false);
_ui->actionEdit_database->setEnabled(false);
_ui->actionStart->setEnabled(false);
_ui->actionPause->setEnabled(true);
_ui->actionPause->setChecked(false);