[Playerstage-commit] SF.net SVN: playerstage:[7610] code/player/trunk/server/drivers/mixed/mricp

[<gb...@us...>]

Revision: 7610
          http://playerstage.svn.sourceforge.net/playerstage/?rev=7610&view=rev
Author:   gbiggs
Date:     2009-05-01 07:51:31 +0000 (Fri, 01 May 2009)

Log Message:
-----------
Applied patch #2778487

Modified Paths:
--------------
    code/player/trunk/server/drivers/mixed/mricp/include/icp.h
    code/player/trunk/server/drivers/mixed/mricp/src/icp.cpp
    code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp

Modified: code/player/trunk/server/drivers/mixed/mricp/include/icp.h
===================================================================
--- code/player/trunk/server/drivers/mixed/mricp/include/icp.h	2009-05-01 07:40:22 UTC (rev 7609)
+++ code/player/trunk/server/drivers/mixed/mricp/include/icp.h	2009-05-01 07:51:31 UTC (rev 7610)
@@ -42,6 +42,7 @@
 	Pose align(std::vector<Point> , std::vector<Point>,Pose , double , int , bool );
 	const std::vector<Point> get_ref_points() { return b; }
 	const std::vector<Point> get_obs_points() { return a; }
+	bool warning_misalign;
 private:
 	vector<Point>   ref;
 	SweepSearch * nn;

Modified: code/player/trunk/server/drivers/mixed/mricp/src/icp.cpp
===================================================================
--- code/player/trunk/server/drivers/mixed/mricp/src/icp.cpp	2009-05-01 07:40:22 UTC (rev 7609)
+++ code/player/trunk/server/drivers/mixed/mricp/src/icp.cpp	2009-05-01 07:51:31 UTC (rev 7610)
@@ -6,12 +6,14 @@
 #include <iostream>
 using namespace std;
 
-namespace Geom2D 
-	{
+namespace Geom2D
+{
 	ICP::ICP()
+		: warning_misalign(true)
 		{
 			index.push_back(0);
-			index.push_back(0);			
+			index.push_back(0);
+			index.push_back(0);
 		};
 	ICP::~ICP()
 		{
@@ -25,25 +27,25 @@
 {
 	ref = reference;
 	nn = new SweepSearch(reference, gate);
-	Pose pse = init; 
-	double gate_sqr = sqr(gate); 
+	Pose pse = init;
+	double gate_sqr = sqr(gate);
 	int size_obs = obs.size();
 
 	 // used if interp == true
-	while (nits-- > 0) 
+	while (nits-- > 0)
 	{
 		Transform2D tr(pse);
 		a.clear();
 		b.clear();
 		// For each point in obs, find its NN in ref
-		for (int i = 0; i < size_obs; ++i) 
+		for (int i = 0; i < size_obs; ++i)
 		{
 			Point p = obs[i];
 			tr.transform_to_global(p); // transform obs[i] to estimated ref coord-frame
 
 			Point q;
 			// simple ICP
-			if (interp == false) 
+			if (interp == false)
 			{
 				int idx = nn->query(p);
 				if (idx == SweepSearch::NOT_FOUND)
@@ -64,7 +66,7 @@
 				lne.second = ref[index[1]];
 				intersection_line_point(q, lne, p);
 			}
-			if (dist_sqr(p,q) < gate_sqr) // check if NN is close enough 
+			if (dist_sqr(p,q) < gate_sqr) // check if NN is close enough
 			{
 				a.push_back(obs[i]);
 				b.push_back(q);
@@ -74,7 +76,8 @@
 		//If Less than half of the Observation is paired => not good alignment
 		if( a.size() < obs.size()/2.0)
 		{
-			cout<<"\n Detected Possible misalignment --- Skipping this Laser Set! Gate is:"<<gate;;
+			if (this->warning_misalign)
+				cout<<"\n Detected Possible misalignment --- Skipping this Laser Set! Gate is:"<<gate;
 			pse.p.x = -1 ; pse.p.y=-1 ; pse.phi = -1;
 			break;
 		}

Modified: code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp
===================================================================
--- code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp	2009-05-01 07:40:22 UTC (rev 7609)
+++ code/player/trunk/server/drivers/mixed/mricp/src/mricp_driver.cpp	2009-05-01 07:51:31 UTC (rev 7610)
@@ -139,8 +139,11 @@
   - Default "0.05"
   - 2nd data association gate for each point in scan
 - debug (bool)
-  - Defult: 0
-  - Display Debug Messeges
+  - Default: 0
+  - Display Debug Messages
+- warning (bool)
+  - Default: 1
+  - Display Warning Messages about misalignment problems
 - Log (bool)
   - Default: 0
   - Loggs the Odom Data (x,y,theta,ltics,rtics,lspeed,rspeed)
@@ -216,6 +219,7 @@
   sparse_scans_rate 3
   map_path "logs/"
   debug 0
+  warning 1
   alwayson 1
   log 1
 )
@@ -335,20 +339,21 @@
 MrIcp::MrIcp(ConfigFile* cf, int section)  : ThreadedDriver(cf, section)
 {
 	char config_temp[40];
-  	this->maxr =             cf->ReadFloat(section,"MAXR",7.8);
-  	this->start_in =         cf->ReadFloat(section,"start_in",2);
-  	this->minr =             cf->ReadFloat(section,"MINR",0.05);
-  	this->period =           cf->ReadFloat(section,"period",0.1);
-  	this->map_resolution =   cf->ReadFloat(section,"map_resolution",0.05); // In METERS
-  	this->map_size = 	     cf->ReadFloat(section,"map_size",20);         // In METERS
-  	this->nit =              cf->ReadInt  (section,"NIT",10);
-  	this->robot_id =         cf->ReadInt  (section,"robot_id",0);
-  	this->number_of_lasers = cf->ReadInt  (section,"number_of_lasers",1);
-  	this->gate1 =            cf->ReadFloat(section,"gate1",0.5);
-  	this->gate2 =            cf->ReadFloat(section,"gate2",0.05);
-  	this->interpolate =      cf->ReadInt  (section, "interpolate", 1);
-  	this->map_path	=(char *)cf->ReadString(section,"map_path","maps/");
+	this->maxr =             cf->ReadFloat(section,"MAXR",7.8);
+	this->start_in =         cf->ReadFloat(section,"start_in",2);
+	this->minr =             cf->ReadFloat(section,"MINR",0.05);
+	this->period =           cf->ReadFloat(section,"period",0.1);
+	this->map_resolution =   cf->ReadFloat(section,"map_resolution",0.05); // In METERS
+	this->map_size = 	     cf->ReadFloat(section,"map_size",20);         // In METERS
+	this->nit =              cf->ReadInt  (section,"NIT",10);
+	this->robot_id =         cf->ReadInt  (section,"robot_id",0);
+	this->number_of_lasers = cf->ReadInt  (section,"number_of_lasers",1);
+	this->gate1 =            cf->ReadFloat(section,"gate1",0.5);
+	this->gate2 =            cf->ReadFloat(section,"gate2",0.05);
+	this->interpolate =      cf->ReadInt  (section, "interpolate", 1);
+	this->map_path	=(char *)cf->ReadString(section,"map_path","maps/");
 	this->debug = 			 cf->ReadInt(section,"debug",0);
+	this->warning_misalign = icp.warning_misalign = cf->ReadInt(section,"warning",1);
 	this->log =   			 cf->ReadInt(section,  "log",0);
 	this->use_odom = 		 cf->ReadInt(section,  "use_odom",0);
 	this->playerv_debug =    cf->ReadInt(section,  "playerv_debug",0);
@@ -1312,7 +1317,8 @@
 	delta_pose = icp.align(laser_set_1,laser_set_2,delta_pose, gate1, nit, interpolate);
 	if(delta_pose.p.x ==-1 && delta_pose.p.y ==-1 && delta_pose.phi==-1)
 	{
-		cout <<"\nWARNING: possible misalignment ICP: 1 - skipping scan";
+		if (this->warning_misalign)
+			cout <<"\nWARNING: possible misalignment ICP: 1 - skipping scan";
 		//laser_set_1 = laser_set_2;
 		return;
 	}
@@ -1338,7 +1344,8 @@
 	global_pose = icp.align(this->local_map,laser_set_2,global_pose, gate1, nit, interpolate);
 	if(global_pose.p.x ==-1 && global_pose.p.y ==-1 && global_pose.phi==-1)
 	{
-		cout <<"\nWARNING: possible misalignment ICP: 2 - skipping scan";
+		if (this->warning_misalign)
+			cout <<"\nWARNING: possible misalignment ICP: 2 - skipping scan";
 		global_pose.p.x = global_pose_prev.p.x;
 		global_pose.p.y = global_pose_prev.p.y;
 		global_pose.phi = global_pose_prev.phi;
@@ -1350,7 +1357,8 @@
 	global_pose = icp.align(this->local_map,laser_set_2,global_pose, gate2, nit, interpolate);
 	if(global_pose.p.x ==-1 && global_pose.p.y ==-1 && global_pose.phi==-1)
 	{
-		cout <<"\nWARNING: possible misalignment ICP: 3 - skipping scan";
+		if (this->warning_misalign)
+			cout <<"\nWARNING: possible misalignment ICP: 3 - skipping scan";
 		global_pose.p.x = global_pose_prev.p.x;
 		global_pose.p.y = global_pose_prev.p.y;
 		global_pose.phi = global_pose_prev.phi;


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.