DatabaseViewer: Added total path length under the Graph view

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1342 f169173b-cf89-36c8-b27e-44dbe73f0c83

guilib/src/DatabaseViewer.cpp

@@ -365,6 +365,8 @@ void DatabaseViewer::updateIds()
 	{
 		ui_->horizontalSlider_loops->setEnabled(false);
 	}
+
+	updateGraphView();
 }
 
 void DatabaseViewer::generateGraph()
@@ -907,9 +909,30 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			}
 			UINFO("Update scans list... done");
 		}
-
+		std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, value);
-		ui_->graphViewer->updateGraph(uValueAt(graphes_, value), links_, scans_);
+		ui_->graphViewer->updateGraph(graph, links_, scans_);
 		ui_->label_iterations->setNum(value);
+
+		//compute total length (neighbor links)
+		float length = 0.0f;
+		for(std::multimap<int, rtabmap::Link>::const_iterator iter=links_.begin(); iter!=links_.end(); ++iter)
+		{
+			std::map<int, rtabmap::Transform>::const_iterator jterA = graph.find(iter->first);
+			std::map<int, rtabmap::Transform>::const_iterator jterB = graph.find(iter->second.to());
+			if(jterA != graph.end() && jterB != graph.end())
+			{
+				const rtabmap::Transform & poseA = jterA->second;
+				const rtabmap::Transform & poseB = jterB->second;
+				if(iter->second.type() == rtabmap::Link::kNeighbor)
+				{
+					Eigen::Vector3f vA, vB;
+					poseA.getTranslation(vA[0], vA[1], vA[2]);
+					poseB.getTranslation(vB[0], vB[1], vB[2]);
+					length += (vB - vA).norm();
+				}
+			}
+		}
+		ui_->label_pathLength->setNum(length);
 	}
 }
 void DatabaseViewer::updateGraphView()

guilib/src/ui/DatabaseViewer.ui

@@ -442,6 +442,20 @@
          </property>
         </widget>
        </item>
+       <item row="3" column="0">
+        <widget class="QLabel" name="label_10">
+         <property name="text">
+          <string>Total path length (m):</string>
+         </property>
+        </widget>
+       </item>
+       <item row="3" column="1">
+        <widget class="QLabel" name="label_pathLength">
+         <property name="text">
+          <string/>
+         </property>
+        </widget>
+       </item>
       </layout>
      </item>
     </layout>

guilib/src/ui/mainWindow.ui

@@ -27,7 +27,7 @@
      <x>0</x>
      <y>0</y>
      <width>947</width>
-     <height>22</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -315,7 +315,7 @@
        <item row="5" column="0">
         <widget class="QLabel" name="label_20">
          <property name="text">
-          <string>Current image id</string>
+          <string>Current image id [Map id]</string>
          </property>
         </widget>
        </item>