DbViewer: coloring graph based on env sensor value (e.g., wifi signal strength) (#1613)

2026-09-09 13:00:19 +08:00 · 2025-11-16 11:37:47 -08:00
parent d1b5d62d21
commit 82fb7ff5d2
5 changed files with 542 additions and 358 deletions
@@ -220,6 +220,7 @@ private:
 	std::map<int, int> mapIds_;
 	std::map<int, int> weights_;
 	std::map<int, std::vector<int> > wmStates_;
+	std::map<int, EnvSensors> envSensors_;
 	QMap<int, int> idToIndex_;
 	QList<rtabmap::Link> neighborLinks_;
 	QList<rtabmap::Link> loopLinks_;
@@ -77,6 +77,7 @@ public:
 	// Use updateNodeColorByValue() instead with valueName="Posterior".
 	RTABMAP_DEPRECATED void updatePosterior(const std::map<int, float> & posterior, float fixedMax = 0.0f, int zValueOffset = 0);
 	void updateNodeColorByValue(const std::string & valueName, const std::map<int, float> & values, float fixedMax = 0.0f, bool invertedColorScale = false, int zValueOffset = 0);
+	void updateNodeColorByValue(const std::string & valueName, const std::map<int, float> & values, float fixedMin, float fixedMax, bool invertedColorScale = false, unsigned short hueMin=0, unsigned short hueMax=180, int zValueOffset = 0);
 	void updateLocalPath(const std::vector<int> & localPath);
 	void setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath);
 	void setCurrentGoalID(int id, const Transform & pose = Transform());
@@ -454,6 +454,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->checkBox_alignScansCloudsWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
+	connect(ui_->comboBox_env_sensor_graph_colormap, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(updateStatistics()));
 	// Graph view
@@ -1129,6 +1130,7 @@ bool DatabaseViewer::closeDatabase()
 		lastWmIds_.clear();
 		mapIds_.clear();
 		weights_.clear();
+		envSensors_.clear();
 		wmStates_.clear();
 		links_.clear();
 		linksAdded_.clear();
@@ -1806,6 +1808,7 @@ void DatabaseViewer::updateIds()
 	idToIndex_.clear();
 	mapIds_.clear();
 	weights_.clear();
+	envSensors_.clear();
 	wmStates_.clear();
 	odomPoses_.clear();
 	groundTruthPoses_.clear();
@@ -1912,6 +1915,7 @@ void DatabaseViewer::updateIds()
 		dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s, g, v, gps, sensors);
 		mapIds_.insert(std::make_pair(ids_[i], mapId));
 		weights_.insert(std::make_pair(ids_[i], w));
+		envSensors_.insert(std::make_pair(ids_[i], sensors));
 		if(w>=0)
 		{
 			for(std::multimap<int, Link>::iterator iter=links.find(ids_[i]); iter!=links.end() && iter->first==ids_[i]; ++iter)
@@ -7242,7 +7246,46 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 		ui_->graphViewer->updateGTGraph(groundTruthPoses_);
 		ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_);
 		ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_, weights_);
-		if(ui_->checkBox_wmState->isEnabled() &&
+		if(ui_->comboBox_env_sensor_graph_colormap->currentIndex() != 0)
+		{
+			std::map<int, float> colors;
+			EnvSensor::Type curentType = (EnvSensor::Type)ui_->comboBox_env_sensor_graph_colormap->currentIndex();
+			for(std::map<int, rtabmap::Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
+			{
+				auto jter = envSensors_.find(iter->first);
+				if(jter != envSensors_.end() && jter->second.find(curentType) != jter->second.end())
+				{
+					colors.insert(std::make_pair(iter->first, jter->second.at(curentType).value()));
+				}
+			}
+			std::string legend;
+			bool invertedColor = false;
+			unsigned char hueMax = 240; // blue
+			switch(curentType)
+			{
+				case EnvSensor::kWifiSignalStrength:
+					legend = "Wifi Signal Strength (dBm)";
+					invertedColor = true;
+					hueMax = 120; // green
+					break;
+				case EnvSensor::kAmbientTemperature:
+					legend = "Ambient Temperature (Celcius)";
+					break;
+				case EnvSensor::kAmbientAirPressure:
+					legend = "Ambient Air Pressure (hPa)";
+					break;
+				case EnvSensor::kAmbientLight:
+					legend = "Ambient Light / Illuminance (lx)";
+					break;
+				case EnvSensor::kAmbientRelativeHumidity:
+					legend = "Ambient Relative Humidity (%)";
+					break;
+				default:
+					break;
+			}
+			ui_->graphViewer->updateNodeColorByValue(legend, colors, 0.0f, 0.0f, invertedColor, 0, hueMax, 1);
+		}
+		else if(ui_->checkBox_wmState->isEnabled() &&
 		   ui_->checkBox_wmState->isChecked() &&
 		   !lastWmIds_.empty())
 		{
@@ -1134,6 +1134,66 @@ void GraphViewer::updateNodeColorByValue(const std::string & valueName, const st
 	}
 }

+void GraphViewer::updateNodeColorByValue(
+	const std::string & valueName,
+	const std::map<int, float> & values,
+	float min,
+	float max,
+	bool invertedColorScale,
+	unsigned short hueMin,
+	unsigned short hueMax,
+	int zValueOffset)
+{
+	hueMax = hueMax > 360 ? 360 : hueMax;
+
+	//find min/max
+	if(min >= max)
+	{
+		bool firstValueSet = false;
+		for(std::map<int, float>::const_iterator iter = values.begin(); iter!=values.end(); ++iter)
+		{
+			if(iter->first > 0)
+			{
+				if(!firstValueSet) {
+					min = max = iter->second;
+					firstValueSet = true;
+				}
+				else if(iter->second>max)
+				{
+					max = iter->second;
+				}
+				else if(iter->second<min)
+				{
+					min = iter->second;
+				}
+			}
+		}
+	}
+	if(min < max && hueMin < hueMax)
+	{
+		float range = max - min;
+		float hueRange = float(hueMax - hueMin);
+		for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
+		{
+			std::map<int,float>::const_iterator jter = values.find(iter.key());
+			if(jter != values.end())
+			{
+				float v = jter->second;
+				v = std::min(v, max);
+				v = std::max(v, min);
+				iter.value()->setColor(QColor::fromHsvF(( invertedColorScale ? (v-min)/range : 1-(v-min)/range )*hueRange/360.0f + hueMin/360.0f, 1, 1, 1), valueName.c_str(), jter->second);
+				iter.value()->setZValue(iter.value()->zValue()+zValueOffset);
+			}
+		}
+	}
+	else if(min < max) {
+		UWARN("min (%f) is not less than max (%f), cannot change color of the graph.", min, max);
+	}
+	else if(hueMin < hueMax) {
+		UWARN("Hue min (%d) is not less than hue max (%d), cannot change color of the graph. The hue values should be set between 0 (red) and 360(pink).", (int)hueMin, (int)hueMax);
+	}
+}
+
 void GraphViewer::setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath)
 {
 	UDEBUG("Set global path size=%d", (int)globalPath.size());
@@ -17,7 +17,7 @@
   <enum>Qt::LeftToRight</enum>
  </property>
  <widget class="QWidget" name="centralwidget">
-   <layout class="QVBoxLayout" name="verticalLayout_6" stretch="1,0,0">
+   <layout class="QVBoxLayout" name="verticalLayout_15" stretch="1,0,0">
    <property name="spacing">
     <number>0</number>
    </property>
@@ -44,7 +44,7 @@
     </layout>
    </item>
    <item>
-     <layout class="QGridLayout" name="gridLayout_11" rowstretch="1,0">
+     <layout class="QGridLayout" name="gridLayout_11">
      <property name="spacing">
       <number>0</number>
      </property>
@@ -776,71 +776,90 @@
       </layout>
      </item>
      <item row="1" column="1">
-       <widget class="QWidget" name="widget_imageControls_B">
-        <layout class="QHBoxLayout" name="horizontalLayout_2">
-          <property name="leftMargin">
-          <number>12</number>
-          </property>
-          <property name="topMargin">
-          <number>12</number>
-          </property>
-          <property name="rightMargin">
-          <number>12</number>
-          </property>
-          <property name="bottomMargin">
-          <number>12</number>
-          </property>
-          <item>
-          <layout class="QVBoxLayout" name="verticalLayout">
-            <item>
-            <widget class="QLabel" name="label_5">
-              <property name="text">
-              <string>Index :</string>
-              </property>
+       <widget class="QWidget" name="widget_imageControls_B" native="true">
+        <layout class="QVBoxLayout" name="verticalLayout_6">
+         <property name="spacing">
+          <number>0</number>
+         </property>
+         <property name="leftMargin">
+          <number>0</number>
+         </property>
+         <property name="topMargin">
+          <number>0</number>
+         </property>
+         <property name="rightMargin">
+          <number>0</number>
+         </property>
+         <property name="bottomMargin">
+          <number>0</number>
+         </property>
+         <item>
+          <layout class="QHBoxLayout" name="horizontalLayout_2">
+           <property name="leftMargin">
+            <number>12</number>
+           </property>
+           <property name="topMargin">
+            <number>12</number>
+           </property>
+           <property name="rightMargin">
+            <number>12</number>
+           </property>
+           <property name="bottomMargin">
+            <number>12</number>
+           </property>
+           <item>
+            <layout class="QVBoxLayout" name="verticalLayout">
+             <item>
+              <widget class="QLabel" name="label_5">
+               <property name="text">
+                <string>Index :</string>
+               </property>
+              </widget>
+             </item>
+             <item>
+              <widget class="QLabel" name="label_4">
+               <property name="text">
+                <string>Id :</string>
+               </property>
+              </widget>
+             </item>
+            </layout>
+           </item>
+           <item>
+            <layout class="QVBoxLayout" name="verticalLayout_2">
+             <item>
+              <widget class="QSpinBox" name="spinBox_indexB">
+               <property name="frame">
+                <bool>false</bool>
+               </property>
+              </widget>
+             </item>
+             <item>
+              <widget class="QLabel" name="label_idB">
+               <property name="text">
+                <string>idB</string>
+               </property>
+              </widget>
+             </item>
+            </layout>
+           </item>
+           <item>
+            <widget class="QSlider" name="horizontalSlider_B">
+             <property name="focusPolicy">
+              <enum>Qt::ClickFocus</enum>
+             </property>
+             <property name="orientation">
+              <enum>Qt::Horizontal</enum>
+             </property>
+             <property name="tickPosition">
+              <enum>QSlider::TicksAbove</enum>
+             </property>
            </widget>
-            </item>
-            <item>
-            <widget class="QLabel" name="label_4">
-              <property name="text">
-              <string>Id :</string>
-              </property>
-            </widget>
-            </item>
+           </item>
          </layout>
-          </item>
-          <item>
-          <layout class="QVBoxLayout" name="verticalLayout_2">
-            <item>
-            <widget class="QSpinBox" name="spinBox_indexB">
-              <property name="frame">
-              <bool>false</bool>
-              </property>
-            </widget>
-            </item>
-            <item>
-            <widget class="QLabel" name="label_idB">
-              <property name="text">
-              <string>idB</string>
-              </property>
-            </widget>
-            </item>
-          </layout>
-          </item>
-          <item>
-          <widget class="QSlider" name="horizontalSlider_B">
-            <property name="focusPolicy">
-            <enum>Qt::ClickFocus</enum>
-            </property>
-            <property name="orientation">
-            <enum>Qt::Horizontal</enum>
-            </property>
-            <property name="tickPosition">
-            <enum>QSlider::TicksAbove</enum>
-            </property>
-          </widget>
-          </item>
+         </item>
        </layout>
-       </widget>       
+       </widget>
      </item>
     </layout>
    </item>
@@ -1252,322 +1271,382 @@
      <widget class="rtabmap::GraphViewer" name="graphViewer"/>
     </item>
     <item>
-      <widget class="QWidget" name="widget_graphControl">
-       <layout class="QVBoxLayout" name="verticalLayout_91">
+      <widget class="QWidget" name="widget_graphControl" native="true">
+       <layout class="QVBoxLayout" name="verticalLayout_61">
+        <property name="spacing">
+         <number>0</number>
+        </property>
+        <property name="leftMargin">
+         <number>0</number>
+        </property>
+        <property name="topMargin">
+         <number>0</number>
+        </property>
+        <property name="rightMargin">
+         <number>0</number>
+        </property>
+        <property name="bottomMargin">
+         <number>0</number>
+        </property>
        <item>
-          <layout class="QHBoxLayout" name="horizontalLayout_15">
+         <layout class="QVBoxLayout" name="verticalLayout_91">
          <item>
-            <widget class="QLabel" name="label_rotation">
-            <property name="text">
-              <string>0.0 deg</string>
-            </property>
-            </widget>
-          </item>
-          <item>
-            <widget class="QSlider" name="horizontalSlider_rotation">
-            <property name="focusPolicy">
-              <enum>Qt::ClickFocus</enum>
-            </property>
-            <property name="minimum">
-              <number>-1799</number>
-            </property>
-            <property name="maximum">
-              <number>1800</number>
-            </property>
-            <property name="sliderPosition">
-              <number>0</number>
-            </property>
-            <property name="orientation">
-              <enum>Qt::Horizontal</enum>
-            </property>
-            <property name="tickPosition">
-              <enum>QSlider::TicksAbove</enum>
-            </property>
-            <property name="tickInterval">
-              <number>100</number>
-            </property>
-            </widget>
-          </item>
-          <item>
-            <widget class="QPushButton" name="pushButton_applyRotation">
-            <property name="toolTip">
-              <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;The rotation will be applied temporary to optimized global graph. To save it to database, do File-&amp;gt;&amp;quot;Regenerate optimized 2D map...&amp;quot;.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-            </property>
-            <property name="text">
-              <string>Apply Rotation</string>
-            </property>
-            </widget>
-          </item>
-          </layout>
-        </item>
-        <item>
-          <layout class="QHBoxLayout" name="horizontalLayout_4">
-          <item>
-            <widget class="QLabel" name="label_iterations">
-            <property name="text">
-              <string>#</string>
-            </property>
-            </widget>
-          </item>
-          <item>
-            <widget class="QSlider" name="horizontalSlider_iterations">
-            <property name="focusPolicy">
-              <enum>Qt::ClickFocus</enum>
-            </property>
-            <property name="orientation">
-              <enum>Qt::Horizontal</enum>
-            </property>
-            <property name="tickPosition">
-              <enum>QSlider::TicksAbove</enum>
-            </property>
-            </widget>
-          </item>
-          <item>
-            <widget class="QComboBox" name="comboBox_optimizationFlavor">
-            <property name="sizeAdjustPolicy">
-              <enum>QComboBox::AdjustToContents</enum>
-            </property>
+           <layout class="QHBoxLayout" name="horizontalLayout_15">
            <item>
+             <widget class="QLabel" name="label_rotation">
              <property name="text">
-              <string>Global Iterative</string>
+               <string>0.0 deg</string>
              </property>
+             </widget>
            </item>
            <item>
-              <property name="text">
-              <string>Global Full</string>
+             <widget class="QSlider" name="horizontalSlider_rotation">
+              <property name="focusPolicy">
+               <enum>Qt::ClickFocus</enum>
              </property>
+              <property name="minimum">
+               <number>-1799</number>
+              </property>
+              <property name="maximum">
+               <number>1800</number>
+              </property>
+              <property name="sliderPosition">
+               <number>0</number>
+              </property>
+              <property name="orientation">
+               <enum>Qt::Horizontal</enum>
+              </property>
+              <property name="tickPosition">
+               <enum>QSlider::TicksAbove</enum>
+              </property>
+              <property name="tickInterval">
+               <number>100</number>
+              </property>
+             </widget>
            </item>
            <item>
-              <property name="text">
-              <string>Local Optimized</string>
+             <widget class="QPushButton" name="pushButton_applyRotation">
+              <property name="toolTip">
+               <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;The rotation will be applied temporary to optimized global graph. To save it to database, do File-&amp;gt;&amp;quot;Regenerate optimized 2D map...&amp;quot;.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
              </property>
+              <property name="text">
+               <string>Apply Rotation</string>
+              </property>
+             </widget>
            </item>
-            </widget>
+           </layout>
          </item>
-          </layout>
-        </item>
-        <item>
-          <layout class="QGridLayout" name="gridLayout_5" columnstretch="0,1">
-          <item row="2" column="1">
-            <widget class="QLabel" name="label_alignPosesWithGroundTruth">
-            <property name="text">
-              <string>Align poses with ground truth</string>
-            </property>
-            <property name="wordWrap">
-              <bool>true</bool>
-            </property>
-            </widget>
-          </item>
-          <item row="4" column="0">
-            <widget class="QCheckBox" name="checkBox_ignoreIntermediateNodes">
-            <property name="text">
-              <string/>
-            </property>
-            <property name="checked">
-              <bool>false</bool>
-            </property>
-            </widget>
-          </item>
-          <item row="0" column="1">
-            <layout class="QHBoxLayout" name="horizontalLayout_7" stretch="0,0,1">
+          <item>
+           <layout class="QHBoxLayout" name="horizontalLayout_4">
            <item>
-              <widget class="QLabel" name="label_optimizeFrom">
+             <widget class="QLabel" name="label_iterations">
              <property name="text">
-                <string>Root</string>
+               <string>#</string>
              </property>
-              </widget>
+             </widget>
            </item>
            <item>
-              <widget class="QCheckBox" name="checkBox_spanAllMaps">
+             <widget class="QSlider" name="horizontalSlider_iterations">
+              <property name="focusPolicy">
+               <enum>Qt::ClickFocus</enum>
+              </property>
+              <property name="orientation">
+               <enum>Qt::Horizontal</enum>
+              </property>
+              <property name="tickPosition">
+               <enum>QSlider::TicksAbove</enum>
+              </property>
+             </widget>
+            </item>
+            <item>
+             <widget class="QComboBox" name="comboBox_optimizationFlavor">
+              <property name="sizeAdjustPolicy">
+               <enum>QComboBox::AdjustToContents</enum>
+              </property>
+              <item>
+               <property name="text">
+                <string>Global Iterative</string>
+               </property>
+              </item>
+              <item>
+               <property name="text">
+                <string>Global Full</string>
+               </property>
+              </item>
+              <item>
+               <property name="text">
+                <string>Local Optimized</string>
+               </property>
+              </item>
+             </widget>
+            </item>
+           </layout>
+          </item>
+          <item>
+           <layout class="QGridLayout" name="gridLayout_5" columnstretch="0,1">
+            <item row="1" column="1">
+             <widget class="QLabel" name="label_alignPosesWithGPS">
              <property name="text">
-                <string>Span to all maps</string>
+               <string>Align poses with GPS</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="8" column="1">
+             <widget class="QLabel" name="label_45">
+              <property name="text">
+               <string>Time grid (s)</string>
+              </property>
+             </widget>
+            </item>
+            <item row="7" column="1">
+             <widget class="QLabel" name="label_39">
+              <property name="text">
+               <string>Time optimization (s)</string>
+              </property>
+             </widget>
+            </item>
+            <item row="7" column="0">
+             <widget class="QLabel" name="label_timeOptimization">
+              <property name="text">
+               <string/>
+              </property>
+              <property name="textInteractionFlags">
+               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="0">
+             <widget class="QCheckBox" name="checkBox_alignPosesWithGPS">
+              <property name="text">
+               <string/>
              </property>
              <property name="checked">
-                <bool>true</bool>
+               <bool>true</bool>
              </property>
-              </widget>
+             </widget>
            </item>
-            <item>
-              <widget class="QCheckBox" name="checkBox_wmState">
+            <item row="3" column="0">
+             <widget class="QCheckBox" name="checkBox_alignScansCloudsWithGroundTruth">
              <property name="text">
-                <string>WM</string>
+               <string/>
              </property>
-              </widget>
+              <property name="checked">
+               <bool>true</bool>
+              </property>
+             </widget>
            </item>
-            </layout>
+            <item row="9" column="0">
+             <widget class="QLabel" name="label_rmse">
+              <property name="text">
+               <string/>
+              </property>
+              <property name="textInteractionFlags">
+               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="1">
+             <widget class="QLabel" name="label_alignPosesWithGroundTruth">
+              <property name="text">
+               <string>Align poses with ground truth</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="9" column="1">
+             <widget class="QLabel" name="label_rmse_title">
+              <property name="text">
+               <string>RMSE (m)</string>
+              </property>
+             </widget>
+            </item>
+            <item row="0" column="1">
+             <layout class="QHBoxLayout" name="horizontalLayout_7" stretch="0,0,1">
+              <item>
+               <widget class="QLabel" name="label_optimizeFrom">
+                <property name="text">
+                 <string>Root</string>
+                </property>
+               </widget>
+              </item>
+              <item>
+               <widget class="QCheckBox" name="checkBox_spanAllMaps">
+                <property name="text">
+                 <string>Span to all maps</string>
+                </property>
+                <property name="checked">
+                 <bool>true</bool>
+                </property>
+               </widget>
+              </item>
+              <item>
+               <widget class="QCheckBox" name="checkBox_wmState">
+                <property name="text">
+                 <string>WM</string>
+                </property>
+               </widget>
+              </item>
+             </layout>
+            </item>
+            <item row="0" column="0">
+             <widget class="QSpinBox" name="spinBox_optimizationsFrom"/>
+            </item>
+            <item row="11" column="0">
+             <widget class="QLabel" name="label_loopClosures">
+              <property name="text">
+               <string/>
+              </property>
+              <property name="textInteractionFlags">
+               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+              </property>
+             </widget>
+            </item>
+            <item row="10" column="0">
+             <widget class="QLabel" name="label_poses">
+              <property name="text">
+               <string/>
+              </property>
+              <property name="textInteractionFlags">
+               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+              </property>
+             </widget>
+            </item>
+            <item row="4" column="1">
+             <widget class="QLabel" name="label_ignoreINtermediateNdoes">
+              <property name="text">
+               <string>Ignore intermediate nodes</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="6" column="0">
+             <widget class="QLabel" name="label_pathLength">
+              <property name="text">
+               <string/>
+              </property>
+              <property name="textInteractionFlags">
+               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+              </property>
+             </widget>
+            </item>
+            <item row="10" column="1">
+             <widget class="QLabel" name="label_52">
+              <property name="text">
+               <string>Poses</string>
+              </property>
+             </widget>
+            </item>
+            <item row="6" column="1">
+             <widget class="QLabel" name="label_10">
+              <property name="text">
+               <string>Path length (m)</string>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="1">
+             <widget class="QLabel" name="label_alignScansCloudsWithGroundTruth">
+              <property name="text">
+               <string>Align scans/clouds with ground truth</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="11" column="1">
+             <widget class="QLabel" name="label_41">
+              <property name="toolTip">
+               <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;N: Neighbor&lt;/p&gt;&lt;p&gt;NM: Neighbor Merged&lt;/p&gt;&lt;p&gt;G: Global&lt;/p&gt;&lt;p&gt;LS: Local by Space (Proximity)&lt;/p&gt;&lt;p&gt;LT: Local by Time (Proximity)&lt;/p&gt;&lt;p&gt;U: User&lt;/p&gt;&lt;p&gt;P: Prior&lt;/p&gt;&lt;p&gt;LM: Landmark&lt;/p&gt;&lt;p&gt;GR: Gravity&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+              </property>
+              <property name="text">
+               <string>Links (N, NM, G, LS, LT, U, P, LM, GR)</string>
+              </property>
+             </widget>
+            </item>
+            <item row="8" column="0">
+             <widget class="QLabel" name="label_timeGrid">
+              <property name="text">
+               <string/>
+              </property>
+              <property name="textInteractionFlags">
+               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+              </property>
+             </widget>
+            </item>
+            <item row="4" column="0">
+             <widget class="QCheckBox" name="checkBox_ignoreIntermediateNodes">
+              <property name="text">
+               <string/>
+              </property>
+              <property name="checked">
+               <bool>false</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="0">
+             <widget class="QCheckBox" name="checkBox_alignPosesWithGroundTruth">
+              <property name="text">
+               <string/>
+              </property>
+              <property name="checked">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="5" column="1">
+             <widget class="QLabel" name="label_21">
+              <property name="text">
+               <string>Env Sensor Colormap</string>
+              </property>
+             </widget>
+            </item>
+            <item row="5" column="0">
+             <widget class="QComboBox" name="comboBox_env_sensor_graph_colormap">
+              <item>
+               <property name="text">
+                <string>Disabled</string>
+               </property>
+              </item>
+              <item>
+               <property name="text">
+                <string>Wifi</string>
+               </property>
+              </item>
+              <item>
+               <property name="text">
+                <string>Temperature</string>
+               </property>
+              </item>
+              <item>
+               <property name="text">
+                <string>Air Pressure</string>
+               </property>
+              </item>
+              <item>
+               <property name="text">
+                <string>Light</string>
+               </property>
+              </item>
+              <item>
+               <property name="text">
+                <string>Relative Humidity</string>
+               </property>
+              </item>
+             </widget>
+            </item>
+           </layout>
          </item>
-          <item row="7" column="1">
-            <widget class="QLabel" name="label_45">
-            <property name="text">
-              <string>Time grid (s)</string>
-            </property>
-            </widget>
-          </item>
-          <item row="8" column="1">
-            <widget class="QLabel" name="label_rmse_title">
-            <property name="text">
-              <string>RMSE (m)</string>
-            </property>
-            </widget>
-          </item>
-          <item row="3" column="0">
-            <widget class="QCheckBox" name="checkBox_alignScansCloudsWithGroundTruth">
-            <property name="text">
-              <string/>
-            </property>
-            <property name="checked">
-              <bool>true</bool>
-            </property>
-            </widget>
-          </item>
-          <item row="9" column="0">
-            <widget class="QLabel" name="label_poses">
-            <property name="text">
-              <string/>
-            </property>
-            <property name="textInteractionFlags">
-              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-            </widget>
-          </item>
-          <item row="6" column="0">
-            <widget class="QLabel" name="label_timeOptimization">
-            <property name="text">
-              <string/>
-            </property>
-            <property name="textInteractionFlags">
-              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-            </widget>
-          </item>
-          <item row="3" column="1">
-            <widget class="QLabel" name="label_alignScansCloudsWithGroundTruth">
-            <property name="text">
-              <string>Align scans/clouds with ground truth</string>
-            </property>
-            <property name="wordWrap">
-              <bool>true</bool>
-            </property>
-            </widget>
-          </item>
-          <item row="8" column="0">
-            <widget class="QLabel" name="label_rmse">
-            <property name="text">
-              <string/>
-            </property>
-            <property name="textInteractionFlags">
-              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-            </widget>
-          </item>
-          <item row="7" column="0">
-            <widget class="QLabel" name="label_timeGrid">
-            <property name="text">
-              <string/>
-            </property>
-            <property name="textInteractionFlags">
-              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-            </widget>
-          </item>
-          <item row="10" column="1">
-            <widget class="QLabel" name="label_41">
-            <property name="toolTip">
-              <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;N: Neighbor&lt;/p&gt;&lt;p&gt;NM: Neighbor Merged&lt;/p&gt;&lt;p&gt;G: Global&lt;/p&gt;&lt;p&gt;LS: Local by Space (Proximity)&lt;/p&gt;&lt;p&gt;LT: Local by Time (Proximity)&lt;/p&gt;&lt;p&gt;U: User&lt;/p&gt;&lt;p&gt;P: Prior&lt;/p&gt;&lt;p&gt;LM: Landmark&lt;/p&gt;&lt;p&gt;GR: Gravity&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-            </property>
-            <property name="text">
-              <string>Links (N, NM, G, LS, LT, U, P, LM, GR)</string>
-            </property>
-            </widget>
-          </item>
-          <item row="5" column="1">
-            <widget class="QLabel" name="label_10">
-            <property name="text">
-              <string>Path length (m)</string>
-            </property>
-            </widget>
-          </item>
-          <item row="9" column="1">
-            <widget class="QLabel" name="label_52">
-            <property name="text">
-              <string>Poses</string>
-            </property>
-            </widget>
-          </item>
-          <item row="5" column="0">
-            <widget class="QLabel" name="label_pathLength">
-            <property name="text">
-              <string/>
-            </property>
-            <property name="textInteractionFlags">
-              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-            </widget>
-          </item>
-          <item row="0" column="0">
-            <widget class="QSpinBox" name="spinBox_optimizationsFrom"/>
-          </item>
-          <item row="10" column="0">
-            <widget class="QLabel" name="label_loopClosures">
-            <property name="text">
-              <string/>
-            </property>
-            <property name="textInteractionFlags">
-              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-            </property>
-            </widget>
-          </item>
-          <item row="4" column="1">
-            <widget class="QLabel" name="label_ignoreINtermediateNdoes">
-            <property name="text">
-              <string>Ignore intermediate nodes</string>
-            </property>
-            <property name="wordWrap">
-              <bool>true</bool>
-            </property>
-            </widget>
-          </item>
-          <item row="6" column="1">
-            <widget class="QLabel" name="label_39">
-            <property name="text">
-              <string>Time optimization (s)</string>
-            </property>
-            </widget>
-          </item>
-          <item row="2" column="0">
-            <widget class="QCheckBox" name="checkBox_alignPosesWithGroundTruth">
-            <property name="text">
-              <string/>
-            </property>
-            <property name="checked">
-              <bool>true</bool>
-            </property>
-            </widget>
-          </item>
-          <item row="1" column="1">
-            <widget class="QLabel" name="label_alignPosesWithGPS">
-            <property name="text">
-              <string>Align poses with GPS</string>
-            </property>
-            <property name="wordWrap">
-              <bool>true</bool>
-            </property>
-            </widget>
-          </item>
-          <item row="1" column="0">
-            <widget class="QCheckBox" name="checkBox_alignPosesWithGPS">
-            <property name="text">
-              <string/>
-            </property>
-            <property name="checked">
-              <bool>true</bool>
-            </property>
-            </widget>
-          </item>
-          </layout>
+         </layout>
        </item>
       </layout>
-      </widget>       
+      </widget>
     </item>
    </layout>
   </widget>
@@ -1805,7 +1884,7 @@
        <property name="geometry">
         <rect>
          <x>0</x>
-          <y>-316</y>
+          <y>0</y>
          <width>518</width>
          <height>1071</height>
         </rect>
@@ -1815,7 +1894,7 @@
        </attribute>
        <layout class="QVBoxLayout" name="verticalLayout_16">
         <item>
-          <layout class="QGridLayout" name="gridLayout_9" columnstretch="0,0">
+          <layout class="QGridLayout" name="gridLayout_9" columnstretch="0,1">
           <item row="3" column="1">
            <widget class="QLabel" name="label_octomap_empty_3">
             <property name="text">
@@ -2526,8 +2605,8 @@
         <rect>
          <x>0</x>
          <y>0</y>
-          <width>296</width>
-          <height>272</height>
+          <width>222</width>
+          <height>306</height>
         </rect>
        </property>
        <attribute name="label">
@@ -2700,8 +2779,8 @@
         <rect>
          <x>0</x>
          <y>0</y>
-          <width>428</width>
-          <height>196</height>
+          <width>181</width>
+          <height>485</height>
         </rect>
        </property>
        <attribute name="label">