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.
------------------------------------------------------------------------------
Register Now & Save for Velocity, the Web Performance & Operations
Conference from O'Reilly Media. Velocity features a full day of
expert-led, hands-on workshops and two days of sessions from industry
leaders in dedicated Performance & Operations tracks. Use code vel09scf
and Save an extra 15% before 5/3. http://p.sf.net/sfu/velocityconf
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
