From 16058dcbbefdf598674b39235e935d806f897125 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@f169173b-cf89-36c8-b27e-44dbe73f0c83>
Date: Sun, 13 Jul 2014 17:59:38 +0000
Subject: [PATCH] radiusPosesFiltering default keepLatest=true

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1550 f169173b-cf89-36c8-b27e-44dbe73f0c83
---
 corelib/include/rtabmap/core/util3d.h |  3 ++-
 corelib/src/util3d.cpp                | 30 ++++++++++++++++++++-------
 guilib/src/PreferencesDialog.cpp      |  2 +-
 guilib/src/ui/preferencesDialog.ui    |  8 +++----
 4 files changed, 30 insertions(+), 13 deletions(-)

diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h
index 200811c4..5b0bb1fd 100644
--- a/corelib/include/rtabmap/core/util3d.h
+++ b/corelib/include/rtabmap/core/util3d.h
@@ -393,7 +393,8 @@ bool RTABMAP_EXP loadTOROGraph(const std::string & fileName,
 std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
 		const std::map<int, Transform> & poses,
 		float radius,
-		float angle);
+		float angle,
+		bool keepLatest = true);
 
 cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
 		const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp
index 9f405e52..37cd4644 100644
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -1793,7 +1793,7 @@ bool loadTOROGraph(const std::string & fileName,
 }
 
 
-std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & poses, float radius, float angle)
+std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & poses, float radius, float angle, bool keepLatest)
 {
 	if(poses.size() > 1 && radius > 0.0f && angle>0.0f)
 	{
@@ -1841,15 +1841,31 @@ std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & p
 					}
 				}
 
-				bool firstAdded = false;
-				for(std::set<int>::iterator iter = cloudIndices.begin(); iter!=cloudIndices.end(); ++iter)
+				if(keepLatest)
 				{
-					if(!firstAdded)
+					bool lastAdded = false;
+					for(std::set<int>::reverse_iterator iter = cloudIndices.rbegin(); iter!=cloudIndices.rend(); ++iter)
 					{
-						indicesKept.insert(*iter);
-						firstAdded = true;
+						if(!lastAdded)
+						{
+							indicesKept.insert(*iter);
+							lastAdded = true;
+						}
+						indicesChecked.insert(*iter);
+					}
+				}
+				else
+				{
+					bool firstAdded = false;
+					for(std::set<int>::iterator iter = cloudIndices.begin(); iter!=cloudIndices.end(); ++iter)
+					{
+						if(!firstAdded)
+						{
+							indicesKept.insert(*iter);
+							firstAdded = true;
+						}
+						indicesChecked.insert(*iter);
 					}
-					indicesChecked.insert(*iter);
 				}
 			}
 		}
diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp
index bfa3cff7..34da904a 100644
--- a/guilib/src/PreferencesDialog.cpp
+++ b/guilib/src/PreferencesDialog.cpp
@@ -773,7 +773,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		}
 
 		_ui->groupBox_poseFiltering->setChecked(false);
-		_ui->doubleSpinBox_cloudFilterRadius->setValue(0.5);
+		_ui->doubleSpinBox_cloudFilterRadius->setValue(0.1);
 		_ui->doubleSpinBox_cloudFilterAngle->setValue(30);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui
index 97d066fd..c41c0032 100644
--- a/guilib/src/ui/preferencesDialog.ui
+++ b/guilib/src/ui/preferencesDialog.ui
@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-530</y>
+            <y>0</y>
             <width>709</width>
-            <height>1140</height>
+            <height>853</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>17</number>
+                <number>1</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -371,7 +371,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                             <double>0.010000000000000</double>
                            </property>
                            <property name="value">
-                            <double>0.500000000000000</double>
+                            <double>0.100000000000000</double>
                            </property>
                           </widget>
                          </item>