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

Moved some util3d methods to Transform.h, Graph.h and Compression.h

Browse Source 
Added computePath() method implementating A star on graph
GUI: SetWindowModified() and user should explicitly save the GUI config to keep them
Calibration: added mirror checkbox, added device id argument
This commit is contained in:
Mathieu Labbe
2015-01-23 11:17:42 -05:00
parent fdf7f69783
commit 7a6bf630ca
34 changed files with 1817 additions and 1282 deletions
Download Patch File Download Diff File Expand all files Collapse all files
guilib/src/DatabaseViewer.cpp
+32 -30
View File
@@ -49,6 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/Compression.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/gui/DataRecorder.h"
#include "rtabmap/core/SensorData.h"
#include "ExportDialog.h"
@@ -239,7 +241,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
// Added links
for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
{
std::multimap<int, rtabmap::Link>::iterator refinedIter = util3d::findLink(linksRefined_, iter->second.from(), iter->second.to());
std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::findLink(linksRefined_, iter->second.from(), iter->second.to());
if(refinedIter != linksRefined_.end())
{
memory_->addLoopClosureLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), refinedIter->second.type(), refinedIter->second.variance());
@@ -373,7 +375,7 @@ void DatabaseViewer::extractImages()
cv::Mat compressedRgb = memory_->getImageCompressed(id);
if(!compressedRgb.empty())
{
cv::Mat imageMat = rtabmap::util3d::uncompressImage(compressedRgb);
cv::Mat imageMat = rtabmap::uncompressImage(compressedRgb);
cv::imwrite(QString("%1/%2.png").arg(path).arg(id).toStdString(), imageMat);
UINFO(QString("Saved %1/%2.png").arg(path).arg(id).toStdString().c_str());
}
@@ -569,7 +571,7 @@ void DatabaseViewer::generateTOROGraph()
QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".graph", tr("TORO file (*.graph)"));
if(!path.isEmpty())
{
rtabmap::util3d::saveTOROGraph(path.toStdString(), uValueAt(graphes_, id), links);
rtabmap::saveTOROGraph(path.toStdString(), uValueAt(graphes_, id), links);
}
}
}
@@ -801,7 +803,7 @@ void DatabaseViewer::detectMoreLoopClosures()
for(int n=0; n<iterations; ++n)
{
UINFO("iteration %d/%d", n+1, iterations);
std::multimap<int, int> clusters = util3d::radiusPosesClustering(
std::multimap<int, int> clusters = rtabmap::radiusPosesClustering(
optimizedPoses,
ui_->doubleSpinBox_detectMore_radius->value(),
ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
@@ -1216,7 +1218,7 @@ void DatabaseViewer::updateStereo(const Signature * data)
if(pcl::isFinite(tmpPt))
{
pt = pcl::transformPoint(tmpPt, util3d::transformToEigen3f(data->getLocalTransform()));
pt = pcl::transformPoint(tmpPt, data->getLocalTransform().toEigen3f());
if(fabs(pt.x) > 2 || fabs(pt.y) > 2 || fabs(pt.z) > 2)
{
status[i] = 100; //blue
@@ -1413,7 +1415,7 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo,
bool updateImageSliders)
{
std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, linkIn.from(), linkIn.to());
std::multimap<int, Link>::iterator iter = rtabmap::findLink(linksRefined_, linkIn.from(), linkIn.to());
rtabmap::Link link = linkIn;
if(iter != linksRefined_.end())
{
@@ -1696,7 +1698,7 @@ void DatabaseViewer::updateConstraintButtons()
//check for modified link
bool modified = false;
std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, currentLink.from(), currentLink.to());
std::multimap<int, Link>::iterator iter = rtabmap::findLink(linksRefined_, currentLink.from(), currentLink.to());
if(iter != linksRefined_.end())
{
currentLink = iter->second;
@@ -1726,7 +1728,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
if(!data.getLaserScanCompressed().empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cv::Mat laserScan = rtabmap::util3d::uncompressData(data.getLaserScanCompressed());
cv::Mat laserScan = rtabmap::uncompressData(data.getLaserScanCompressed());
cloud = rtabmap::util3d::laserScanToPointCloud(laserScan);
scans_.insert(std::make_pair(ids_.at(i), cloud));
}
@@ -1786,8 +1788,8 @@ void DatabaseViewer::updateGraphView()
graphes_.push_back(poses_);
ui_->actionGenerate_TORO_graph_graph->setEnabled(true);
std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
std::map<int, int> depthGraph = util3d::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), 0);
util3d::optimizeTOROGraph(
std::map<int, int> depthGraph = rtabmap::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), 0);
rtabmap::optimizeTOROGraph(
depthGraph,
poses_,
links, finalPoses,
@@ -1813,21 +1815,21 @@ void DatabaseViewer::updateGraphView()
Link DatabaseViewer::findActiveLink(int from, int to)
{
Link link;
std::multimap<int, Link>::iterator findIter = util3d::findLink(linksRefined_, from ,to);
std::multimap<int, Link>::iterator findIter = rtabmap::findLink(linksRefined_, from ,to);
if(findIter != linksRefined_.end())
{
link = findIter->second;
}
else
{
findIter = util3d::findLink(linksAdded_, from ,to);
findIter = rtabmap::findLink(linksAdded_, from ,to);
if(findIter != linksAdded_.end())
{
link = findIter->second;
}
else if(!containsLink(linksRemoved_, from ,to))
{
findIter = util3d::findLink(links_, from ,to);
findIter = rtabmap::findLink(links_, from ,to);
if(findIter != links_.end())
{
link = findIter->second;
@@ -1839,7 +1841,7 @@ Link DatabaseViewer::findActiveLink(int from, int to)
bool DatabaseViewer::containsLink(std::multimap<int, Link> & links, int from, int to)
{
return util3d::findLink(links, from, to) != links.end();
return rtabmap::findLink(links, from, to) != links.end();
}
void DatabaseViewer::refineConstraint()
@@ -1897,8 +1899,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
if(ui_->checkBox_icp_2d->isChecked())
{
//2D
cv::Mat oldLaserScan = util3d::uncompressData(dataFrom.getLaserScanCompressed());
cv::Mat newLaserScan = util3d::uncompressData(dataTo.getLaserScanCompressed());
cv::Mat oldLaserScan = rtabmap::uncompressData(dataFrom.getLaserScanCompressed());
cv::Mat newLaserScan = rtabmap::uncompressData(dataTo.getLaserScanCompressed());
if(!oldLaserScan.empty() && !newLaserScan.empty())
{
@@ -1928,13 +1930,13 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
else
{
//3D
cv::Mat depthA = rtabmap::util3d::uncompressImage(dataFrom.getDepthCompressed());
cv::Mat depthB = rtabmap::util3d::uncompressImage(dataTo.getDepthCompressed());
cv::Mat depthA = rtabmap::uncompressImage(dataFrom.getDepthCompressed());
cv::Mat depthB = rtabmap::uncompressImage(dataTo.getDepthCompressed());
if(depthA.type() == CV_8UC1)
{
cv::Mat leftMono;
cv::Mat left = rtabmap::util3d::uncompressImage(dataFrom.getImageCompressed());
cv::Mat left = rtabmap::uncompressImage(dataFrom.getImageCompressed());
if(left.channels() > 1)
{
cv::cvtColor(left, leftMono, CV_BGR2GRAY);
@@ -1967,7 +1969,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool updateGraph)
if(depthB.type() == CV_8UC1)
{
cv::Mat leftMono;
cv::Mat left = rtabmap::util3d::uncompressImage(dataTo.getImageCompressed());
cv::Mat left = rtabmap::uncompressImage(dataTo.getImageCompressed());
if(left.channels() > 1)
{
cv::cvtColor(left, leftMono, CV_BGR2GRAY);
@@ -2266,7 +2268,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
// We are 2D here, make sure the guess has only YAW rotation
float x,y,z,r,p,yaw;
t.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
t = util3d::transformFromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
t = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
}
// transform is valid, make a link
@@ -2277,7 +2279,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
else if(containsLink(linksRemoved_, from, to))
{
//simply remove from linksRemoved
linksRemoved_.erase(util3d::findLink(linksRemoved_, from, to));
linksRemoved_.erase(rtabmap::findLink(linksRemoved_, from, to));
updateSlider = true;
}
@@ -2310,19 +2312,19 @@ void DatabaseViewer::resetConstraint()
}
std::multimap<int, Link>::iterator iter = util3d::findLink(linksRefined_, from, to);
std::multimap<int, Link>::iterator iter = rtabmap::findLink(linksRefined_, from, to);
if(iter != linksRefined_.end())
{
linksRefined_.erase(iter);
this->updateGraphView();
}
iter = util3d::findLink(links_, from, to);
iter = rtabmap::findLink(links_, from, to);
if(iter != links_.end())
{
this->updateConstraintView(iter->second);
}
iter = util3d::findLink(linksAdded_, from, to);
iter = rtabmap::findLink(linksAdded_, from, to);
if(iter != linksAdded_.end())
{
this->updateConstraintView(iter->second);
@@ -2350,7 +2352,7 @@ void DatabaseViewer::rejectConstraint()
// find the original one
std::multimap<int, Link>::iterator iter;
iter = util3d::findLink(links_, from, to);
iter = rtabmap::findLink(links_, from, to);
if(iter != links_.end())
{
if(iter->second.type() == Link::kNeighbor)
@@ -2363,13 +2365,13 @@ void DatabaseViewer::rejectConstraint()
}
// remove from refined and added
iter = util3d::findLink(linksRefined_, from, to);
iter = rtabmap::findLink(linksRefined_, from, to);
if(iter != linksRefined_.end())
{
linksRefined_.erase(iter);
removed = true;
}
iter = util3d::findLink(linksAdded_, from, to);
iter = rtabmap::findLink(linksAdded_, from, to);
if(iter != linksAdded_.end())
{
linksAdded_.erase(iter);
@@ -2392,7 +2394,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
{
std::multimap<int, rtabmap::Link>::iterator findIter;
findIter = util3d::findLink(linksRemoved_, iter->second.from(), iter->second.to());
findIter = rtabmap::findLink(linksRemoved_, iter->second.from(), iter->second.to());
if(findIter != linksRemoved_.end())
{
if(!(iter->second.from() == findIter->second.from() &&
@@ -2410,7 +2412,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
}
}
findIter = util3d::findLink(linksRefined_, iter->second.from(), iter->second.to());
findIter = rtabmap::findLink(linksRefined_, iter->second.from(), iter->second.to());
if(findIter!=linksRefined_.end())
{
if(iter->second.from() == findIter->second.from() &&