DbViewer: Fixed octomap visualization when ground is not segmented

2026-09-02 17:40:23 +08:00 · 2017-11-21 13:24:21 -05:00
parent 274e7c579e
commit 1df95702a3
2 changed files with 146 additions and 111 deletions
--- a/guilib/src/DatabaseViewer.cpp
+++ b/guilib/src/DatabaseViewer.cpp
@@ -198,7 +198,10 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	setupMainLayout(ui_->actionVertical_Layout->isChecked());
 	ui_->checkBox_grid_cubes->setVisible(ui_->checkBox_octomap->isChecked());
 	ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
-	ui_->checkBox_grid_empty->setVisible(ui_->checkBox_octomap->isChecked());
+	ui_->checkBox_grid_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
+	ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
+	ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
+	ui_->label_octomap_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());

 	ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
 	ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
@@ -3096,7 +3099,7 @@ void DatabaseViewer::update(int value,
 							localMaps.insert(*generatedLocalMaps_.find(data.id()));
 							localMapsInfo.insert(*generatedLocalMapsInfo_.find(data.id()));
 						}
-						else if(!data.gridGroundCellsRaw().empty() && !data.gridObstacleCellsRaw().empty())
+						else if(!data.gridGroundCellsRaw().empty() || !data.gridObstacleCellsRaw().empty())
 						{
 							localMaps.insert(std::make_pair(data.id(), std::make_pair(data.gridGroundCellsRaw(), data.gridObstacleCellsRaw())));
 							localMapsInfo.insert(std::make_pair(data.id(), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
@@ -3104,13 +3107,14 @@ void DatabaseViewer::update(int value,
 						if(!localMaps.empty())
 						{
 							std::map<int, Transform> poses;
-							poses.insert(std::make_pair(data.id(), Transform::getIdentity()));
+							poses.insert(std::make_pair(data.id(), pose));

 #ifdef RTABMAP_OCTOMAP
 							OctoMap * octomap = 0;
 							if(ui_->checkBox_octomap->isChecked() &&
-								localMaps.begin()->second.first.channels() > 2 &&
-								localMaps.begin()->second.second.channels() > 2 &&
+								(!localMaps.begin()->second.first.empty() || !localMaps.begin()->second.second.empty()) &&
+								(localMaps.begin()->second.first.empty() || localMaps.begin()->second.first.channels() > 2) &&
+								(localMaps.begin()->second.second.empty() || localMaps.begin()->second.second.channels() > 2) &&
 								localMapsInfo.begin()->second.first > 0.0f)
 							{
 								//create local octomap
@@ -3159,7 +3163,7 @@ void DatabaseViewer::update(int value,
 									{
 										pcl::IndicesPtr obstacles(new std::vector<int>);
 										pcl::IndicesPtr empty(new std::vector<int>);
-										pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(0, obstacles.get(), empty.get());
+										pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get());
 										pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 										pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
 										cloudViewer_->addCloud("obstacles", obstaclesCloud);
@@ -3175,7 +3179,7 @@ void DatabaseViewer::update(int value,
 									}
 									else
 									{
-										cloudViewer_->addOctomap(octomap);
+										cloudViewer_->addOctomap(octomap, ui_->spinBox_grid_depth->value());
 									}
 								}
 								else
@@ -3184,11 +3188,11 @@ void DatabaseViewer::update(int value,
 									// occupancy cloud
 									cloudViewer_->addCloud("ground",
 											util3d::laserScanToPointCloud(localMaps.begin()->second.first),
-											Transform::getIdentity(),
+											pose,
 											Qt::green);
 									cloudViewer_->addCloud("obstacles",
 											util3d::laserScanToPointCloud(localMaps.begin()->second.second),
-											Transform::getIdentity(),
+											pose,
 											Qt::red);
 									cloudViewer_->setCloudPointSize("ground", 5);
 									cloudViewer_->setCloudPointSize("obstacles", 5);
@@ -4364,7 +4368,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 				}
 				else if(localMaps_.find(ids[i]) != localMaps_.end())
 				{
-					if(!localMaps_.find(ids[i])->second.first.empty() || !localMaps_.find(ids[i])->second.first.empty())
+					if(!localMaps_.find(ids[i])->second.first.empty() || !localMaps_.find(ids[i])->second.second.empty())
 					{
 						localMaps.insert(*localMaps_.find(ids.at(i)));
 						localMapsInfo.insert(*localMapsInfo_.find(ids[i]));
@@ -4812,7 +4816,10 @@ void DatabaseViewer::updateGrid()
 	{
 		ui_->checkBox_grid_cubes->setVisible(ui_->checkBox_octomap->isChecked());
 		ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
-		ui_->checkBox_grid_empty->setVisible(ui_->checkBox_octomap->isChecked());
+		ui_->checkBox_grid_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
+		ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
+		ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
+		ui_->label_octomap_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());

 		update3dView();
 		updateGraphView();
@@ -4822,6 +4829,13 @@ void DatabaseViewer::updateGrid()
 void DatabaseViewer::updateOctomapView()
 {
 #ifdef RTABMAP_OCTOMAP
+		ui_->checkBox_grid_cubes->setVisible(ui_->checkBox_octomap->isChecked());
+		ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
+		ui_->checkBox_grid_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
+		ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
+		ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
+		ui_->label_octomap_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
+
 		if(ui_->checkBox_octomap->isChecked())
 		{
 			if(octomap_)
--- a/guilib/src/ui/DatabaseViewer.ui
+++ b/guilib/src/ui/DatabaseViewer.ui
@@ -52,8 +52,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-           <width>438</width>
-           <height>202</height>
+           <width>420</width>
+           <height>256</height>
          </rect>
         </property>
         <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -244,8 +244,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-           <width>437</width>
-           <height>202</height>
+           <width>420</width>
+           <height>256</height>
          </rect>
         </property>
         <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -552,7 +552,7 @@
     <x>0</x>
     <y>0</y>
     <width>1547</width>
-     <height>21</height>
+     <height>22</height>
    </rect>
   </property>
   <widget class="QMenu" name="menuFile">
@@ -1090,15 +1090,15 @@
     <item>
      <widget class="QToolBox" name="toolBox">
       <property name="currentIndex">
-        <number>0</number>
+        <number>1</number>
       </property>
       <widget class="QWidget" name="page_3">
        <property name="geometry">
         <rect>
          <x>0</x>
          <y>0</y>
-          <width>274</width>
-          <height>192</height>
+          <width>324</width>
+          <height>188</height>
         </rect>
        </property>
        <attribute name="label">
@@ -1233,8 +1233,8 @@
         <rect>
          <x>0</x>
          <y>0</y>
-          <width>206</width>
-          <height>520</height>
+          <width>296</width>
+          <height>732</height>
         </rect>
        </property>
        <attribute name="label">
@@ -1243,6 +1243,55 @@
        <layout class="QVBoxLayout" name="verticalLayout_16">
         <item>
          <layout class="QGridLayout" name="gridLayout_9" columnstretch="0,1">
+           <item row="7" column="1">
+            <widget class="QLabel" name="label_51">
+             <property name="text">
+              <string>Gain compensation radius (Constraints view)</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="7" column="0">
+            <widget class="QDoubleSpinBox" name="doubleSpinBox_gainCompensationRadius">
+             <property name="suffix">
+              <string> m</string>
+             </property>
+             <property name="decimals">
+              <number>2</number>
+             </property>
+             <property name="minimum">
+              <double>0.000000000000000</double>
+             </property>
+             <property name="maximum">
+              <double>10.000000000000000</double>
+             </property>
+             <property name="singleStep">
+              <double>0.010000000000000</double>
+             </property>
+             <property name="value">
+              <double>0.000000000000000</double>
+             </property>
+            </widget>
+           </item>
+           <item row="2" column="1">
+            <widget class="QLabel" name="label_53">
+             <property name="text">
+              <string>OctoMap</string>
+             </property>
+            </widget>
+           </item>
+           <item row="8" column="1">
+            <widget class="QLabel" name="label_54">
+             <property name="text">
+              <string>Voxel size (for clouds and scans)</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
           <item row="1" column="1">
            <widget class="QLabel" name="label_44">
             <property name="text">
@@ -1277,45 +1326,6 @@
             </property>
            </widget>
           </item>
-           <item row="3" column="1">
-            <widget class="QLabel" name="label_51">
-             <property name="text">
-              <string>Gain compensation radius (Constraints view)</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="3" column="0">
-            <widget class="QDoubleSpinBox" name="doubleSpinBox_gainCompensationRadius">
-             <property name="suffix">
-              <string> m</string>
-             </property>
-             <property name="decimals">
-              <number>2</number>
-             </property>
-             <property name="minimum">
-              <double>0.000000000000000</double>
-             </property>
-             <property name="maximum">
-              <double>10.000000000000000</double>
-             </property>
-             <property name="singleStep">
-              <double>0.010000000000000</double>
-             </property>
-             <property name="value">
-              <double>0.000000000000000</double>
-             </property>
-            </widget>
-           </item>
-           <item row="2" column="1">
-            <widget class="QLabel" name="label_53">
-             <property name="text">
-              <string>OctoMap</string>
-             </property>
-            </widget>
-           </item>
           <item row="2" column="0">
            <widget class="QCheckBox" name="checkBox_octomap">
             <property name="text">
@@ -1323,17 +1333,7 @@
             </property>
            </widget>
           </item>
-           <item row="4" column="1">
-            <widget class="QLabel" name="label_54">
-             <property name="text">
-              <string>Voxel size (for clouds and scans)</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
-           <item row="4" column="0">
+           <item row="8" column="0">
            <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize">
             <property name="suffix">
              <string> m</string>
@@ -1355,6 +1355,63 @@
             </property>
            </widget>
           </item>
+           <item row="5" column="0">
+            <widget class="QCheckBox" name="checkBox_grid_cubes">
+             <property name="text">
+              <string/>
+             </property>
+             <property name="checked">
+              <bool>false</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="4" column="0">
+            <widget class="QCheckBox" name="checkBox_grid_empty">
+             <property name="text">
+              <string/>
+             </property>
+             <property name="checked">
+              <bool>false</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="6" column="0">
+            <widget class="QSpinBox" name="spinBox_grid_depth">
+             <property name="prefix">
+              <string/>
+             </property>
+             <property name="minimum">
+              <number>0</number>
+             </property>
+             <property name="maximum">
+              <number>16</number>
+             </property>
+             <property name="value">
+              <number>16</number>
+             </property>
+            </widget>
+           </item>
+           <item row="4" column="1">
+            <widget class="QLabel" name="label_octomap_empty">
+             <property name="text">
+              <string>OctoMap: Empty space</string>
+             </property>
+            </widget>
+           </item>
+           <item row="5" column="1">
+            <widget class="QLabel" name="label_octomap_cubes">
+             <property name="text">
+              <string>OctoMap: Cubes rendering</string>
+             </property>
+            </widget>
+           </item>
+           <item row="6" column="1">
+            <widget class="QLabel" name="label_octomap_depth">
+             <property name="text">
+              <string>OctoMap: Tree depth</string>
+             </property>
+            </widget>
+           </item>
          </layout>
         </item>
         <item>
@@ -1596,8 +1653,8 @@
         <rect>
          <x>0</x>
          <y>0</y>
-          <width>152</width>
-          <height>96</height>
+          <width>201</width>
+          <height>126</height>
         </rect>
        </property>
        <attribute name="label">
@@ -1696,8 +1753,8 @@
         <rect>
          <x>0</x>
          <y>0</y>
-          <width>138</width>
-          <height>381</height>
+          <width>186</width>
+          <height>496</height>
         </rect>
        </property>
        <attribute name="label">
@@ -2128,42 +2185,6 @@
         </property>
        </widget>
       </item>
-       <item>
-        <widget class="QCheckBox" name="checkBox_grid_cubes">
-         <property name="text">
-          <string>Cubes Rendering</string>
-         </property>
-         <property name="checked">
-          <bool>false</bool>
-         </property>
-        </widget>
-       </item>
-       <item>
-        <widget class="QCheckBox" name="checkBox_grid_empty">
-         <property name="text">
-          <string>Empty Space</string>
-         </property>
-         <property name="checked">
-          <bool>false</bool>
-         </property>
-        </widget>
-       </item>
-       <item>
-        <widget class="QSpinBox" name="spinBox_grid_depth">
-         <property name="prefix">
-          <string>Depth </string>
-         </property>
-         <property name="minimum">
-          <number>0</number>
-         </property>
-         <property name="maximum">
-          <number>16</number>
-         </property>
-         <property name="value">
-          <number>16</number>
-         </property>
-        </widget>
-       </item>
       <item>
        <spacer name="horizontalSpacer_5">
         <property name="orientation">