From 2b5ae3f9a8b8682f05179c3412a7386983737ad6 Mon Sep 17 00:00:00 2001
From: Mathieu Labbe <matlabbe@gmail.com>
Date: Wed, 1 Apr 2015 14:34:43 -0400
Subject: [PATCH] DatabaseViewer: added "Ignore pose correction" option

---
 guilib/src/DatabaseViewer.cpp   | 27 ++++++++++++-
 guilib/src/ui/DatabaseViewer.ui | 71 ++++++++++++++++-----------------
 2 files changed, 61 insertions(+), 37 deletions(-)

diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp
index da8de194..139b7180 100644
--- a/guilib/src/DatabaseViewer.cpp
+++ b/guilib/src/DatabaseViewer.cpp
@@ -192,6 +192,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->spinBox_iterations, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
+	connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
@@ -314,6 +315,7 @@ void DatabaseViewer::readSettings()
 	settings.beginGroup("optimization");
 	ui_->spinBox_iterations->setValue(settings.value("iterations", ui_->spinBox_iterations->value()).toInt());
 	ui_->checkBox_ignoreCovariance->setChecked(settings.value("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked()).toBool());
+	ui_->checkBox_ignorePoseCorrection->setChecked(settings.value("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked()).toBool());
 	ui_->comboBox_graphOptimizer->setCurrentIndex(settings.value("strategy", ui_->comboBox_graphOptimizer->currentIndex()).toInt());
 	ui_->checkBox_2dslam->setChecked(settings.value("slam2d", ui_->checkBox_2dslam->isChecked()).toBool());
 	ui_->spinBox_optimizationDepth->setValue(settings.value("depth", ui_->spinBox_optimizationDepth->value()).toInt());
@@ -387,6 +389,7 @@ void DatabaseViewer::writeSettings()
 	settings.beginGroup("optimization");
 	settings.setValue("iterations", ui_->spinBox_iterations->value());
 	settings.setValue("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked());
+	settings.setValue("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked());
 	settings.setValue("strategy", ui_->comboBox_graphOptimizer->currentIndex());
 	settings.setValue("slam2d", ui_->checkBox_2dslam->isChecked());
 	settings.setValue("depth", ui_->spinBox_optimizationDepth->value());
@@ -2289,7 +2292,29 @@ void DatabaseViewer::updateGraphView()
 		std::map<int, rtabmap::Transform> finalPoses;
 		graphes_.push_back(poses_);
 		ui_->actionGenerate_TORO_graph_graph->setEnabled(true);
-		std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
+		std::multimap<int, rtabmap::Link> links;
+		if(ui_->checkBox_ignorePoseCorrection->isChecked())
+		{
+			std::multimap<int, Link> tmp = links_;
+			for(std::multimap<int, Link>::iterator iter=tmp.begin(); iter!=tmp.end(); ++iter)
+			{
+				if(iter->second.type() == Link::kNeighbor)
+				{
+					Transform poseFrom = uValue(poses_, iter->second.from(), Transform());
+					Transform poseTo = uValue(poses_, iter->second.to(), Transform());
+					if(!poseFrom.isNull() && !poseTo.isNull())
+					{
+						iter->second.setTransform(poseFrom.inverse() * poseTo); // recompute raw odom transformation
+
+					}
+				}
+			}
+			links = updateLinksWithModifications(tmp);
+		}
+		else
+		{
+			links = updateLinksWithModifications(links_);
+		}
 		graph::Optimizer * optimizer = 0;
 		if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeG2O)
 		{
diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui
index 6a06e759..2e82d379 100644
--- a/guilib/src/ui/DatabaseViewer.ui
+++ b/guilib/src/ui/DatabaseViewer.ui
@@ -42,7 +42,7 @@
              <x>0</x>
              <y>0</y>
              <width>153</width>
-             <height>136</height>
+             <height>127</height>
             </rect>
            </property>
            <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -122,16 +122,7 @@
         </item>
         <item>
          <layout class="QHBoxLayout" name="horizontalLayout">
-          <property name="leftMargin">
-           <number>12</number>
-          </property>
-          <property name="topMargin">
-           <number>12</number>
-          </property>
-          <property name="rightMargin">
-           <number>12</number>
-          </property>
-          <property name="bottomMargin">
+          <property name="margin">
            <number>12</number>
           </property>
           <item>
@@ -206,7 +197,7 @@
              <x>0</x>
              <y>0</y>
              <width>152</width>
-             <height>136</height>
+             <height>127</height>
             </rect>
            </property>
            <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -286,16 +277,7 @@
         </item>
         <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">
+          <property name="margin">
            <number>12</number>
           </property>
           <item>
@@ -358,7 +340,7 @@
      <x>0</x>
      <y>0</y>
      <width>1182</width>
-     <height>22</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -1181,6 +1163,22 @@
     <layout class="QVBoxLayout" name="verticalLayout_12">
      <item>
       <layout class="QGridLayout" name="gridLayout_7" columnstretch="0,1">
+       <item row="2" column="0">
+        <widget class="QSpinBox" name="spinBox_optimizationDepth">
+         <property name="suffix">
+          <string/>
+         </property>
+         <property name="minimum">
+          <number>0</number>
+         </property>
+         <property name="maximum">
+          <number>1000</number>
+         </property>
+         <property name="value">
+          <number>0</number>
+         </property>
+        </widget>
+       </item>
        <item row="1" column="1">
         <widget class="QLabel" name="label_7">
          <property name="text">
@@ -1218,7 +1216,7 @@
          </property>
         </widget>
        </item>
-       <item row="4" column="1">
+       <item row="5" column="1">
         <widget class="QLabel" name="label_41">
          <property name="text">
           <string>2d SLAM</string>
@@ -1249,7 +1247,7 @@
          </property>
         </widget>
        </item>
-       <item row="4" column="0">
+       <item row="5" column="0">
         <widget class="QCheckBox" name="checkBox_2dslam">
          <property name="text">
           <string/>
@@ -1266,19 +1264,20 @@
          </property>
         </widget>
        </item>
-       <item row="2" column="0">
-        <widget class="QSpinBox" name="spinBox_optimizationDepth">
-         <property name="suffix">
+       <item row="4" column="1">
+        <widget class="QLabel" name="label_35">
+         <property name="text">
+          <string>Ignore pose correction</string>
+         </property>
+        </widget>
+       </item>
+       <item row="4" column="0">
+        <widget class="QCheckBox" name="checkBox_ignorePoseCorrection">
+         <property name="text">
           <string/>
          </property>
-         <property name="minimum">
-          <number>0</number>
-         </property>
-         <property name="maximum">
-          <number>1000</number>
-         </property>
-         <property name="value">
-          <number>0</number>
+         <property name="checked">
+          <bool>false</bool>
          </property>
         </widget>
        </item>