mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-03 10:00:23 +08:00
fixed compilation error on Windows (on util3d::removeNaNFromPointCloud() method)
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:
@@ -96,7 +96,7 @@ typename pcl::PointCloud<PointT>::Ptr passThrough(
|
|||||||
}
|
}
|
||||||
|
|
||||||
template<typename PointT>
|
template<typename PointT>
|
||||||
typename pcl::PointCloud<PointT>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
|
typename pcl::PointCloud<PointT>::Ptr removeNaNFromPointCloud(
|
||||||
const typename pcl::PointCloud<PointT>::Ptr & cloud)
|
const typename pcl::PointCloud<PointT>::Ptr & cloud)
|
||||||
{
|
{
|
||||||
typedef typename pcl::PointCloud<PointT> PointCloud;
|
typedef typename pcl::PointCloud<PointT> PointCloud;
|
||||||
@@ -108,7 +108,7 @@ typename pcl::PointCloud<PointT>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
|
|||||||
}
|
}
|
||||||
|
|
||||||
template<typename PointT>
|
template<typename PointT>
|
||||||
typename pcl::PointCloud<PointT>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
|
typename pcl::PointCloud<PointT>::Ptr removeNaNNormalsFromPointCloud(
|
||||||
const typename pcl::PointCloud<PointT>::Ptr & cloud)
|
const typename pcl::PointCloud<PointT>::Ptr & cloud)
|
||||||
{
|
{
|
||||||
typedef typename pcl::PointCloud<PointT> PointCloud;
|
typedef typename pcl::PointCloud<PointT> PointCloud;
|
||||||
@@ -120,7 +120,7 @@ typename pcl::PointCloud<PointT>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud
|
|||||||
}
|
}
|
||||||
|
|
||||||
template<typename PointT>
|
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 typename pcl::PointCloud<PointT>::Ptr & cloud,
|
||||||
const Transform & transform)
|
const Transform & transform)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -1964,7 +1964,7 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
|
|||||||
|
|
||||||
if(image.size())
|
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
|
else
|
||||||
{
|
{
|
||||||
@@ -2015,7 +2015,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
|
|||||||
|
|
||||||
if(depth.size())
|
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
|
else
|
||||||
{
|
{
|
||||||
@@ -2045,7 +2045,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
|
|||||||
|
|
||||||
if(depth2d.size())
|
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
|
else
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -92,7 +92,7 @@ void Feature2D::filterKeypointsByDepth(
|
|||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
cv::Mat newDescriptors(keypoints.size(), descriptors.cols, descriptors.type());
|
cv::Mat newDescriptors((int)keypoints.size(), descriptors.cols, descriptors.type());
|
||||||
int di = 0;
|
int di = 0;
|
||||||
for(unsigned int i=0; i<indexes.size(); ++i)
|
for(unsigned int i=0; i<indexes.size(); ++i)
|
||||||
{
|
{
|
||||||
@@ -159,7 +159,7 @@ void Feature2D::filterKeypointsByDisparity(
|
|||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
cv::Mat newDescriptors(keypoints.size(), descriptors.cols, descriptors.type());
|
cv::Mat newDescriptors((int)keypoints.size(), descriptors.cols, descriptors.type());
|
||||||
int di = 0;
|
int di = 0;
|
||||||
for(unsigned int i=0; i<indexes.size(); ++i)
|
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
|
// 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::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
|
||||||
std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
|
std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
|
||||||
cv::Mat descriptorsTmp;
|
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());
|
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
|
||||||
keypoints = kptsTmp;
|
keypoints = kptsTmp;
|
||||||
if(descriptors.rows)
|
if(descriptors.rows)
|
||||||
|
|||||||
@@ -1033,7 +1033,7 @@ void Memory::clear()
|
|||||||
_lastSignature?_lastSignature->id():0,
|
_lastSignature?_lastSignature->id():0,
|
||||||
UProcessInfo::getMemoryUsage(),
|
UProcessInfo::getMemoryUsage(),
|
||||||
_dbDriver->getMemoryUsed(),
|
_dbDriver->getMemoryUsed(),
|
||||||
_vwd->getVisualWords().size());
|
(int)_vwd->getVisualWords().size());
|
||||||
}
|
}
|
||||||
UDEBUG("");
|
UDEBUG("");
|
||||||
|
|
||||||
@@ -2936,7 +2936,7 @@ int Memory::getNi(int signatureId) const
|
|||||||
const Signature * s = this->getSignature(signatureId);
|
const Signature * s = this->getSignature(signatureId);
|
||||||
if(s)
|
if(s)
|
||||||
{
|
{
|
||||||
ni = ((Signature *)s)->getWords().size();
|
ni = (int)((Signature *)s)->getWords().size();
|
||||||
}
|
}
|
||||||
else
|
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;
|
int meanWordsPerLocation = 0;
|
||||||
if(treeSize > 0)
|
if(treeSize > 0)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -228,7 +228,7 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
|
|||||||
const Signature * newSignature = _memory->getLastWorkingSignature();
|
const Signature * newSignature = _memory->getLastWorkingSignature();
|
||||||
if(newSignature)
|
if(newSignature)
|
||||||
{
|
{
|
||||||
nFeatures = newSignature->getWords().size();
|
nFeatures = (int)newSignature->getWords().size();
|
||||||
}
|
}
|
||||||
|
|
||||||
if(previousSignature && newSignature)
|
if(previousSignature && newSignature)
|
||||||
@@ -260,7 +260,7 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
|
|||||||
|
|
||||||
if((int)inliers1->size() >= this->getMinInliers())
|
if((int)inliers1->size() >= this->getMinInliers())
|
||||||
{
|
{
|
||||||
correspondences = inliers1->size();
|
correspondences = (int)inliers1->size();
|
||||||
|
|
||||||
// the transform returned is global odometry pose, not incremental one
|
// the transform returned is global odometry pose, not incremental one
|
||||||
std::vector<int> inliersV;
|
std::vector<int> inliersV;
|
||||||
@@ -272,7 +272,7 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
|
|||||||
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
|
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
|
||||||
&inliersV);
|
&inliersV);
|
||||||
|
|
||||||
inliers = inliersV.size();
|
inliers = (int)inliersV.size();
|
||||||
if(!transform.isNull())
|
if(!transform.isNull())
|
||||||
{
|
{
|
||||||
// make it incremental
|
// make it incremental
|
||||||
|
|||||||
@@ -280,6 +280,15 @@ void Rtabmap::init(const std::string & configFile, const std::string & databaseP
|
|||||||
|
|
||||||
void Rtabmap::close()
|
void Rtabmap::close()
|
||||||
{
|
{
|
||||||
|
_retrievedId = 0;
|
||||||
|
_lcHypothesisValue = 0;
|
||||||
|
_lcHypothesisId = 0;
|
||||||
|
_lastProcessTime = 0.0;
|
||||||
|
_optimizedPoses.clear();
|
||||||
|
_constraints.clear();
|
||||||
|
_mapCorrection.setIdentity();
|
||||||
|
_mapTransform.setIdentity();
|
||||||
|
|
||||||
flushStatisticLogs();
|
flushStatisticLogs();
|
||||||
if(_foutFloat)
|
if(_foutFloat)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -108,7 +108,7 @@ void Signature::addNeighbor(int neighbor, const Transform & transform)
|
|||||||
|
|
||||||
void Signature::removeNeighbor(int neighborId)
|
void Signature::removeNeighbor(int neighborId)
|
||||||
{
|
{
|
||||||
int count = _neighbors.erase(neighborId);
|
int count = (int)_neighbors.erase(neighborId);
|
||||||
if(count)
|
if(count)
|
||||||
{
|
{
|
||||||
_neighborsModified = true;
|
_neighborsModified = true;
|
||||||
@@ -172,7 +172,7 @@ float Signature::compareTo(const Signature * s) const
|
|||||||
if(words.size() != 0 && _words.size() != 0)
|
if(words.size() != 0 && _words.size() != 0)
|
||||||
{
|
{
|
||||||
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
|
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);
|
EpipolarGeometry::findPairs(words, _words, pairs);
|
||||||
|
|
||||||
similarity = float(pairs.size()) / float(totalWords);
|
similarity = float(pairs.size()) / float(totalWords);
|
||||||
@@ -244,22 +244,7 @@ SensorData Signature::toSensorData()
|
|||||||
|
|
||||||
void Signature::uncompressData()
|
void Signature::uncompressData()
|
||||||
{
|
{
|
||||||
if(_imageRaw.empty() && _image.size())
|
uncompressData(&_imageRaw, &_depthRaw, &_depth2DRaw);
|
||||||
{
|
|
||||||
//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();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Signature::uncompressData(cv::Mat * image, cv::Mat * depth, cv::Mat * depth2D) const
|
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();
|
ctImage.join();
|
||||||
ctDepth.join();
|
ctDepth.join();
|
||||||
ctDepth2D.join();
|
ctDepth2D.join();
|
||||||
if(image)
|
if(image && image->empty())
|
||||||
{
|
{
|
||||||
*image = ctImage.getUncompressedData();
|
*image = ctImage.getUncompressedData();
|
||||||
}
|
}
|
||||||
if(depth)
|
if(depth && depth->empty())
|
||||||
{
|
{
|
||||||
*depth = ctDepth.getUncompressedData();
|
*depth = ctDepth.getUncompressedData();
|
||||||
}
|
}
|
||||||
if(depth2D)
|
if(depth2D && depth2D->empty())
|
||||||
{
|
{
|
||||||
*depth2D = ctDepth2D.getUncompressedData();
|
*depth2D = ctDepth2D.getUncompressedData();
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1443,7 +1443,7 @@ Transform transformFromXYZCorrespondences(
|
|||||||
std::vector<int> target_indices (cloud1->size());
|
std::vector<int> target_indices (cloud1->size());
|
||||||
|
|
||||||
// Copy the query-match indices
|
// 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;
|
source_indices[i] = i;
|
||||||
target_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
|
// Select the new inliers based on the optimized coefficients and new threshold
|
||||||
model->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers);
|
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.",
|
UDEBUG("RANSAC refineModel: Number of inliers found (before/after): %d/%d, with an error threshold of %f.",
|
||||||
prev_inliers.size (), new_inliers.size (), error_threshold);
|
(int)prev_inliers.size (), (int)new_inliers.size (), error_threshold);
|
||||||
|
|
||||||
if (new_inliers.empty ())
|
if (new_inliers.empty ())
|
||||||
{
|
{
|
||||||
@@ -1503,7 +1503,7 @@ Transform transformFromXYZCorrespondences(
|
|||||||
double variance = model->computeVariance ();
|
double variance = model->computeVariance ();
|
||||||
error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
|
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);
|
error_threshold, refine_iterations, refineModelIterations);
|
||||||
inlier_changed = false;
|
inlier_changed = false;
|
||||||
std::swap (prev_inliers, new_inliers);
|
std::swap (prev_inliers, new_inliers);
|
||||||
@@ -1568,7 +1568,7 @@ Transform transformFromXYZCorrespondences(
|
|||||||
bestTransformation.row (3) = model_coefficients.segment<4>(12);
|
bestTransformation.row (3) = model_coefficients.segment<4>(12);
|
||||||
|
|
||||||
transform = util3d::transformFromEigen4f(bestTransformation);
|
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)
|
return transform.inverse(); // inverse to get actual pose transform (not correspondences transform)
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -521,7 +521,7 @@ void DatabaseViewer::generateTOROGraph()
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
bool ok = false;
|
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)
|
if(ok)
|
||||||
{
|
{
|
||||||
@@ -842,7 +842,7 @@ void DatabaseViewer::generate3DMap()
|
|||||||
if(ids.size() > 0)
|
if(ids.size() > 0)
|
||||||
{
|
{
|
||||||
rtabmap::DetailedProgressDialog progressDialog;
|
rtabmap::DetailedProgressDialog progressDialog;
|
||||||
progressDialog.setMaximumSteps(ids.size()+2);
|
progressDialog.setMaximumSteps((int)ids.size()+2);
|
||||||
progressDialog.show();
|
progressDialog.show();
|
||||||
|
|
||||||
progressDialog.appendText("Graph generation...");
|
progressDialog.appendText("Graph generation...");
|
||||||
|
|||||||
@@ -728,7 +728,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
|
|||||||
Signature signature = stat.getSignature();
|
Signature signature = stat.getSignature();
|
||||||
signature.uncompressData(); // make sure data are uncompressed
|
signature.uncompressData(); // make sure data are uncompressed
|
||||||
_cachedSignatures.insert(stat.getSignature().id(), signature);
|
_cachedSignatures.insert(stat.getSignature().id(), signature);
|
||||||
UDEBUG("");
|
|
||||||
|
|
||||||
// map ids
|
// map ids
|
||||||
for(std::map<int, int>::const_iterator iter = stat.getMapIds().begin();
|
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));
|
int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
|
||||||
float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
|
float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
|
||||||
int matchId = 0;
|
int matchId = 0;
|
||||||
cv::Mat loopImage;
|
Signature loopSignature;
|
||||||
cv::Mat loopDepth;
|
|
||||||
cv::Mat loopDepth2D;
|
|
||||||
int shownLoopId = 0;
|
int shownLoopId = 0;
|
||||||
if(highestHypothesisId > 0 || stat.localLoopClosureId()>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);
|
QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
|
||||||
if(iter != _cachedSignatures.end())
|
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
|
//update image views
|
||||||
{
|
{
|
||||||
UCvMat2QImageThread qimageThread(signature.getImageRaw());
|
UCvMat2QImageThread qimageThread(signature.getImageRaw());
|
||||||
UCvMat2QImageThread qimageLoopThread(loopImage);
|
UCvMat2QImageThread qimageLoopThread(loopSignature.getImageRaw());
|
||||||
UCvMat2QImageThread qdepthThread(signature.getDepthRaw());
|
UCvMat2QImageThread qdepthThread(signature.getDepthRaw());
|
||||||
UCvMat2QImageThread qdepthLoopThread(loopDepth);
|
UCvMat2QImageThread qdepthLoopThread(loopSignature.getDepthRaw());
|
||||||
qimageThread.start();
|
qimageThread.start();
|
||||||
qdepthThread.start();
|
qdepthThread.start();
|
||||||
qimageLoopThread.start();
|
qimageLoopThread.start();
|
||||||
@@ -873,16 +871,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
|
|||||||
_ui->imageView_loopClosure->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio);
|
_ui->imageView_loopClosure->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio);
|
||||||
|
|
||||||
// do it after scaling
|
// do it after scaling
|
||||||
std::multimap<int, cv::KeyPoint> loopWords;
|
this->drawKeypoints(signature.getWords(), loopSignature.getWords());
|
||||||
if(shownLoopId)
|
|
||||||
{
|
|
||||||
QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
|
|
||||||
if(iter!=_cachedSignatures.end())
|
|
||||||
{
|
|
||||||
loopWords = iter->getWords();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
this->drawKeypoints(signature.getWords(), loopWords);
|
|
||||||
|
|
||||||
if(_preferencesDialog->isImageFlipped())
|
if(_preferencesDialog->isImageFlipped())
|
||||||
{
|
{
|
||||||
@@ -893,7 +882,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
|
|||||||
UDEBUG("time= %d ms", time.restart());
|
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 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
|
// PDF AND LIKELIHOOD
|
||||||
if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
|
if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
|
||||||
@@ -952,36 +941,19 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
|
|||||||
|
|
||||||
// loop closure view
|
// loop closure view
|
||||||
if((stat.loopClosureId() > 0 || stat.localLoopClosureId() > 0) &&
|
if((stat.loopClosureId() > 0 || stat.localLoopClosureId() > 0) &&
|
||||||
!stat.loopClosureTransform().isNull())
|
!stat.loopClosureTransform().isNull() &&
|
||||||
|
!loopSignature.getImageRaw().empty())
|
||||||
{
|
{
|
||||||
// the last loop closure data
|
// the last loop closure data
|
||||||
Transform loopClosureTransform = stat.loopClosureTransform();
|
Transform loopClosureTransform = stat.loopClosureTransform();
|
||||||
int loopOldId = stat.loopClosureId();
|
signature.setPose(loopClosureTransform);
|
||||||
if(!loopOldId)
|
_ui->widget_loopClosureViewer->setData(loopSignature, signature);
|
||||||
|
if(_ui->dockWidget_loopClosureViewer->isVisible())
|
||||||
{
|
{
|
||||||
loopOldId = stat.localLoopClosureId();
|
UTimer loopTimer;
|
||||||
}
|
_ui->widget_loopClosureViewer->updateView();
|
||||||
|
UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
|
||||||
QMap<int, Signature>::iterator oldIter = _cachedSignatures.find(loopOldId);
|
_ui->statsToolBox->updateStat("/Gui RGB-D closure view/ms", stat.refImageId(), int(loopTimer.elapsed()*1000.0f));
|
||||||
|
|
||||||
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));
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
UDEBUG("time= %d ms", time.restart());
|
UDEBUG("time= %d ms", time.restart());
|
||||||
@@ -1533,7 +1505,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
|
|||||||
UINFO(" poses = %d", event.getPoses().size());
|
UINFO(" poses = %d", event.getPoses().size());
|
||||||
UINFO(" constraints = %d", event.getConstraints().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()));
|
_initProgressDialog->appendText(QString("Inserting data in the cache (%1 signatures downloaded)...").arg(event.getSignatures().size()));
|
||||||
QApplication::processEvents();
|
QApplication::processEvents();
|
||||||
|
|
||||||
@@ -4060,6 +4032,7 @@ void MainWindow::changeState(MainWindow::State newState)
|
|||||||
case kInitializing:
|
case kInitializing:
|
||||||
_ui->actionNew_database->setEnabled(false);
|
_ui->actionNew_database->setEnabled(false);
|
||||||
_ui->actionOpen_database->setEnabled(false);
|
_ui->actionOpen_database->setEnabled(false);
|
||||||
|
_ui->actionClose_database->setEnabled(false);
|
||||||
_ui->actionEdit_database->setEnabled(false);
|
_ui->actionEdit_database->setEnabled(false);
|
||||||
_state = newState;
|
_state = newState;
|
||||||
break;
|
break;
|
||||||
@@ -4101,6 +4074,10 @@ void MainWindow::changeState(MainWindow::State newState)
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
case kDetecting:
|
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->actionStart->setEnabled(false);
|
||||||
_ui->actionPause->setEnabled(true);
|
_ui->actionPause->setEnabled(true);
|
||||||
_ui->actionPause->setChecked(false);
|
_ui->actionPause->setChecked(false);
|
||||||
|
|||||||
Reference in New Issue
Block a user