i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +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( void updateCameraTargetPosition(
const Transform & pose); const Transform & pose);
void addOrUpdateCoordinate(
const std::string & id,
const Transform & transform,
double scale);
void removeCoordinate(const std::string & id);
void removeAllCoordinates();
void addOrUpdateGraph( void addOrUpdateGraph(
const std::string & id, const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & graph, const pcl::PointCloud<pcl::PointXYZ>::Ptr & graph,
@@ -226,6 +233,7 @@ private:
QAction * _aSetBackgroundColor; QAction * _aSetBackgroundColor;
QMenu * _menu; QMenu * _menu;
std::set<std::string> _graphes; std::set<std::string> _graphes;
std::set<std::string> _coordinates;
pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory; pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
unsigned int _maxTrajectorySize; unsigned int _maxTrajectorySize;
unsigned int _gridCellCount; unsigned int _gridCellCount;

67
guilib/src/CloudViewer.cpp
View File

@@ -134,7 +134,7 @@ CloudViewer::CloudViewer(QWidget *parent) :
0, 0, 1); 0, 0, 1);
#ifndef _WIN32 #ifndef _WIN32
// Crash on startup on Windows (vtk issue) // Crash on startup on Windows (vtk issue)
_visualizer->addCoordinateSystem(0.2, 0, 0, 0, 0); this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
#endif #endif
//setup menu/actions //setup menu/actions
@@ -580,6 +580,61 @@ void CloudViewer::removeOccupancyGridMap()
#endif #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( void CloudViewer::addOrUpdateGraph(
const std::string & id, const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & graph, 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]; cameras.front().view[2] = _aLockViewZ->isChecked()?1:Fp[10];
} }
#if PCL_VERSION_COMPARE(>=, 1, 7, 2) this->addOrUpdateCoordinate("reference", pose, 0.2);
_visualizer->removeCoordinateSystem("reference", 0);
_visualizer->addCoordinateSystem(0.2, m, "reference", 0);
#else
_visualizer->removeCoordinateSystem(0);
_visualizer->addCoordinateSystem(0.2, m, 0);
#endif
_visualizer->setCameraPosition( _visualizer->setCameraPosition(
cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2], cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[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()) if(color.isValid())
{ {
this->setDefaultBackgroundColor(color); this->setDefaultBackgroundColor(color);
this->update();
} }
} }
else if(a == _aLockViewZ) else if(a == _aLockViewZ)

87
guilib/src/DatabaseViewer.cpp
View File

@@ -1685,7 +1685,7 @@ void DatabaseViewer::update(int value,
updateConstraintButtons(); updateConstraintButtons();
updateWordsMatching(); updateWordsMatching();
if(updateConstraintView) if(updateConstraintView && ui_->dockWidget_constraints->isVisible())
{ {
// update constraint view // update constraint view
int from = ids_.at(ui_->horizontalSlider_A->value()); 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->blockSignals(true);
ui_->horizontalSlider_loops->setValue(i); ui_->horizontalSlider_loops->setValue(i);
ui_->horizontalSlider_loops->blockSignals(false); 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->blockSignals(true);
ui_->horizontalSlider_neighbors->setValue(0); ui_->horizontalSlider_neighbors->setValue(0);
@@ -1722,7 +1722,7 @@ void DatabaseViewer::update(int value,
ui_->horizontalSlider_neighbors->blockSignals(true); ui_->horizontalSlider_neighbors->blockSignals(true);
ui_->horizontalSlider_neighbors->setValue(i); ui_->horizontalSlider_neighbors->setValue(i);
ui_->horizontalSlider_neighbors->blockSignals(false); 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->blockSignals(true);
ui_->horizontalSlider_loops->setValue(0); ui_->horizontalSlider_loops->setValue(0);
@@ -1738,10 +1738,30 @@ void DatabaseViewer::update(int value,
ui_->horizontalSlider_neighbors->blockSignals(true); ui_->horizontalSlider_neighbors->blockSignals(true);
ui_->horizontalSlider_loops->setValue(0); ui_->horizontalSlider_loops->setValue(0);
ui_->horizontalSlider_neighbors->setValue(0); ui_->horizontalSlider_neighbors->setValue(0);
ui_->constraintsViewer->removeAllClouds();
ui_->constraintsViewer->update();
ui_->horizontalSlider_loops->blockSignals(false); ui_->horizontalSlider_loops->blockSignals(false);
ui_->horizontalSlider_neighbors->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) if(cloudFrom->size() == 0 && cloudTo->size() == 0)
{ {
//cloud 3d //cloud 3d
if(!ui_->checkBox_show3DWords->isChecked()) if(ui_->checkBox_show3Dclouds->isChecked())
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo;
cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1); cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1);
@@ -2186,15 +2206,15 @@ void DatabaseViewer::updateConstraintView(
if(cloudFrom->size()) if(cloudFrom->size())
{ {
ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red); ui_->constraintsViewer->addOrUpdateCloud("words0", cloudFrom, Transform::getIdentity(), Qt::red);
} }
if(cloudTo->size()) if(cloudTo->size())
{ {
cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t); 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 * sFrom = memory_->getSignature(link.from());
const Signature * sTo = memory_->getSignature(link.to()); const Signature * sTo = memory_->getSignature(link.to());
@@ -2273,26 +2293,29 @@ void DatabaseViewer::updateConstraintView(
if(scanFrom->size() == 0 && scanTo->size() == 0) if(scanFrom->size() == 0 && scanTo->size() == 0)
{ {
//cloud 2d if(ui_->checkBox_show2DScans->isChecked())
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); //cloud 2d
} pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
else scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw());
{ scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw());
ui_->constraintsViewer->removeCloud("scan0"); scanB = rtabmap::util3d::transformPointCloud(scanB, t);
} if(scanA->size())
if(scanB->size()) {
{ ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA, Transform::getIdentity(), Qt::yellow);
ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB, Transform::getIdentity(), Qt::magenta); }
} else
else {
{ ui_->constraintsViewer->removeCloud("scan0");
ui_->constraintsViewer->removeCloud("scan1"); }
if(scanB->size())
{
ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB, Transform::getIdentity(), Qt::magenta);
}
else
{
ui_->constraintsViewer->removeCloud("scan1");
}
} }
} }
else else
@@ -2315,8 +2338,12 @@ void DatabaseViewer::updateConstraintView(
} }
} }
//update cordinate //update coordinate
ui_->constraintsViewer->updateCameraTargetPosition(t);
ui_->constraintsViewer->addOrUpdateCoordinate("from_coordinate", Transform::getIdentity(), 0.2);
ui_->constraintsViewer->addOrUpdateCoordinate("to_coordinate", t, 0.2);
ui_->constraintsViewer->clearTrajectory(); ui_->constraintsViewer->clearTrajectory();
ui_->constraintsViewer->update(); ui_->constraintsViewer->update();

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

@@ -50,8 +50,8 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>166</width> <width>154</width>
<height>173</height> <height>184</height>
</rect> </rect>
</property> </property>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1"> <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -236,8 +236,8 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>165</width> <width>154</width>
<height>173</height> <height>184</height>
</rect> </rect>
</property> </property>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1"> <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -418,7 +418,7 @@
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>1285</width> <width>1285</width>
<height>25</height> <height>22</height>
</rect> </rect>
</property> </property>
<widget class="QMenu" name="menuFile"> <widget class="QMenu" name="menuFile">
@@ -470,12 +470,56 @@
<number>2</number> <number>2</number>
</attribute> </attribute>
<widget class="QWidget" name="dockWidgetContents"> <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> <item>
<widget class="rtabmap::CloudViewer" name="constraintsViewer" native="true"/> <widget class="rtabmap::CloudViewer" name="constraintsViewer" native="true"/>
</item> </item>
<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"> <item row="0" column="0">
<widget class="QLabel" name="label"> <widget class="QLabel" name="label">
<property name="text"> <property name="text">
@@ -572,20 +616,6 @@
</property> </property>
</widget> </widget>
</item> </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> </layout>
</item> </item>
<item> <item>
@@ -805,8 +835,8 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>314</width> <width>312</width>
<height>303</height> <height>314</height>
</rect> </rect>
</property> </property>
<attribute name="label"> <attribute name="label">
@@ -1025,8 +1055,8 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>351</width> <width>366</width>
<height>347</height> <height>361</height>
</rect> </rect>
</property> </property>
<attribute name="label"> <attribute name="label">
@@ -1315,8 +1345,8 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>333</width> <width>330</width>
<height>306</height> <height>304</height>
</rect> </rect>
</property> </property>
<attribute name="label"> <attribute name="label">
@@ -1532,8 +1562,8 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>243</width> <width>248</width>
<height>284</height> <height>319</height>
</rect> </rect>
</property> </property>
<attribute name="label"> <attribute name="label">
@@ -1727,7 +1757,7 @@
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>201</width> <width>201</width>
<height>117</height> <height>126</height>
</rect> </rect>
</property> </property>
<attribute name="label"> <attribute name="label">
@@ -1826,8 +1856,8 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>285</width> <width>283</width>
<height>309</height> <height>322</height>
</rect> </rect>
</property> </property>
<attribute name="label"> <attribute name="label">