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

CloudViewer: Added addOrUpdateCoordinate() method (working only with PCL >= 1.7.2)

Browse Source
This commit is contained in:
matlabbe
2015-07-24 16:29:45 -04:00
parent b66fcdc070
commit f5dac46252
4 changed files with 186 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
guilib/include/rtabmap/gui/CloudViewer.h
View File

@@ -137,6 +137,13 @@ public:
void updateCameraTargetPosition(
const Transform & pose);
void addOrUpdateCoordinate(
const std::string & id,
const Transform & transform,
double scale);
void removeCoordinate(const std::string & id);
void removeAllCoordinates();
void addOrUpdateGraph(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & graph,
@@ -226,6 +233,7 @@ private:
QAction * _aSetBackgroundColor;
QMenu * _menu;
std::set<std::string> _graphes;
std::set<std::string> _coordinates;
pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
unsigned int _maxTrajectorySize;
unsigned int _gridCellCount;

67
guilib/src/CloudViewer.cpp
View File

@@ -134,7 +134,7 @@ CloudViewer::CloudViewer(QWidget *parent) :
0, 0, 1);
#ifndef _WIN32
// Crash on startup on Windows (vtk issue)
_visualizer->addCoordinateSystem(0.2, 0, 0, 0, 0);
this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
#endif
//setup menu/actions
@@ -580,6 +580,61 @@ void CloudViewer::removeOccupancyGridMap()
#endif
}
void CloudViewer::addOrUpdateCoordinate(
const std::string & id,
const Transform & transform,
double scale)
{
if(id.empty())
{
UERROR("id should not be empty!");
return;
}
removeCoordinate(id);
if(!transform.isNull())
{
_coordinates.insert(id);
#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
_visualizer->addCoordinateSystem(scale, transform.toEigen3f(), id);
#else
// Well, on older versions, just update the main coordinate
_visualizer->addCoordinateSystem(scale, transform.toEigen3f(), 0);
#endif
}
}
void CloudViewer::removeCoordinate(const std::string & id)
{
if(id.empty())
{
UERROR("id should not be empty!");
return;
}
if(_coordinates.find(id) != _coordinates.end())
{
#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
_visualizer->removeCoordinateSystem(id);
#else
// Well, on older versions, just update the main coordinate
_visualizer->removeCoordinateSystem(0);
#endif
_coordinates.erase(id);
}
}
void CloudViewer::removeAllCoordinates()
{
std::set<std::string> coordinates = _coordinates;
for(std::set<std::string>::iterator iter = coordinates.begin(); iter!=coordinates.end(); ++iter)
{
this->removeCoordinate(*iter);
}
UASSERT(_coordinates.empty());
}
void CloudViewer::addOrUpdateGraph(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & graph,
@@ -825,13 +880,8 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
cameras.front().view[2] = _aLockViewZ->isChecked()?1:Fp[10];
}
#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
_visualizer->removeCoordinateSystem("reference", 0);
_visualizer->addCoordinateSystem(0.2, m, "reference", 0);
#else
_visualizer->removeCoordinateSystem(0);
_visualizer->addCoordinateSystem(0.2, m, 0);
#endif
this->addOrUpdateCoordinate("reference", pose, 0.2);
_visualizer->setCameraPosition(
cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
@@ -1355,6 +1405,7 @@ void CloudViewer::handleAction(QAction * a)
if(color.isValid())
{
this->setDefaultBackgroundColor(color);
this->update();
}
}
else if(a == _aLockViewZ)

87
guilib/src/DatabaseViewer.cpp
View File

@@ -1685,7 +1685,7 @@ void DatabaseViewer::update(int value,
updateConstraintButtons();
updateWordsMatching();
if(updateConstraintView)
if(updateConstraintView && ui_->dockWidget_constraints->isVisible())
{
// update constraint view
int from = ids_.at(ui_->horizontalSlider_A->value());
@@ -1703,7 +1703,7 @@ void DatabaseViewer::update(int value,
ui_->horizontalSlider_loops->blockSignals(true);
ui_->horizontalSlider_loops->setValue(i);
ui_->horizontalSlider_loops->blockSignals(false);
this->updateConstraintView(loopLinks_.at(i), false);
this->updateConstraintView(loopLinks_[i].from() == from?loopLinks_.at(i):loopLinks_.at(i).inverse(), false);
}
ui_->horizontalSlider_neighbors->blockSignals(true);
ui_->horizontalSlider_neighbors->setValue(0);
@@ -1722,7 +1722,7 @@ void DatabaseViewer::update(int value,
ui_->horizontalSlider_neighbors->blockSignals(true);
ui_->horizontalSlider_neighbors->setValue(i);
ui_->horizontalSlider_neighbors->blockSignals(false);
this->updateConstraintView(neighborLinks_.at(i), false);
this->updateConstraintView(neighborLinks_[i].from() == from?neighborLinks_.at(i):neighborLinks_.at(i).inverse(), false);
}
ui_->horizontalSlider_loops->blockSignals(true);
ui_->horizontalSlider_loops->setValue(0);
@@ -1738,10 +1738,30 @@ void DatabaseViewer::update(int value,
ui_->horizontalSlider_neighbors->blockSignals(true);
ui_->horizontalSlider_loops->setValue(0);
ui_->horizontalSlider_neighbors->setValue(0);
ui_->constraintsViewer->removeAllClouds();
ui_->constraintsViewer->update();
ui_->horizontalSlider_loops->blockSignals(false);
ui_->horizontalSlider_neighbors->blockSignals(false);
ui_->constraintsViewer->removeAllClouds();
// make a fake link using globally optimized poses
if(graphes_.size())
{
std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(optimizedPoses.size() > 0)
{
std::map<int, Transform>::iterator fromIter = optimizedPoses.find(from);
std::map<int, Transform>::iterator toIter = optimizedPoses.find(to);
if(fromIter != optimizedPoses.end() &&
toIter != optimizedPoses.end())
{
Link link(from, to, Link::kUndef, fromIter->second.inverse() * toIter->second);
this->updateConstraintView(link, false);
}
}
}
ui_->constraintsViewer->update();
}
}
@@ -2178,7 +2198,7 @@ void DatabaseViewer::updateConstraintView(
if(cloudFrom->size() == 0 && cloudTo->size() == 0)
{
//cloud 3d
if(!ui_->checkBox_show3DWords->isChecked())
if(ui_->checkBox_show3Dclouds->isChecked())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo;
cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1);
@@ -2186,15 +2206,15 @@ void DatabaseViewer::updateConstraintView(
if(cloudFrom->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
ui_->constraintsViewer->addOrUpdateCloud("words0", cloudFrom, Transform::getIdentity(), Qt::red);
}
if(cloudTo->size())
{
cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
ui_->constraintsViewer->addOrUpdateCloud("words1", cloudTo, Transform::getIdentity(), Qt::cyan);
}
}
else
if(ui_->checkBox_show3DWords->isChecked())
{
const Signature * sFrom = memory_->getSignature(link.from());
const Signature * sTo = memory_->getSignature(link.to());
@@ -2273,26 +2293,29 @@ void DatabaseViewer::updateConstraintView(
if(scanFrom->size() == 0 && scanTo->size() == 0)
{
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw());
scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw());
scanB = rtabmap::util3d::transformPointCloud(scanB, t);
if(scanA->size())
if(ui_->checkBox_show2DScans->isChecked())
{
ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA, Transform::getIdentity(), Qt::yellow);
}
else
{
ui_->constraintsViewer->removeCloud("scan0");
}
if(scanB->size())
{
ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB, Transform::getIdentity(), Qt::magenta);
}
else
{
ui_->constraintsViewer->removeCloud("scan1");
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw());
scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw());
scanB = rtabmap::util3d::transformPointCloud(scanB, t);
if(scanA->size())
{
ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA, Transform::getIdentity(), Qt::yellow);
}
else
{
ui_->constraintsViewer->removeCloud("scan0");
}
if(scanB->size())
{
ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB, Transform::getIdentity(), Qt::magenta);
}
else
{
ui_->constraintsViewer->removeCloud("scan1");
}
}
}
else
@@ -2315,8 +2338,12 @@ void DatabaseViewer::updateConstraintView(
}
}
//update cordinate
ui_->constraintsViewer->updateCameraTargetPosition(t);
//update coordinate
ui_->constraintsViewer->addOrUpdateCoordinate("from_coordinate", Transform::getIdentity(), 0.2);
ui_->constraintsViewer->addOrUpdateCoordinate("to_coordinate", t, 0.2);
ui_->constraintsViewer->clearTrajectory();
ui_->constraintsViewer->update();

94
guilib/src/ui/DatabaseViewer.ui
View File

@@ -50,8 +50,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>166</width>
<height>173</height>
<width>154</width>
<height>184</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -236,8 +236,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>165</width>
<height>173</height>
<width>154</width>
<height>184</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -418,7 +418,7 @@
<x>0</x>
<y>0</y>
<width>1285</width>
<height>25</height>
<height>22</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -470,12 +470,56 @@
<number>2</number>
</attribute>
<widget class="QWidget" name="dockWidgetContents">
<layout class="QVBoxLayout" name="verticalLayout_8" stretch="1,0,0">
<layout class="QVBoxLayout" name="verticalLayout_8" stretch="1,0,0,0">
<item>
<widget class="rtabmap::CloudViewer" name="constraintsViewer" native="true"/>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_6" columnstretch="0,1">
<layout class="QHBoxLayout" name="horizontalLayout_10">
<item>
<widget class="QCheckBox" name="checkBox_show3DWords">
<property name="text">
<string>Words</string>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBox_show3Dclouds">
<property name="text">
<string>Clouds</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBox_show2DScans">
<property name="text">
<string>Scans</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_6" columnstretch="0,0">
<item row="0" column="0">
<widget class="QLabel" name="label">
<property name="text">
@@ -572,20 +616,6 @@
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="label_32">
<property name="text">
<string>Show 3D words</string>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QCheckBox" name="checkBox_show3DWords">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -805,8 +835,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>314</width>
<height>303</height>
<width>312</width>
<height>314</height>
</rect>
</property>
<attribute name="label">
@@ -1025,8 +1055,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>351</width>
<height>347</height>
<width>366</width>
<height>361</height>
</rect>
</property>
<attribute name="label">
@@ -1315,8 +1345,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>333</width>
<height>306</height>
<width>330</width>
<height>304</height>
</rect>
</property>
<attribute name="label">
@@ -1532,8 +1562,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>243</width>
<height>284</height>
<width>248</width>
<height>319</height>
</rect>
</property>
<attribute name="label">
@@ -1727,7 +1757,7 @@
<x>0</x>
<y>0</y>
<width>201</width>
<height>117</height>
<height>126</height>
</rect>
</property>
<attribute name="label">
@@ -1826,8 +1856,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>285</width>
<height>309</height>
<width>283</width>
<height>322</height>
</rect>
</property>
<attribute name="label">