diff --git a/sm/drivers/playerstage/mricp/.directory b/sm/drivers/playerstage/mricp/.directory
new file mode 100644
index 0000000000000000000000000000000000000000..7196faf0560ee3676489bb98abcf4e089d2477e4
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/.directory
@@ -0,0 +1,3 @@
+[Dolphin]
+ShowPreview=true
+Timestamp=2010,6,8,13,2,39
diff --git a/sm/drivers/playerstage/mricp/README b/sm/drivers/playerstage/mricp/README
new file mode 100644
index 0000000000000000000000000000000000000000..cb9075636bdccddc7f4311ec0626b2589bb5ad4e
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/README
@@ -0,0 +1,181 @@
+This is the MRICP driver for player/stage.
+
+This Driver is used as a localizer and Occupancy Grid Map Builder using simple Bayesian
+Probability Update. It uses Iterative Closest Point method to align laser scans and estimate
+the change of pose.
+
+USE
+
+The driver requires at least one laser interface, and optionally a position
+interface (the latter is required if you actually want to drive the
+robot via this driver).  The driver provides a position interface which
+returns the position of the robot in the current patch as calculated by
+mricp.
+
+	Specifications / Operation
+- Upon start, the driver initializes the initial pose to [0 0 0] m,m,rd
+- When the driver recieves a PLAYER_POSITION_RESET_ODOM_REQ configuration
+  request, it:
+  1- Saves the current map patch and starts a new one.
+  2- Raves the pose of the new patch in respect to the old one.
+  3- It resets the Robot pose to[0 0 0].
+  4- Resets the Occupancy grid probability and assumes all the cells unknown.
+
+- The driver saves the information about the map patches and their position
+  relative to the previous patch map_path/map_configs.txt where map_path is the path
+  of the directory to contain the maps specified in the driver configuration file.
+
+- The driver computes the Occupancy Grid using Bayesian probability update. It computes
+  the uncertainty in the sensor readings and uses the prior knowledge of each cell to get
+  the posterior (new estimation) of the cells state.
+
+|********************|
+| CONFIG PARAMETERS  |
+|********************|
+
+- MAXR (double)
+  - Default: "7.8"
+  - Maximium Laser Range, This must be smaller than the laser's maximum range,
+    especially for SICK lasers.  Use 3.9 for Hokuyo URG.
+- MINR (double)
+  - Default: "0.05"
+  - Minimium Laser Range
+- period (double)
+  - Default: "0.1"
+  - Time in sec defining how frequently to grab laser scans
+    for scan matching. It is not a good idea to use maximum scans available on a SICK.
+    0.1 to 0.2 works well depending on robot turnrate and speed.
+- map_resolution (double)
+  - Default: "0.05"
+  - Pixel resolution in meter of the map to be build , a number close to the laser
+    precision is wise.
+- map_size (double)
+  - Default: 20
+  - This is defined from the origin to the boundary, so is actually half the size of the
+    map PATCH, not the whole map.
+- interpolate (bool)
+  - Default "1"
+  - 0 - Simple ICP, 1 - ICP with interpolation
+- NIT (int)
+  - Default "10"
+  - Number of iterations for each scan-matching.
+- gate1 (float)
+  - Default "0.5"
+  - 1st data association gate for each point in scan
+- gate2 (float)
+  - Default "0.05"
+  - 2nd data association gate for each point in scan
+- debug (bool)
+  - Defult: 0
+  - Display Debug Messeges
+- Log (bool)
+  - Default: 0
+  - Loggs the Odom Data (x,y,theta,ltics,rtics,lspeed,rspeed)
+- map_path(string)
+  - Default: "maps/"
+  - Specifies the locations where patches and logs are to be saved,
+- start_in(int)
+  - Default : 2
+  - Delay Before starting, unit is in seconds
+- robot_id(int)
+  - Default : 0
+  - The Robot id assigned for map identification
+- number_of_laser(int)
+  - Default : 1
+  - The number of lasers to be used in the scan matching (index starts from 0) all lasers
+    should be declared in the requires section
+@par Example
+
+@verbatim
+driver
+(
+  name "mricp"
+  provides ["position2d:1" "map:0"]
+  requires ["position2d:0" "laser:0"]
+  number_of_lasers 1
+  laser0_ranges [-120 120]
+  playerv_debug 0
+  period 0.2
+  MAXR 3.9
+  MINR 0.05
+  map_resolution 0.05
+  map_saving_period 5
+  map_size 10
+  use_max_range 4
+  sparse_scans_rate 3
+  map_path "logs/"
+  debug 0
+  alwayson 1
+  log 1
+)
+|****************|
+| STAGE TESTING  |
+|****************|
+
+- The directory stage_test contains a stage configuration for testing
+  mricp.  Simply run:
+  "player stage_mricp.cfg"
+
+- To test multiple laser support, modify the world file and add X laser models
+  to your robot (X is the number of lasers). Then modify your configuration file
+  so that stage provides that X laser interfaces.
+  Note: Laser pose is essential for the Mricp to work, specify the correct laser
+        poses.
+  Example :
+  your .world file should contain something like this for 2 Laser support
+		define sicklaser1 laser
+		(
+		  range_min 0.0
+		  range_max 8.0
+		  fov 180.0
+		  samples 180
+		  color "LightBlue"
+		  size [ 0.14 0.14 ]
+		  pose [0.8 -0.15 0 ]
+		)
+		define sicklaser2 laser
+		(
+		  range_min 0.0
+		  range_max 8.0
+		  fov 180.0
+		  samples 180
+		  color "LightBlue"
+		  size [ 0.14 0.14 ]
+		  pose [-0.10 0.15 -180 ]
+		)
+		# extend the wheelchair definition from wheelchair.inc
+
+		wheelchair
+		(
+		   name "robot"
+		   pose [14.2 -4.6 180]
+		   sicklaser1()
+		   sicklaser2()
+		)
+	and your configuration should contain something like:
+		driver
+		(
+		  name "stage"
+		  provides ["position:0" "laser:0" "laser:1"]
+		  model "robot"
+		)
+		driver
+		(
+		  name "MrIcpDriver"
+		  plugin "MrIcpDriver.so"
+		  provides ["position:1" "map:0"]
+		  requires ["laser:0" "laser:1" "position:0"]
+		  map_size 10
+		  map_resolution 0.05
+		  number_of_lasers 2
+		  period 0.1
+		  MAXR 7.5
+		  MINR 0.05
+		  debug 0
+		  alwayson 1
+		  log 1
+		)
+
+JP 21/4/06
+modified
+TT 16/5/06
diff --git a/sm/drivers/playerstage/mricp/doc/html/.directory b/sm/drivers/playerstage/mricp/doc/html/.directory
new file mode 100644
index 0000000000000000000000000000000000000000..a76835f3861b7e89f0f74f45d57dbd4cc2e18df1
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/.directory
@@ -0,0 +1,6 @@
+[Dolphin]
+ShowPreview=true
+Timestamp=2010,6,8,13,1,37
+
+[Settings]
+ShowDotFiles=true
diff --git a/sm/drivers/playerstage/mricp/doc/html/Timer_8h-source.html b/sm/drivers/playerstage/mricp/doc/html/Timer_8h-source.html
new file mode 100644
index 0000000000000000000000000000000000000000..be08b560fa519a4fe0c40b28a1df12853b7037cd
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/Timer_8h-source.html
@@ -0,0 +1,42 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: include/Timer.h Source File</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li><a href="modules.html"><span>Modules</span></a></li>
+    <li><a href="annotated.html"><span>Classes</span></a></li>
+    <li id="current"><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<h1>include/Timer.h</h1><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="preprocessor">#ifndef TIMER_H_</span>
+<a name="l00002"></a>00002 <span class="preprocessor"></span><span class="preprocessor">#define TIMER_H_</span>
+<a name="l00003"></a>00003 <span class="preprocessor"></span><span class="preprocessor">#include &lt;sys/time.h&gt;</span>
+<a name="l00004"></a>00004 <span class="preprocessor">#include &lt;iostream&gt;</span>
+<a name="l00005"></a>00005 <span class="keyword">class </span>MricpTimer
+<a name="l00006"></a>00006 {
+<a name="l00007"></a>00007         <span class="keyword">private</span>:
+<a name="l00008"></a>00008                 <span class="keyword">struct </span>timeval start_time,end_time;
+<a name="l00009"></a>00009                 <span class="keywordtype">double</span> time_diff;
+<a name="l00010"></a>00010         <span class="keyword">public</span>:
+<a name="l00011"></a>00011                 MricpTimer();
+<a name="l00012"></a>00012                 <span class="keywordtype">double</span> TimeElapsed(); <span class="comment">// time elapsed in usec since last call</span>
+<a name="l00013"></a>00013                 <span class="keywordtype">void</span> Reset();                           <span class="comment">// resets timer</span>
+<a name="l00014"></a>00014                 <span class="keyword">virtual</span> ~MricpTimer();
+<a name="l00015"></a>00015                 <span class="comment">/* Synchronize the loop within a period</span>
+<a name="l00016"></a>00016 <span class="comment">                 * To use this u will have to initialize the timer</span>
+<a name="l00017"></a>00017 <span class="comment">                 * reset the timer at the beginning of the loop</span>
+<a name="l00018"></a>00018 <span class="comment">                 * and call the Synch function at the end of the loop</span>
+<a name="l00019"></a>00019 <span class="comment">                 */</span>
+<a name="l00020"></a>00020                 <span class="keywordtype">void</span> Synch(<span class="keywordtype">double</span> period); <span class="comment">// period should be in msec</span>
+<a name="l00021"></a>00021 };
+<a name="l00022"></a>00022 
+<a name="l00023"></a>00023 <span class="preprocessor">#endif </span><span class="comment">/*TIMER_H_*/</span>
+</pre></div><hr size="1"><address style="align: right;"><small>Generated on Tue Aug 8 12:54:32 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/annotated.html b/sm/drivers/playerstage/mricp/doc/html/annotated.html
new file mode 100644
index 0000000000000000000000000000000000000000..3ca17e15482dac4064cb8af038039df31bb547f2
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/annotated.html
@@ -0,0 +1,27 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: Class List</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li><a href="modules.html"><span>Modules</span></a></li>
+    <li id="current"><a href="annotated.html"><span>Classes</span></a></li>
+    <li><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<div class="tabs">
+  <ul>
+    <li id="current"><a href="annotated.html"><span>Class&nbsp;List</span></a></li>
+    <li><a href="hierarchy.html"><span>Class&nbsp;Hierarchy</span></a></li>
+  </ul></div>
+<h1>MapReferenceICP Class List</h1>Here are the classes, structs, unions and interfaces with brief descriptions:<table>
+  <tr><td class="indexkey"><a class="el" href="structlaser__range.html">laser_range</a></td><td class="indexvalue">MRICP DRIVER Class /// </td></tr>
+</table>
+<hr size="1"><address style="align: right;"><small>Generated on Tue Aug 8 12:54:32 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/doxygen.css b/sm/drivers/playerstage/mricp/doc/html/doxygen.css
new file mode 100644
index 0000000000000000000000000000000000000000..05615b2e61db5e7101503448aeb4f95bbd8d6a22
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/doxygen.css
@@ -0,0 +1,310 @@
+BODY,H1,H2,H3,H4,H5,H6,P,CENTER,TD,TH,UL,DL,DIV {
+	font-family: Geneva, Arial, Helvetica, sans-serif;
+}
+BODY,TD {
+       font-size: 90%;
+}
+H1 {
+	text-align: center;
+       font-size: 160%;
+}
+H2 {
+       font-size: 120%;
+}
+H3 {
+       font-size: 100%;
+}
+CAPTION { font-weight: bold }
+DIV.qindex {
+	width: 100%;
+	background-color: #e8eef2;
+	border: 1px solid #84b0c7;
+	text-align: center;
+	margin: 2px;
+	padding: 2px;
+	line-height: 140%;
+}
+DIV.nav {
+	width: 100%;
+	background-color: #e8eef2;
+	border: 1px solid #84b0c7;
+	text-align: center;
+	margin: 2px;
+	padding: 2px;
+	line-height: 140%;
+}
+DIV.navtab {
+       background-color: #e8eef2;
+       border: 1px solid #84b0c7;
+       text-align: center;
+       margin: 2px;
+       margin-right: 15px;
+       padding: 2px;
+}
+TD.navtab {
+       font-size: 70%;
+}
+A.qindex {
+       text-decoration: none;
+       font-weight: bold;
+       color: #1A419D;
+}
+A.qindex:visited {
+       text-decoration: none;
+       font-weight: bold;
+       color: #1A419D
+}
+A.qindex:hover {
+	text-decoration: none;
+	background-color: #ddddff;
+}
+A.qindexHL {
+	text-decoration: none;
+	font-weight: bold;
+	background-color: #6666cc;
+	color: #ffffff;
+	border: 1px double #9295C2;
+}
+A.qindexHL:hover {
+	text-decoration: none;
+	background-color: #6666cc;
+	color: #ffffff;
+}
+A.qindexHL:visited { text-decoration: none; background-color: #6666cc; color: #ffffff }
+A.el { text-decoration: none; font-weight: bold }
+A.elRef { font-weight: bold }
+A.code:link { text-decoration: none; font-weight: normal; color: #0000FF}
+A.code:visited { text-decoration: none; font-weight: normal; color: #0000FF}
+A.codeRef:link { font-weight: normal; color: #0000FF}
+A.codeRef:visited { font-weight: normal; color: #0000FF}
+A:hover { text-decoration: none; background-color: #f2f2ff }
+DL.el { margin-left: -1cm }
+.fragment {
+       font-family: Fixed, monospace;
+       font-size: 95%;
+}
+PRE.fragment {
+	border: 1px solid #CCCCCC;
+	background-color: #f5f5f5;
+	margin-top: 4px;
+	margin-bottom: 4px;
+	margin-left: 2px;
+	margin-right: 8px;
+	padding-left: 6px;
+	padding-right: 6px;
+	padding-top: 4px;
+	padding-bottom: 4px;
+}
+DIV.ah { background-color: black; font-weight: bold; color: #ffffff; margin-bottom: 3px; margin-top: 3px }
+TD.md { background-color: #F4F4FB; font-weight: bold; }
+TD.mdPrefix {
+       background-color: #F4F4FB;
+       color: #606060;
+	font-size: 80%;
+}
+TD.mdname1 { background-color: #F4F4FB; font-weight: bold; color: #602020; }
+TD.mdname { background-color: #F4F4FB; font-weight: bold; color: #602020; width: 600px; }
+DIV.groupHeader {
+       margin-left: 16px;
+       margin-top: 12px;
+       margin-bottom: 6px;
+       font-weight: bold;
+}
+DIV.groupText { margin-left: 16px; font-style: italic; font-size: 90% }
+BODY {
+	background: white;
+	color: black;
+	margin-right: 20px;
+	margin-left: 20px;
+}
+TD.indexkey {
+	background-color: #e8eef2;
+	font-weight: bold;
+	padding-right  : 10px;
+	padding-top    : 2px;
+	padding-left   : 10px;
+	padding-bottom : 2px;
+	margin-left    : 0px;
+	margin-right   : 0px;
+	margin-top     : 2px;
+	margin-bottom  : 2px;
+	border: 1px solid #CCCCCC;
+}
+TD.indexvalue {
+	background-color: #e8eef2;
+	font-style: italic;
+	padding-right  : 10px;
+	padding-top    : 2px;
+	padding-left   : 10px;
+	padding-bottom : 2px;
+	margin-left    : 0px;
+	margin-right   : 0px;
+	margin-top     : 2px;
+	margin-bottom  : 2px;
+	border: 1px solid #CCCCCC;
+}
+TR.memlist {
+   background-color: #f0f0f0; 
+}
+P.formulaDsp { text-align: center; }
+IMG.formulaDsp { }
+IMG.formulaInl { vertical-align: middle; }
+SPAN.keyword       { color: #008000 }
+SPAN.keywordtype   { color: #604020 }
+SPAN.keywordflow   { color: #e08000 }
+SPAN.comment       { color: #800000 }
+SPAN.preprocessor  { color: #806020 }
+SPAN.stringliteral { color: #002080 }
+SPAN.charliteral   { color: #008080 }
+.mdTable {
+	border: 1px solid #868686;
+	background-color: #F4F4FB;
+}
+.mdRow {
+	padding: 8px 10px;
+}
+.mdescLeft {
+       padding: 0px 8px 4px 8px;
+	font-size: 80%;
+	font-style: italic;
+	background-color: #FAFAFA;
+	border-top: 1px none #E0E0E0;
+	border-right: 1px none #E0E0E0;
+	border-bottom: 1px none #E0E0E0;
+	border-left: 1px none #E0E0E0;
+	margin: 0px;
+}
+.mdescRight {
+       padding: 0px 8px 4px 8px;
+	font-size: 80%;
+	font-style: italic;
+	background-color: #FAFAFA;
+	border-top: 1px none #E0E0E0;
+	border-right: 1px none #E0E0E0;
+	border-bottom: 1px none #E0E0E0;
+	border-left: 1px none #E0E0E0;
+	margin: 0px;
+}
+.memItemLeft {
+	padding: 1px 0px 0px 8px;
+	margin: 4px;
+	border-top-width: 1px;
+	border-right-width: 1px;
+	border-bottom-width: 1px;
+	border-left-width: 1px;
+	border-top-color: #E0E0E0;
+	border-right-color: #E0E0E0;
+	border-bottom-color: #E0E0E0;
+	border-left-color: #E0E0E0;
+	border-top-style: solid;
+	border-right-style: none;
+	border-bottom-style: none;
+	border-left-style: none;
+	background-color: #FAFAFA;
+	font-size: 80%;
+}
+.memItemRight {
+	padding: 1px 8px 0px 8px;
+	margin: 4px;
+	border-top-width: 1px;
+	border-right-width: 1px;
+	border-bottom-width: 1px;
+	border-left-width: 1px;
+	border-top-color: #E0E0E0;
+	border-right-color: #E0E0E0;
+	border-bottom-color: #E0E0E0;
+	border-left-color: #E0E0E0;
+	border-top-style: solid;
+	border-right-style: none;
+	border-bottom-style: none;
+	border-left-style: none;
+	background-color: #FAFAFA;
+	font-size: 80%;
+}
+.memTemplItemLeft {
+	padding: 1px 0px 0px 8px;
+	margin: 4px;
+	border-top-width: 1px;
+	border-right-width: 1px;
+	border-bottom-width: 1px;
+	border-left-width: 1px;
+	border-top-color: #E0E0E0;
+	border-right-color: #E0E0E0;
+	border-bottom-color: #E0E0E0;
+	border-left-color: #E0E0E0;
+	border-top-style: none;
+	border-right-style: none;
+	border-bottom-style: none;
+	border-left-style: none;
+	background-color: #FAFAFA;
+	font-size: 80%;
+}
+.memTemplItemRight {
+	padding: 1px 8px 0px 8px;
+	margin: 4px;
+	border-top-width: 1px;
+	border-right-width: 1px;
+	border-bottom-width: 1px;
+	border-left-width: 1px;
+	border-top-color: #E0E0E0;
+	border-right-color: #E0E0E0;
+	border-bottom-color: #E0E0E0;
+	border-left-color: #E0E0E0;
+	border-top-style: none;
+	border-right-style: none;
+	border-bottom-style: none;
+	border-left-style: none;
+	background-color: #FAFAFA;
+	font-size: 80%;
+}
+.memTemplParams {
+	padding: 1px 0px 0px 8px;
+	margin: 4px;
+	border-top-width: 1px;
+	border-right-width: 1px;
+	border-bottom-width: 1px;
+	border-left-width: 1px;
+	border-top-color: #E0E0E0;
+	border-right-color: #E0E0E0;
+	border-bottom-color: #E0E0E0;
+	border-left-color: #E0E0E0;
+	border-top-style: solid;
+	border-right-style: none;
+	border-bottom-style: none;
+	border-left-style: none;
+       color: #606060;
+	background-color: #FAFAFA;
+	font-size: 80%;
+}
+.search     { color: #003399;
+              font-weight: bold;
+}
+FORM.search {
+              margin-bottom: 0px;
+              margin-top: 0px;
+}
+INPUT.search { font-size: 75%;
+               color: #000080;
+               font-weight: normal;
+               background-color: #e8eef2;
+}
+TD.tiny      { font-size: 75%;
+}
+a {
+	color: #1A41A8;
+}
+a:visited {
+	color: #2A3798;
+}
+.dirtab { padding: 4px;
+          border-collapse: collapse;
+          border: 1px solid #84b0c7;
+}
+TH.dirtab { background: #e8eef2;
+            font-weight: bold;
+}
+HR { height: 1px;
+     border: none;
+     border-top: 1px solid black;
+}
+
diff --git a/sm/drivers/playerstage/mricp/doc/html/doxygen.png b/sm/drivers/playerstage/mricp/doc/html/doxygen.png
new file mode 100644
index 0000000000000000000000000000000000000000..f0a274bbaffdd67f6d784c894d9cf28729db0e14
Binary files /dev/null and b/sm/drivers/playerstage/mricp/doc/html/doxygen.png differ
diff --git a/sm/drivers/playerstage/mricp/doc/html/files.html b/sm/drivers/playerstage/mricp/doc/html/files.html
new file mode 100644
index 0000000000000000000000000000000000000000..a25e4b012cf494e26d49d119a222c438e4c7dca6
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/files.html
@@ -0,0 +1,27 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: File Index</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li><a href="modules.html"><span>Modules</span></a></li>
+    <li><a href="annotated.html"><span>Classes</span></a></li>
+    <li id="current"><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<h1>MapReferenceICP File List</h1>Here is a list of all documented files with brief descriptions:<table>
+  <tr><td class="indexkey">include/<b>geometry2D.h</b> <a href="geometry2D_8h-source.html">[code]</a></td><td class="indexvalue"></td></tr>
+  <tr><td class="indexkey">include/<b>icp.h</b> <a href="icp_8h-source.html">[code]</a></td><td class="indexvalue"></td></tr>
+  <tr><td class="indexkey">include/<b>lasermodel.h</b> <a href="lasermodel_8h-source.html">[code]</a></td><td class="indexvalue"></td></tr>
+  <tr><td class="indexkey">include/<b>map.h</b> <a href="map_8h-source.html">[code]</a></td><td class="indexvalue"></td></tr>
+  <tr><td class="indexkey">include/<b>nn.h</b> <a href="nn_8h-source.html">[code]</a></td><td class="indexvalue"></td></tr>
+  <tr><td class="indexkey">include/<b>Timer.h</b> <a href="Timer_8h-source.html">[code]</a></td><td class="indexvalue"></td></tr>
+</table>
+<hr size="1"><address style="align: right;"><small>Generated on Tue Aug 8 12:54:32 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/geometry2D_8h-source.html b/sm/drivers/playerstage/mricp/doc/html/geometry2D_8h-source.html
new file mode 100644
index 0000000000000000000000000000000000000000..e6d7aaff1fcd8d60920134619bd694919cb60361
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/geometry2D_8h-source.html
@@ -0,0 +1,162 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: include/geometry2D.h Source File</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li><a href="modules.html"><span>Modules</span></a></li>
+    <li><a href="annotated.html"><span>Classes</span></a></li>
+    <li id="current"><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<h1>include/geometry2D.h</h1><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/* Simple 2D geometric operations with points, poses, and lines.</span>
+<a name="l00002"></a>00002 <span class="comment"> *</span>
+<a name="l00003"></a>00003 <span class="comment"> * These algorithms were worked out by me from first principles several years</span>
+<a name="l00004"></a>00004 <span class="comment"> * ago. They work ok, but are not particularly good implementations. I recommend</span>
+<a name="l00005"></a>00005 <span class="comment"> * the geometric source-code and resources by David Eberly on the "Magic Software"</span>
+<a name="l00006"></a>00006 <span class="comment"> * website: www.magic-software.com.</span>
+<a name="l00007"></a>00007 <span class="comment"> *</span>
+<a name="l00008"></a>00008 <span class="comment"> * Tim Bailey 2004.</span>
+<a name="l00009"></a>00009 <span class="comment"> */</span>
+<a name="l00010"></a>00010 <span class="preprocessor">#ifndef GEOMETRY_2D_H_</span>
+<a name="l00011"></a>00011 <span class="preprocessor"></span><span class="preprocessor">#define GEOMETRY_2D_H_</span>
+<a name="l00012"></a>00012 <span class="preprocessor"></span>
+<a name="l00013"></a>00013 <span class="preprocessor">#include &lt;vector&gt;</span>
+<a name="l00014"></a>00014 
+<a name="l00015"></a>00015 <span class="keyword">namespace </span>Geom2D {
+<a name="l00016"></a>00016 
+<a name="l00017"></a>00017 <span class="comment">//</span>
+<a name="l00018"></a>00018 <span class="comment">// Basic Structures</span>
+<a name="l00019"></a>00019 <span class="comment">//</span>
+<a name="l00020"></a>00020 
+<a name="l00021"></a>00021 <span class="keyword">struct </span>Point 
+<a name="l00022"></a>00022 {
+<a name="l00023"></a>00023         <span class="keywordtype">double</span> x;
+<a name="l00024"></a>00024         <span class="keywordtype">double</span> y;
+<a name="l00025"></a>00025         <span class="keywordtype">short</span> <span class="keywordtype">int</span> laser_index;
+<a name="l00026"></a>00026 };
+<a name="l00027"></a>00027 
+<a name="l00028"></a>00028 <span class="keyword">struct </span>Pose 
+<a name="l00029"></a>00029 {
+<a name="l00030"></a>00030         Point p;
+<a name="l00031"></a>00031         <span class="keywordtype">double</span> phi;
+<a name="l00032"></a>00032 };
+<a name="l00033"></a>00033 
+<a name="l00034"></a>00034 <span class="keyword">struct </span>Line {
+<a name="l00035"></a>00035         Point first;
+<a name="l00036"></a>00036         Point second;
+<a name="l00037"></a>00037 };
+<a name="l00038"></a>00038 
+<a name="l00039"></a>00039 <span class="comment">//</span>
+<a name="l00040"></a>00040 <span class="comment">// Utility functions</span>
+<a name="l00041"></a>00041 <span class="comment">//</span>
+<a name="l00042"></a>00042 
+<a name="l00043"></a>00043 <span class="keyword">const</span> <span class="keywordtype">double</span> PI = 3.14159265358979;
+<a name="l00044"></a>00044 
+<a name="l00045"></a>00045 <span class="keyword">inline</span> 
+<a name="l00046"></a>00046 <span class="keywordtype">double</span> sqr(<span class="keywordtype">double</span> x) { <span class="keywordflow">return</span> x*x; }
+<a name="l00047"></a>00047 
+<a name="l00048"></a>00048 <span class="keyword">inline</span> 
+<a name="l00049"></a>00049 <span class="keywordtype">double</span> abs(<span class="keywordtype">double</span> x) { <span class="keywordflow">return</span> (x&lt;0.) ? -x : x; }
+<a name="l00050"></a>00050 
+<a name="l00051"></a>00051 <span class="keyword">inline</span>
+<a name="l00052"></a>00052 <span class="keywordtype">double</span> round(<span class="keywordtype">double</span> x) { 
+<a name="l00053"></a>00053         <span class="keywordflow">return</span> (x&lt;0.) ? -static_cast&lt;int&gt;(0.5-x) : static_cast&lt;int&gt;(0.5+x); 
+<a name="l00054"></a>00054 }
+<a name="l00055"></a>00055 <span class="comment">/*</span>
+<a name="l00056"></a>00056 <span class="comment">template&lt;class T&gt;</span>
+<a name="l00057"></a>00057 <span class="comment">inline</span>
+<a name="l00058"></a>00058 <span class="comment">void swap(T&amp; a, T&amp; b) </span>
+<a name="l00059"></a>00059 <span class="comment">{</span>
+<a name="l00060"></a>00060 <span class="comment">        T tmp(a);</span>
+<a name="l00061"></a>00061 <span class="comment">        a = b;</span>
+<a name="l00062"></a>00062 <span class="comment">        b = tmp;</span>
+<a name="l00063"></a>00063 <span class="comment">}</span>
+<a name="l00064"></a>00064 <span class="comment">*/</span>
+<a name="l00065"></a>00065 <span class="keyword">inline</span>
+<a name="l00066"></a>00066 <span class="keywordtype">double</span> pi_to_pi(<span class="keywordtype">double</span> angle) { <span class="comment">// normalise an angle to within +/- PI</span>
+<a name="l00067"></a>00067         <span class="keywordflow">while</span> (angle &lt; -PI)
+<a name="l00068"></a>00068                 angle += 2.*PI;
+<a name="l00069"></a>00069         <span class="keywordflow">while</span> (angle &gt; PI)
+<a name="l00070"></a>00070                 angle -= 2.*PI;
+<a name="l00071"></a>00071         <span class="keywordflow">return</span> angle;
+<a name="l00072"></a>00072 }
+<a name="l00073"></a>00073 
+<a name="l00074"></a>00074 <span class="comment">//</span>
+<a name="l00075"></a>00075 <span class="comment">// Point and Pose algorithms</span>
+<a name="l00076"></a>00076 <span class="comment">//</span>
+<a name="l00077"></a>00077 
+<a name="l00078"></a>00078 <span class="keyword">inline</span> 
+<a name="l00079"></a>00079 <span class="keywordtype">double</span> dist_sqr(<span class="keyword">const</span> Point&amp; p, <span class="keyword">const</span> Point&amp; q) { <span class="comment">// squared distance between two Points</span>
+<a name="l00080"></a>00080         <span class="keywordflow">return</span> (sqr(p.x-q.x) + sqr(p.y-q.y));
+<a name="l00081"></a>00081 }
+<a name="l00082"></a>00082 
+<a name="l00083"></a>00083 <span class="keywordtype">double</span> dist(<span class="keyword">const</span> Point&amp; p, <span class="keyword">const</span> Point&amp; q);
+<a name="l00084"></a>00084 Pose compute_relative_pose(<span class="keyword">const</span> std::vector&lt;Point&gt;&amp; a, <span class="keyword">const</span> std::vector&lt;Point&gt;&amp; b);
+<a name="l00085"></a>00085 
+<a name="l00086"></a>00086 <span class="comment">//</span>
+<a name="l00087"></a>00087 <span class="comment">// Line algorithms</span>
+<a name="l00088"></a>00088 <span class="comment">//</span>
+<a name="l00089"></a>00089 
+<a name="l00090"></a>00090 <span class="keywordtype">bool</span> intersection_line_line (Point&amp; p, <span class="keyword">const</span> Line&amp; l, <span class="keyword">const</span> Line&amp; m);
+<a name="l00091"></a>00091 <span class="keywordtype">double</span> distance_line_point (<span class="keyword">const</span> Line&amp; lne, <span class="keyword">const</span> Point&amp; p);
+<a name="l00092"></a>00092 <span class="keywordtype">void</span> intersection_line_point(Point&amp; p, <span class="keyword">const</span> Line&amp; l, <span class="keyword">const</span> Point&amp; q);
+<a name="l00093"></a>00093 
+<a name="l00094"></a>00094 <span class="comment">//</span>
+<a name="l00095"></a>00095 <span class="comment">// Basic transformations on 2-D Points (x,y) and Poses (x,y,phi).</span>
+<a name="l00096"></a>00096 <span class="comment">//</span>
+<a name="l00097"></a>00097 
+<a name="l00098"></a>00098 <span class="keyword">class </span>Transform2D {
+<a name="l00099"></a>00099 <span class="keyword">public</span>:
+<a name="l00100"></a>00100         Transform2D(<span class="keyword">const</span> Pose&amp; ref);
+<a name="l00101"></a>00101 
+<a name="l00102"></a>00102         <span class="keywordtype">void</span> transform_to_relative(Point &amp;p);
+<a name="l00103"></a>00103         <span class="keywordtype">void</span> transform_to_relative(Pose &amp;p);
+<a name="l00104"></a>00104         <span class="keywordtype">void</span> transform_to_global(Point &amp;p);
+<a name="l00105"></a>00105         <span class="keywordtype">void</span> transform_to_global(Pose &amp;p);
+<a name="l00106"></a>00106 
+<a name="l00107"></a>00107 <span class="keyword">private</span>:
+<a name="l00108"></a>00108         <span class="keyword">const</span> Pose base;
+<a name="l00109"></a>00109         <span class="keywordtype">double</span> c;
+<a name="l00110"></a>00110         <span class="keywordtype">double</span> s;
+<a name="l00111"></a>00111 };
+<a name="l00112"></a>00112 
+<a name="l00113"></a>00113 <span class="keyword">inline</span>
+<a name="l00114"></a>00114 <span class="keywordtype">void</span> Transform2D::transform_to_relative(Point &amp;p) {
+<a name="l00115"></a>00115         p.x -= base.p.x; 
+<a name="l00116"></a>00116         p.y -= base.p.y;
+<a name="l00117"></a>00117         <span class="keywordtype">double</span> t(p.x);
+<a name="l00118"></a>00118         p.x = p.x*c + p.y*s;
+<a name="l00119"></a>00119         p.y = p.y*c -   t*s;
+<a name="l00120"></a>00120 }
+<a name="l00121"></a>00121 
+<a name="l00122"></a>00122 <span class="keyword">inline</span>
+<a name="l00123"></a>00123 <span class="keywordtype">void</span> Transform2D::transform_to_global(Point &amp;p) {
+<a name="l00124"></a>00124         <span class="keywordtype">double</span> t(p.x); 
+<a name="l00125"></a>00125         p.x = base.p.x + c*p.x - s*p.y;
+<a name="l00126"></a>00126         p.y = base.p.y + s*t   + c*p.y;
+<a name="l00127"></a>00127 }
+<a name="l00128"></a>00128 
+<a name="l00129"></a>00129 <span class="keyword">inline</span>
+<a name="l00130"></a>00130 <span class="keywordtype">void</span> Transform2D::transform_to_relative(Pose &amp;p) {
+<a name="l00131"></a>00131         transform_to_relative(p.p);
+<a name="l00132"></a>00132         p.phi= pi_to_pi(p.phi-base.phi);
+<a name="l00133"></a>00133 }
+<a name="l00134"></a>00134 
+<a name="l00135"></a>00135 <span class="keyword">inline</span>
+<a name="l00136"></a>00136 <span class="keywordtype">void</span> Transform2D::transform_to_global(Pose &amp;p) {
+<a name="l00137"></a>00137         transform_to_global(p.p);
+<a name="l00138"></a>00138         p.phi= pi_to_pi(p.phi+base.phi);
+<a name="l00139"></a>00139 }
+<a name="l00140"></a>00140 
+<a name="l00141"></a>00141 } <span class="comment">// namespace Geom2D</span>
+<a name="l00142"></a>00142 
+<a name="l00143"></a>00143 <span class="preprocessor">#endif</span>
+</pre></div><hr size="1"><address style="align: right;"><small>Generated on Tue Aug 8 12:54:32 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/group__MRICP.html b/sm/drivers/playerstage/mricp/doc/html/group__MRICP.html
new file mode 100644
index 0000000000000000000000000000000000000000..de4d1e807cb6fa1ca8b62836f21d636145f6347c
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/group__MRICP.html
@@ -0,0 +1,125 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: Driver</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li><a href="modules.html"><span>Modules</span></a></li>
+    <li><a href="annotated.html"><span>Classes</span></a></li>
+    <li><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<h1>Driver<br>
+<small>
+[<a class="el" href="group__drivers.html">Drivers</a>]</small>
+</h1><table border="0" cellpadding="0" cellspacing="0">
+<tr><td></td></tr>
+</table>
+This driver is created to support UTS CAS mobile Platforms. This driver can be quite usefull in a lot of applications where odom correction or matching laser scans is essential. I would appreciate any feedback and recommendations that can lead to improving the performance of this driver.<p>
+<dl compact><dt><b>Compile-time dependencies</b></dt><dd></dd></dl>
+<ul>
+<li>none</li></ul>
+<p>
+<dl compact><dt><b>Provides</b></dt><dd></dd></dl>
+The MRICP driver provides the following device interfaces, some of them named:<p>
+<ul>
+<li>"Position" player_interface_position<ul>
+<li>This interface returns odometry data.</li></ul>
+</li><li>"Opaque" player_icp_plugin_interface<ul>
+<li>This is a dummy interface supporting requests and commands to the ICP / TODO</li></ul>
+</li><li>"Map" player_map_interface<ul>
+<li>This interface supports map data requests / TODO</li></ul>
+</li></ul>
+<p>
+<dl compact><dt><b>Supported configuration requests</b></dt><dd></dd></dl>
+<ul>
+<li>"Position" player_interface_position:<ul>
+<li>PLAYER_POSITION_SET_ODOM_REQ</li><li>PLAYER_POSITION_RESET_ODOM_REQ</li><li>PLAYER_POSITION_GET_GEOM_REQ</li><li>"Map" player_interface_map:</li><li>PLAYER_POSITION_GET_GEOM_DATA</li></ul>
+</li><li>player_icp_plugin_interface (not clear now, i will know it later)<ul>
+<li>PLAYER_ICP_X_REQ</li><li>PLAYER_ICP_Y_REQ</li></ul>
+</li></ul>
+<p>
+<dl compact><dt><b>Configuration file options</b></dt><dd></dd></dl>
+<ul>
+<li>MAXR (double)<ul>
+<li>Default: "7.8"</li><li>Maximium Laser Range</li></ul>
+</li><li>MINR (double)<ul>
+<li>Default: "0.05"</li><li>Minimium Laser Range</li></ul>
+</li><li>period (double)<ul>
+<li>Default: "0.5"</li><li>Time in sec between scans to be matched.</li></ul>
+</li><li>map_resolution (double)<ul>
+<li>Default: "0.05"</li><li>Pixel resolution in meter of the map to be build</li></ul>
+</li><li>map_size (double)<ul>
+<li>Default: 20</li><li>This is defined from the origin to the boundary, so is actually half the size of the map PATCH, not the whole map.</li></ul>
+</li><li>interpolate (bool)<ul>
+<li>Default "1"</li><li>0 - Simple ICP, 1 - ICP with interpolation</li></ul>
+</li><li>NIT (int)<ul>
+<li>Default "10"</li><li>Number of iterations for each scan-matching.</li></ul>
+</li><li>gate1 (float)<ul>
+<li>Default "0.5"</li><li>1st data association gate for each point in scan</li></ul>
+</li><li>gate2 (float)<ul>
+<li>Default "0.05"</li><li>2nd data association gate for each point in scan</li></ul>
+</li><li>debug (bool)<ul>
+<li>Defult: 0</li><li>Display Debug Messeges</li></ul>
+</li><li>Log (bool)<ul>
+<li>Default: 0</li><li>Loggs the Odom Data (x,y,theta,ltics,rtics,lspeed,rspeed)</li></ul>
+</li><li>map_path(string)<ul>
+<li>Default: "maps/"</li><li>Specifies the locations where patches and logs are to be saved</li></ul>
+</li><li>start_in(int)<ul>
+<li>Default : 2</li><li>Delay Before starting, unit is in seconds</li></ul>
+</li><li>robot_id(int)<ul>
+<li>Default : 0</li><li>The Robot id assigned for map identification</li></ul>
+</li><li>number_of_laser(int)<ul>
+<li>Default : 1</li><li>The number of lasers to be used in the scan matching (index starts from 0) all lasers should be declared in the requires section</li></ul>
+</li><li>playerv_debug (bool)<ul>
+<li>Default : 0</li><li>If enabled, the map occupancy will be represented by +1, 0, -1 (occupied, unoccupied, unknown), other wise, the probability will be scaled from 0 - 255</li></ul>
+</li><li>laserX_ranges tuple of (int) where X is an int<ul>
+<li>Default : [-90 90]</li><li>Determines the acceptable laser scan ranges, even number of elements should exist in the tuple, smaller range should be first followed by the larger range. You will have to manually assign the value of X: eg. in ur configuration file u should have something like this for 2 lasers: number_of_lasers 2 laser0_ranges [-90 -70 -50 -30 -10 90] laser1_ranges [-120 120]<ul>
+<li>this represent the following acceptable ranges: for Laser:0 [-90 -70] [-50 -30] [-10 90] for laser:1 [-120 120]</li></ul>
+</li></ul>
+</li><li>use_max_range (float)<ul>
+<li>Default: 0</li><li>Specify if you want to use ranges more than the max to update empty spaces in Occupancy grid map, if it's not zero , then the range specified will be used to update the cells within that range only (usefull to reduce the effect of false returns)</li></ul>
+</li><li>sparse_scans_rate (int)<ul>
+<li>Default: 1</li><li>Specifies the number of laser scan samples resolution, 1 means all beams, 2 means every take one every 2 and so on. (OG will not be affected by this, all samples will be use for OG) it reduces the CPU usage.</li></ul>
+</li><li>use_odom (bool)<ul>
+<li>Default: 0</li><li>Specify if you want to use the underlying poisition driver's odom in laser scan correction the existance of an underlying position driver no longer means that u are using the odom automatically since it can be used for passing velocity commands.</li></ul>
+</li><li>free_space_prob (float) between 0 and 1<ul>
+<li>Default: 0.4</li><li>The probability to be used for updating the free space , lower values will help in reducing the false readings effect.</li></ul>
+</li><li>map_saving_period (float)<ul>
+<li>Default : 10 sec</li><li>Determines how long time we should wait before saving the map. <dl compact><dt><b>Example </b></dt><dd></dd></dl>
+<div class="fragment"><pre class="fragment">driver
+(
+  name "MRICP_Driver"
+  requires ["position:0" "laser:0"]
+  provides ["position:1" "opaque:0" "map:0"]
+  plugin "MRICP.so"
+  MINR 0.05
+  MAXR 3.9
+  period 1
+  map_resolution 0.6
+  map_path maps/
+  use_max_range 1
+  number_of_lasers 2
+  free_space_prob 0.4
+  sparse_scans_rate 3
+  laser0_ranges [-90 -50 -30 90]
+  laser1_ranges [-120 120]
+  start_in 1
+  interpolate 0
+  use_odom 1
+  robot_id 1
+  NIT 15
+  log 1
+)
+</pre></div></li></ul>
+</li></ul>
+<p>
+<dl compact><dt><b>Authors</b></dt><dd></dd></dl>
+Tarek Taha <hr size="1"><address style="align: right;"><small>Generated on Tue Aug 8 12:42:46 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/group__drivers.html b/sm/drivers/playerstage/mricp/doc/html/group__drivers.html
new file mode 100644
index 0000000000000000000000000000000000000000..701d65747d04c7eed003fd8199f4dade2b67b7a4
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/group__drivers.html
@@ -0,0 +1,25 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: Drivers</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li><a href="modules.html"><span>Modules</span></a></li>
+    <li><a href="annotated.html"><span>Classes</span></a></li>
+    <li><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<h1>Drivers</h1><table border="0" cellpadding="0" cellspacing="0">
+<tr><td></td></tr>
+<tr><td colspan="2"><br><h2>Modules</h2></td></tr>
+<tr><td class="memItemLeft" nowrap align="right" valign="top">&nbsp;</td><td class="memItemRight" valign="bottom"><a class="el" href="group__MRICP.html">Driver</a></td></tr>
+
+</table>
+<hr size="1"><address style="align: right;"><small>Generated on Tue Aug 8 12:42:46 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/gui_8h-source.html b/sm/drivers/playerstage/mricp/doc/html/gui_8h-source.html
new file mode 100644
index 0000000000000000000000000000000000000000..c306533570b0105207940a91144c0226c42258f2
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/gui_8h-source.html
@@ -0,0 +1,188 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: include/gui.h Source File</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li><a href="modules.html"><span>Modules</span></a></li>
+    <li><a href="annotated.html"><span>Classes</span></a></li>
+    <li id="current"><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<h1>include/gui.h</h1><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="preprocessor">#include &lt;assert.h&gt;</span>
+<a name="l00002"></a>00002 <span class="preprocessor">#include &lt;math.h&gt;</span>
+<a name="l00003"></a>00003 <span class="preprocessor">#include &lt;signal.h&gt;</span>
+<a name="l00004"></a>00004 <span class="preprocessor">#include &lt;stdio.h&gt;</span>
+<a name="l00005"></a>00005 <span class="preprocessor">#include &lt;stdlib.h&gt;</span>
+<a name="l00006"></a>00006 <span class="preprocessor">#include &lt;string.h&gt;</span>
+<a name="l00007"></a>00007 <span class="preprocessor">#include &lt;getopt.h&gt;</span>
+<a name="l00008"></a>00008 <span class="preprocessor">#include &lt;unistd.h&gt;</span>
+<a name="l00009"></a>00009 <span class="preprocessor">#include &lt;sys/time.h&gt;</span>
+<a name="l00010"></a>00010 <span class="comment">// GUI settings</span>
+<a name="l00011"></a>00011 <span class="keyword">static</span> <span class="keywordtype">int</span> win,height,width;
+<a name="l00012"></a>00012 <span class="keyword">static</span> <span class="keywordtype">int</span> gui_pause = 0;
+<a name="l00013"></a>00013 <span class="keyword">static</span> <span class="keywordtype">int</span> viewport_width = 0;
+<a name="l00014"></a>00014 <span class="keyword">static</span> <span class="keywordtype">int</span> viewport_height = 0;
+<a name="l00015"></a>00015 <span class="keyword">static</span> <span class="keywordtype">int</span> show_samples = 1;
+<a name="l00016"></a>00016 <span class="keyword">static</span> <span class="keywordtype">double</span> res,offsetx,offsety,mouse_offsetx,mouse_offsety;
+<a name="l00017"></a>00017 <span class="comment">// Mapping phases</span>
+<a name="l00018"></a>00018 
+<a name="l00019"></a>00019 <span class="comment">// Local functions</span>
+<a name="l00020"></a>00020 <span class="keyword">static</span> <span class="keywordtype">void</span> process();
+<a name="l00021"></a>00021 
+<a name="l00022"></a>00022 <span class="comment">// Key callback</span>
+<a name="l00023"></a>00023 <span class="keywordtype">void</span> win_key(<span class="keywordtype">unsigned</span> <span class="keywordtype">char</span> key, <span class="keywordtype">int</span> x, <span class="keywordtype">int</span> y)
+<a name="l00024"></a>00024 {
+<a name="l00025"></a>00025   <span class="comment">// Show the samples</span>
+<a name="l00026"></a>00026   <span class="keywordflow">if</span> (key == <span class="charliteral">'T'</span> || key == <span class="charliteral">'t'</span>)
+<a name="l00027"></a>00027   {
+<a name="l00028"></a>00028     show_samples = !show_samples;
+<a name="l00029"></a>00029     glutPostRedisplay();
+<a name="l00030"></a>00030   }
+<a name="l00031"></a>00031 
+<a name="l00032"></a>00032   <span class="comment">// Pause</span>
+<a name="l00033"></a>00033   <span class="keywordflow">else</span> <span class="keywordflow">if</span> (key == <span class="charliteral">' '</span>)
+<a name="l00034"></a>00034   {
+<a name="l00035"></a>00035     <span class="keywordflow">if</span> (!gui_pause)
+<a name="l00036"></a>00036       fprintf(stderr, <span class="stringliteral">"paused\n"</span>);
+<a name="l00037"></a>00037     <span class="keywordflow">else</span>
+<a name="l00038"></a>00038       fprintf(stderr, <span class="stringliteral">"running\n"</span>);
+<a name="l00039"></a>00039     gui_pause = !gui_pause;
+<a name="l00040"></a>00040   }
+<a name="l00041"></a>00041 
+<a name="l00042"></a>00042   <span class="comment">// Save the current map</span>
+<a name="l00043"></a>00043   <span class="keywordflow">else</span> <span class="keywordflow">if</span> (key == <span class="charliteral">'W'</span> || key == <span class="charliteral">'w'</span>)
+<a name="l00044"></a>00044   {
+<a name="l00045"></a>00045     <span class="comment">//save();</span>
+<a name="l00046"></a>00046   }
+<a name="l00047"></a>00047   
+<a name="l00048"></a>00048   <span class="keywordflow">return</span>;
+<a name="l00049"></a>00049 }
+<a name="l00050"></a>00050 
+<a name="l00051"></a>00051 
+<a name="l00052"></a>00052 
+<a name="l00053"></a>00053 <span class="comment">// Mouse callback</span>
+<a name="l00054"></a>00054 <span class="keywordtype">void</span> win_mouse(<span class="keywordtype">int</span> button, <span class="keywordtype">int</span> state, <span class="keywordtype">int</span> x, <span class="keywordtype">int</span> y)
+<a name="l00055"></a>00055 {
+<a name="l00056"></a>00056   <span class="keywordflow">if</span> (state == GLUT_DOWN)
+<a name="l00057"></a>00057   {
+<a name="l00058"></a>00058     mouse_offsetx = 2 * x * res;
+<a name="l00059"></a>00059     mouse_offsety = 2 * y * res;      
+<a name="l00060"></a>00060   }
+<a name="l00061"></a>00061   <span class="keywordflow">else</span> <span class="keywordflow">if</span> (state == GLUT_UP)
+<a name="l00062"></a>00062   {
+<a name="l00063"></a>00063     offsetx += 2 * x * res - mouse_offsetx;
+<a name="l00064"></a>00064     offsety -= 2 * y * res - mouse_offsety;
+<a name="l00065"></a>00065     glutPostRedisplay();
+<a name="l00066"></a>00066   }
+<a name="l00067"></a>00067 
+<a name="l00068"></a>00068   <span class="keywordflow">return</span>;
+<a name="l00069"></a>00069 }
+<a name="l00070"></a>00070 <span class="comment">// Handle window reshape events</span>
+<a name="l00071"></a>00071 <span class="keywordtype">void</span> win_reshape(<span class="keywordtype">int</span> width, <span class="keywordtype">int</span> height)
+<a name="l00072"></a>00072 {
+<a name="l00073"></a>00073   glViewport(0, 0, width, height);
+<a name="l00074"></a>00074   viewport_width = width;
+<a name="l00075"></a>00075   viewport_height = height;
+<a name="l00076"></a>00076   <span class="keywordflow">return</span>;
+<a name="l00077"></a>00077 }
+<a name="l00078"></a>00078 <span class="comment">// Redraw the window</span>
+<a name="l00079"></a>00079 <span class="keywordtype">void</span> win_redraw()
+<a name="l00080"></a>00080 {
+<a name="l00081"></a>00081   <span class="keywordtype">double</span> left, right, top, bot;
+<a name="l00082"></a>00082   glClearColor(0.7, 0.7, 0.7, 1);
+<a name="l00083"></a>00083   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+<a name="l00084"></a>00084 
+<a name="l00085"></a>00085   left  = -res * width;
+<a name="l00086"></a>00086   right = +res * width;
+<a name="l00087"></a>00087   top   = -res * height;
+<a name="l00088"></a>00088   bot   = +res * height;
+<a name="l00089"></a>00089 
+<a name="l00090"></a>00090   glMatrixMode(GL_PROJECTION);
+<a name="l00091"></a>00091   glLoadIdentity();
+<a name="l00092"></a>00092   glOrtho(left, right, top, bot, -1, +10);
+<a name="l00093"></a>00093 
+<a name="l00094"></a>00094   glMatrixMode(GL_MODELVIEW);
+<a name="l00095"></a>00095   glLoadIdentity();
+<a name="l00096"></a>00096   glTranslatef(offsetx,offsety, 0.0);
+<a name="l00097"></a>00097 
+<a name="l00098"></a>00098   <span class="comment">// Draw the grid map</span>
+<a name="l00099"></a>00099   <span class="comment">// pmap_draw_map(pmap, opt_scale);</span>
+<a name="l00100"></a>00100 
+<a name="l00101"></a>00101   <span class="comment">// Draw the origin marker</span>
+<a name="l00102"></a>00102   glColor3f(0, 0, 0);
+<a name="l00103"></a>00103   glBegin(GL_LINE_LOOP);
+<a name="l00104"></a>00104   glVertex3f(-1, -1, 0);
+<a name="l00105"></a>00105   glVertex3f(+1, -1, 0);
+<a name="l00106"></a>00106   glVertex3f(+1, +1, 0);
+<a name="l00107"></a>00107   glVertex3f(-1, +1, 0);
+<a name="l00108"></a>00108   glEnd();
+<a name="l00109"></a>00109   <span class="comment">// draw the map</span>
+<a name="l00110"></a>00110   glutSwapBuffers();
+<a name="l00111"></a>00111   <span class="keywordflow">return</span>;
+<a name="l00112"></a>00112 }
+<a name="l00113"></a>00113 
+<a name="l00114"></a>00114 
+<a name="l00115"></a>00115 <span class="comment">// Idle callback</span>
+<a name="l00116"></a>00116 <span class="keywordtype">void</span> win_idle()
+<a name="l00117"></a>00117 {
+<a name="l00118"></a>00118   <span class="keywordflow">if</span> (!gui_pause)
+<a name="l00119"></a>00119   {
+<a name="l00120"></a>00120     process();
+<a name="l00121"></a>00121     glutPostRedisplay();
+<a name="l00122"></a>00122   }
+<a name="l00123"></a>00123   <span class="keywordflow">else</span>
+<a name="l00124"></a>00124     usleep(100000);
+<a name="l00125"></a>00125   <span class="keywordflow">return</span>;
+<a name="l00126"></a>00126 }
+<a name="l00127"></a>00127 <span class="comment">// Trap SIGINTS</span>
+<a name="l00128"></a>00128 <span class="keywordtype">void</span> signal_handle(<span class="keywordtype">int</span> arg)
+<a name="l00129"></a>00129 {
+<a name="l00130"></a>00130   <span class="comment">// Save current state</span>
+<a name="l00131"></a>00131 
+<a name="l00132"></a>00132   <span class="comment">// Move on to fine phase or exit</span>
+<a name="l00133"></a>00133   
+<a name="l00134"></a>00134   <span class="keywordflow">return</span>;
+<a name="l00135"></a>00135 };
+<a name="l00136"></a>00136 <span class="comment">// Run the GUI</span>
+<a name="l00137"></a>00137 <span class="keywordtype">int</span> win_run(<span class="keywordtype">int</span> w, <span class="keywordtype">int</span> h , <span class="keywordtype">double</span> resolution)
+<a name="l00138"></a>00138 {
+<a name="l00139"></a>00139   width = w;
+<a name="l00140"></a>00140   height = h;
+<a name="l00141"></a>00141   res = resolution;
+<a name="l00142"></a>00142   <span class="comment">// Register signal handlers</span>
+<a name="l00143"></a>00143   assert(signal(SIGINT, signal_handle) != SIG_ERR);
+<a name="l00144"></a>00144   <span class="keywordtype">int</span> argc;
+<a name="l00145"></a>00145   <span class="keywordtype">char</span> **argv;
+<a name="l00146"></a>00146   printf(<span class="stringliteral">"\n AM HERE 1"</span>); fflush(stdout);
+<a name="l00147"></a>00147   glutInit(&amp;argc, argv);
+<a name="l00148"></a>00148   printf(<span class="stringliteral">"\n AM HERE 2"</span>); fflush(stdout);
+<a name="l00149"></a>00149   <span class="comment">// Create a window</span>
+<a name="l00150"></a>00150   glutInitDisplayMode(GLUT_RGBA | GLUT_DEPTH | GLUT_DOUBLE);
+<a name="l00151"></a>00151   glutInitWindowSize( w + 16, h + 16);
+<a name="l00152"></a>00152   win = glutCreateWindow(<span class="stringliteral">"Map Building Viewer"</span>);
+<a name="l00153"></a>00153   glutReshapeFunc(win_reshape);
+<a name="l00154"></a>00154   glutDisplayFunc(win_redraw);
+<a name="l00155"></a>00155   glutKeyboardFunc(win_key);
+<a name="l00156"></a>00156   glutMouseFunc(win_mouse);
+<a name="l00157"></a>00157   <span class="comment">// Idle loop callback</span>
+<a name="l00158"></a>00158   glutIdleFunc(win_idle);
+<a name="l00159"></a>00159   glutMainLoop();
+<a name="l00160"></a>00160   <span class="keywordflow">return</span> 0;
+<a name="l00161"></a>00161 }
+<a name="l00162"></a>00162 <span class="comment">// Do some appropriate form of processing</span>
+<a name="l00163"></a>00163 <span class="keywordtype">void</span> process()
+<a name="l00164"></a>00164 {
+<a name="l00165"></a>00165   cout&lt;&lt;<span class="stringliteral">"\nUpdating"</span>;
+<a name="l00166"></a>00166   <span class="keywordflow">return</span>;
+<a name="l00167"></a>00167 }
+<a name="l00168"></a>00168 
+<a name="l00169"></a>00169 
+</pre></div><hr size="1"><address style="align: right;"><small>Generated on Tue Aug 1 16:54:04 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/hierarchy.html b/sm/drivers/playerstage/mricp/doc/html/hierarchy.html
new file mode 100644
index 0000000000000000000000000000000000000000..1236a8c2a6bcb41bce2cdcc337c8dc7b475971af
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/hierarchy.html
@@ -0,0 +1,27 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: Hierarchical Index</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li><a href="modules.html"><span>Modules</span></a></li>
+    <li id="current"><a href="annotated.html"><span>Classes</span></a></li>
+    <li><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<div class="tabs">
+  <ul>
+    <li><a href="annotated.html"><span>Class&nbsp;List</span></a></li>
+    <li id="current"><a href="hierarchy.html"><span>Class&nbsp;Hierarchy</span></a></li>
+  </ul></div>
+<h1>MapReferenceICP Class Hierarchy</h1>This inheritance list is sorted roughly, but not completely, alphabetically:<ul>
+<li><a class="el" href="structlaser__range.html">laser_range</a>
+</ul>
+<hr size="1"><address style="align: right;"><small>Generated on Tue Aug 8 12:54:32 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/icp_8h-source.html b/sm/drivers/playerstage/mricp/doc/html/icp_8h-source.html
new file mode 100644
index 0000000000000000000000000000000000000000..8e86e8a4f091a6a10628d8befc52ad03c724a4bb
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/icp_8h-source.html
@@ -0,0 +1,72 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: include/icp.h Source File</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li><a href="modules.html"><span>Modules</span></a></li>
+    <li><a href="annotated.html"><span>Classes</span></a></li>
+    <li id="current"><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<h1>include/icp.h</h1><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/* Iterated closest point (ICP) algorithm.</span>
+<a name="l00002"></a>00002 <span class="comment"> * </span>
+<a name="l00003"></a>00003 <span class="comment"> * This is a very simple implementation of ICP, with one simple extension where the</span>
+<a name="l00004"></a>00004 <span class="comment"> * association may be chosen as an interpolation between two nearest neighbours rather</span>
+<a name="l00005"></a>00005 <span class="comment"> * than just point-to-point with the nearest neighbour.</span>
+<a name="l00006"></a>00006 <span class="comment"> *</span>
+<a name="l00007"></a>00007 <span class="comment"> * A lot of extensions to this basic algorithm are possible. For example,</span>
+<a name="l00008"></a>00008 <span class="comment"> *      1. Record at each iteration the set of NN associations for each observation. </span>
+<a name="l00009"></a>00009 <span class="comment"> *      If these do not change, then ICP has converged completely.</span>
+<a name="l00010"></a>00010 <span class="comment"> *      </span>
+<a name="l00011"></a>00011 <span class="comment"> *      2. Various speed ups given in the following papers:</span>
+<a name="l00012"></a>00012 <span class="comment"> *</span>
+<a name="l00013"></a>00013 <span class="comment"> *      (a) P.J. Besl and N.D. McKay. A method for registration of 3-D shapes. IEEE </span>
+<a name="l00014"></a>00014 <span class="comment"> *      Transactions on Pattern Analysis and Machine Intelligence, 14(2):239256, 1992.</span>
+<a name="l00015"></a>00015 <span class="comment"> *</span>
+<a name="l00016"></a>00016 <span class="comment"> *      (b) F. Lu and E. Milios. Robot pose estimation in unknown environments by matching</span>
+<a name="l00017"></a>00017 <span class="comment"> *      2D range scans. Journal of Intelligent and Robotic Systems, 18:249275, 1997.</span>
+<a name="l00018"></a>00018 <span class="comment"> *</span>
+<a name="l00019"></a>00019 <span class="comment"> *      (c) S. Rusinkiewicz and M. Levoy. Efficient variants of the ICP algorithm. In Third</span>
+<a name="l00020"></a>00020 <span class="comment"> *      International Conference on 3D Digital Imaging and Modeling, pages 145152, 2001.</span>
+<a name="l00021"></a>00021 <span class="comment"> *</span>
+<a name="l00022"></a>00022 <span class="comment"> *      3. Methods for estimating the error introduced by point-wise correspondence in </span>
+<a name="l00023"></a>00023 <span class="comment"> *      the paper (b) above and also:</span>
+<a name="l00024"></a>00024 <span class="comment"> *</span>
+<a name="l00025"></a>00025 <span class="comment"> *      S.T. P?ster, K.L. Kriechbaum, S.I. Roumeliotis, and J.W. Burdick. Weighted range</span>
+<a name="l00026"></a>00026 <span class="comment"> *      sensor matching algorithms for mobile robot displacement estimation. In IEEE </span>
+<a name="l00027"></a>00027 <span class="comment"> *      International Conference on Robotics and Automation, 2002.</span>
+<a name="l00028"></a>00028 <span class="comment"> *</span>
+<a name="l00029"></a>00029 <span class="comment"> * Tim Bailey 2004.</span>
+<a name="l00030"></a>00030 <span class="comment"> */</span>
+<a name="l00031"></a>00031 <span class="preprocessor">#include &lt;vector&gt;</span>
+<a name="l00032"></a>00032 <span class="preprocessor">#include "geometry2D.h"</span>
+<a name="l00033"></a>00033 <span class="preprocessor">#include "nn.h"</span>
+<a name="l00034"></a>00034 <span class="preprocessor">#include &lt;memory&gt;</span>
+<a name="l00035"></a>00035 <span class="keyword">using namespace </span>std;
+<a name="l00036"></a>00036 <span class="keyword">namespace </span>Geom2D {
+<a name="l00037"></a>00037 
+<a name="l00038"></a>00038 <span class="keyword">class </span>ICP {
+<a name="l00039"></a>00039 <span class="keyword">public</span>:
+<a name="l00040"></a>00040         ICP();
+<a name="l00041"></a>00041         ~ICP();
+<a name="l00042"></a>00042         Pose align(std::vector&lt;Point&gt; , std::vector&lt;Point&gt;,Pose , <span class="keywordtype">double</span> , <span class="keywordtype">int</span> , <span class="keywordtype">bool</span> );
+<a name="l00043"></a>00043         <span class="keyword">const</span> std::vector&lt;Point&gt; get_ref_points() { <span class="keywordflow">return</span> b; }
+<a name="l00044"></a>00044         <span class="keyword">const</span> std::vector&lt;Point&gt; get_obs_points() { <span class="keywordflow">return</span> a; }
+<a name="l00045"></a>00045 <span class="keyword">private</span>:
+<a name="l00046"></a>00046         vector&lt;Point&gt;   ref;
+<a name="l00047"></a>00047         SweepSearch * nn;
+<a name="l00048"></a>00048         vector&lt;Point&gt; a;
+<a name="l00049"></a>00049         vector&lt;Point&gt; b;
+<a name="l00050"></a>00050         vector&lt;int&gt; index;
+<a name="l00051"></a>00051 };
+<a name="l00052"></a>00052 
+<a name="l00053"></a>00053 } <span class="comment">// namespace Geom2D </span>
+</pre></div><hr size="1"><address style="align: right;"><small>Generated on Tue Aug 8 12:54:32 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/index.html b/sm/drivers/playerstage/mricp/doc/html/index.html
new file mode 100644
index 0000000000000000000000000000000000000000..4d910d56ad2e1d305c1d6824a479b814a1b52926
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/index.html
@@ -0,0 +1,21 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: Main Page</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li id="current"><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li><a href="modules.html"><span>Modules</span></a></li>
+    <li><a href="annotated.html"><span>Classes</span></a></li>
+    <li><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<h1>MapReferenceICP Documentation</h1>
+<p>
+<h3 align="center">$VERSION$ </h3><hr size="1"><address style="align: right;"><small>Generated on Tue Aug 8 12:54:32 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/lasermodel_8h-source.html b/sm/drivers/playerstage/mricp/doc/html/lasermodel_8h-source.html
new file mode 100644
index 0000000000000000000000000000000000000000..d19a93637cad9b2b2979e6d76b12b7661cfb27e0
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/lasermodel_8h-source.html
@@ -0,0 +1,50 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: include/lasermodel.h Source File</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li><a href="modules.html"><span>Modules</span></a></li>
+    <li><a href="annotated.html"><span>Classes</span></a></li>
+    <li id="current"><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<h1>include/lasermodel.h</h1><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="preprocessor">#ifndef LASERMODEL_H_</span>
+<a name="l00002"></a>00002 <span class="preprocessor"></span><span class="preprocessor">#define LASERMODEL_H_</span>
+<a name="l00003"></a>00003 <span class="preprocessor"></span><span class="comment">// Info for a single range measurement</span>
+<a name="l00004"></a>00004 <span class="keyword">typedef</span> <span class="keyword">struct</span>
+<a name="l00005"></a>00005 <span class="keyword"></span>{
+<a name="l00006"></a>00006         <span class="keywordtype">double</span> range, bearing;
+<a name="l00007"></a>00007 }       laser_range_t;
+<a name="l00008"></a>00008 
+<a name="l00009"></a>00009 <span class="keyword">class </span>LaserModel
+<a name="l00010"></a>00010 {
+<a name="l00011"></a>00011         <span class="keyword">private</span>:
+<a name="l00012"></a>00012                 mapgrid * * map;        <span class="comment">// Pointer to the OG map</span>
+<a name="l00013"></a>00013                 <span class="keywordtype">double</span> range_cov;       <span class="comment">// Covariance in the range reading</span>
+<a name="l00014"></a>00014                 <span class="keywordtype">double</span> range_bad;       <span class="comment">// Probability of spurious range readings</span>
+<a name="l00015"></a>00015                 <span class="comment">// Pre-computed laser sensor model</span>
+<a name="l00016"></a>00016                 <span class="keywordtype">int</span> lut_size;
+<a name="l00017"></a>00017                 <span class="keywordtype">double</span>  lut_res;
+<a name="l00018"></a>00018                 <span class="keywordtype">double</span> *lut_probs;
+<a name="l00019"></a>00019                 <span class="keywordtype">int</span> range_count;
+<a name="l00020"></a>00020                 laser_range_t *ranges;
+<a name="l00021"></a>00021         <span class="keyword">public</span> :
+<a name="l00022"></a>00022                 <span class="keywordtype">void</span>    ClearRanges();
+<a name="l00023"></a>00023                 <span class="keywordtype">void</span>    AddRange(<span class="keywordtype">double</span>,<span class="keywordtype">double</span>);
+<a name="l00024"></a>00024                 <span class="keywordtype">void</span>    PreCompute();
+<a name="l00025"></a>00025                 <span class="keywordtype">double</span>  RangeProb(<span class="keywordtype">double</span>,<span class="keywordtype">double</span>);
+<a name="l00026"></a>00026                 <span class="keywordtype">double</span>  PoseProb();
+<a name="l00027"></a>00027                                 LaserModel();
+<a name="l00028"></a>00028                                 ~LaserModel();
+<a name="l00029"></a>00029                                 LaserModel(mapgrid * * );
+<a name="l00030"></a>00030 };
+<a name="l00031"></a>00031 <span class="preprocessor">#endif </span><span class="comment">/*LASERMODEL_H_*/</span>
+</pre></div><hr size="1"><address style="align: right;"><small>Generated on Tue Aug 8 12:54:32 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/map_8h-source.html b/sm/drivers/playerstage/mricp/doc/html/map_8h-source.html
new file mode 100644
index 0000000000000000000000000000000000000000..bc2d74fa590bc829f5509b8f49490358a2da5347
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/map_8h-source.html
@@ -0,0 +1,79 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: include/map.h Source File</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li><a href="modules.html"><span>Modules</span></a></li>
+    <li><a href="annotated.html"><span>Classes</span></a></li>
+    <li id="current"><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<h1>include/map.h</h1><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/***************************************************************************</span>
+<a name="l00002"></a>00002 <span class="comment"> *   Copyright (C) 2005 by Tarek Taha                                      *</span>
+<a name="l00003"></a>00003 <span class="comment"> *   tataha@eng.uts.edu.au                                                 *</span>
+<a name="l00004"></a>00004 <span class="comment"> *                                                                         *</span>
+<a name="l00005"></a>00005 <span class="comment"> *   This program is free software; you can redistribute it and/or modify  *</span>
+<a name="l00006"></a>00006 <span class="comment"> *   it under the terms of the GNU General Public License as published by  *</span>
+<a name="l00007"></a>00007 <span class="comment"> *   the Free Software Foundation; either version 2 of the License, or     *</span>
+<a name="l00008"></a>00008 <span class="comment"> *   (at your option) any later version.                                   *</span>
+<a name="l00009"></a>00009 <span class="comment"> *                                                                         *</span>
+<a name="l00010"></a>00010 <span class="comment"> *   This program is distributed in the hope that it will be useful,       *</span>
+<a name="l00011"></a>00011 <span class="comment"> *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *</span>
+<a name="l00012"></a>00012 <span class="comment"> *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *</span>
+<a name="l00013"></a>00013 <span class="comment"> *   GNU General Public License for more details.                          *</span>
+<a name="l00014"></a>00014 <span class="comment"> *                                                                         *</span>
+<a name="l00015"></a>00015 <span class="comment"> *   You should have received a copy of the GNU General Public License     *</span>
+<a name="l00016"></a>00016 <span class="comment"> *   along with this program; if not, write to the                         *</span>
+<a name="l00017"></a>00017 <span class="comment"> *   Free Software Foundation, Inc.,                                       *</span>
+<a name="l00018"></a>00018 <span class="comment"> *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *</span>
+<a name="l00019"></a>00019 <span class="comment"> ***************************************************************************/</span>
+<a name="l00020"></a>00020 <span class="preprocessor">#ifndef map_HEADER</span>
+<a name="l00021"></a>00021 <span class="preprocessor"></span><span class="preprocessor">#define map_HEADER</span>
+<a name="l00022"></a>00022 <span class="preprocessor"></span>
+<a name="l00023"></a>00023 <span class="preprocessor">#include &lt;gdk-pixbuf/gdk-pixbuf.h&gt;</span>
+<a name="l00024"></a>00024 <span class="preprocessor">#include &lt;gtk/gtk.h&gt;</span>
+<a name="l00025"></a>00025 <span class="preprocessor">#include &lt;glib/gprintf.h&gt;</span>
+<a name="l00026"></a>00026 
+<a name="l00027"></a>00027 <span class="preprocessor">#include &lt;sys/stat.h&gt;</span>
+<a name="l00028"></a>00028 <span class="keyword">class </span>MapInfo
+<a name="l00029"></a>00029 {
+<a name="l00030"></a>00030 <span class="keyword">public</span> :
+<a name="l00031"></a>00031         <span class="keywordtype">int</span> height;
+<a name="l00032"></a>00032         <span class="keywordtype">int</span> width;
+<a name="l00033"></a>00033         <span class="keywordtype">double</span> pixel_size;
+<a name="l00034"></a>00034 };
+<a name="l00035"></a>00035 <span class="keyword">typedef</span> <span class="keyword">struct </span>mapgrid
+<a name="l00036"></a>00036 {
+<a name="l00037"></a>00037         <span class="keywordtype">double</span> prob_occ,prob_free;
+<a name="l00038"></a>00038         <span class="keywordtype">bool</span> added;
+<a name="l00039"></a>00039 } mapgrid_t;
+<a name="l00040"></a>00040 <span class="keyword">class </span>MAP
+<a name="l00041"></a>00041 {
+<a name="l00042"></a>00042         <span class="keyword">private</span>:
+<a name="l00043"></a>00043                 <span class="keywordtype">double</span> pixel_size;
+<a name="l00044"></a>00044                 <span class="keywordtype">int</span> negate;
+<a name="l00045"></a>00045                         GdkPixbuf * pixbuf;
+<a name="l00046"></a>00046         <span class="keyword">public</span>:
+<a name="l00047"></a>00047                         <span class="keywordtype">int</span> size_x, size_y;
+<a name="l00048"></a>00048                         mapgrid * * occ_grid;
+<a name="l00049"></a>00049                 <span class="keywordtype">char</span> * Mapname;
+<a name="l00050"></a>00050                         MapInfo    GetMapInfo();
+<a name="l00051"></a>00051                         <span class="keywordtype">void</span> SavePixelBufferToFile();
+<a name="l00052"></a>00052                         <span class="keywordtype">void</span> DrawPixel(<span class="keywordtype">int</span>,<span class="keywordtype">int</span>,<span class="keywordtype">int</span>,<span class="keywordtype">int</span>,<span class="keywordtype">int</span>);
+<a name="l00053"></a>00053                         <span class="keywordtype">void</span> ClearData();
+<a name="l00054"></a>00054                         <span class="keywordtype">void</span> ResetProb();
+<a name="l00055"></a>00055                         <span class="keywordtype">int</span>  SavePgm();
+<a name="l00056"></a>00056                         GdkPixbuf * CreateMap();
+<a name="l00057"></a>00057                         ~MAP();
+<a name="l00058"></a>00058                         MAP(<span class="keywordtype">char</span>*, <span class="keywordtype">double</span>,<span class="keywordtype">double</span>);
+<a name="l00059"></a>00059 };
+<a name="l00060"></a>00060 <span class="preprocessor">#endif</span>
+</pre></div><hr size="1"><address style="align: right;"><small>Generated on Tue Aug 8 12:54:32 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/modules.html b/sm/drivers/playerstage/mricp/doc/html/modules.html
new file mode 100644
index 0000000000000000000000000000000000000000..be1cbb716ec6df3969d48275645f651f34266190
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/modules.html
@@ -0,0 +1,22 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: Module Index</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li id="current"><a href="modules.html"><span>Modules</span></a></li>
+    <li><a href="annotated.html"><span>Classes</span></a></li>
+    <li><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<h1>MapReferenceICP Modules</h1>Here is a list of all modules:<ul>
+<li><a class="el" href="group__ComponentNavigator.html">MRICP</a>
+</ul>
+<hr size="1"><address style="align: right;"><small>Generated on Tue Aug 8 12:54:32 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/nn_8h-source.html b/sm/drivers/playerstage/mricp/doc/html/nn_8h-source.html
new file mode 100644
index 0000000000000000000000000000000000000000..6929694eed64910375cb42142a41967182a580e1
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/nn_8h-source.html
@@ -0,0 +1,72 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: include/nn.h Source File</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li><a href="modules.html"><span>Modules</span></a></li>
+    <li><a href="annotated.html"><span>Classes</span></a></li>
+    <li id="current"><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<h1>include/nn.h</h1><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/* Nearest-neighbours algorithm for 2D point-sets.</span>
+<a name="l00002"></a>00002 <span class="comment"> * Compute the single nearest-neighbour of a point, or the set of k-nearest neighbours.</span>
+<a name="l00003"></a>00003 <span class="comment"> *</span>
+<a name="l00004"></a>00004 <span class="comment"> * This is a very simple algorithm that is reasonably efficient for planar</span>
+<a name="l00005"></a>00005 <span class="comment"> * points. However much faster algorithms are possible.</span>
+<a name="l00006"></a>00006 <span class="comment"> *</span>
+<a name="l00007"></a>00007 <span class="comment"> * Tim Bailey 2004.</span>
+<a name="l00008"></a>00008 <span class="comment"> */</span>
+<a name="l00009"></a>00009 
+<a name="l00010"></a>00010 <span class="preprocessor">#ifndef NN_HEADER</span>
+<a name="l00011"></a>00011 <span class="preprocessor"></span><span class="preprocessor">#define NN_HEADER</span>
+<a name="l00012"></a>00012 <span class="preprocessor"></span>
+<a name="l00013"></a>00013 <span class="preprocessor">#include "geometry2D.h"</span>
+<a name="l00014"></a>00014 <span class="preprocessor">#include &lt;vector&gt;</span>
+<a name="l00015"></a>00015 
+<a name="l00016"></a>00016 <span class="keyword">namespace </span>Geom2D {
+<a name="l00017"></a>00017 
+<a name="l00018"></a>00018 <span class="comment">// Simple 2D k-nearest-neighbours search</span>
+<a name="l00019"></a>00019 <span class="comment">//</span>
+<a name="l00020"></a>00020 <span class="keyword">class </span>SweepSearch {
+<a name="l00021"></a>00021 <span class="keyword">public</span>:
+<a name="l00022"></a>00022         <span class="keyword">enum</span> { NOT_FOUND = -1 };
+<a name="l00023"></a>00023 
+<a name="l00024"></a>00024         SweepSearch(<span class="keyword">const</span> std::vector&lt;Point&gt; &amp;p, <span class="keywordtype">double</span> dmax);
+<a name="l00025"></a>00025 
+<a name="l00026"></a>00026         <span class="keywordtype">int</span> query(<span class="keyword">const</span> Point &amp;q) <span class="keyword">const</span>;
+<a name="l00027"></a>00027         std::vector&lt;double&gt;&amp; query(<span class="keyword">const</span> Point &amp;q, std::vector&lt;int&gt; &amp;idx);
+<a name="l00028"></a>00028 
+<a name="l00029"></a>00029 <span class="keyword">private</span>:        
+<a name="l00030"></a>00030         <span class="keyword">struct </span>PointIdx { 
+<a name="l00031"></a>00031                 PointIdx() {}
+<a name="l00032"></a>00032                 PointIdx(<span class="keyword">const</span> Point &amp;p_, <span class="keyword">const</span> <span class="keywordtype">int</span>&amp; i_) : p(p_), i(i_) {}
+<a name="l00033"></a>00033                 Point p;
+<a name="l00034"></a>00034                 <span class="keywordtype">int</span> i; 
+<a name="l00035"></a>00035         };
+<a name="l00036"></a>00036 
+<a name="l00037"></a>00037         <span class="keyword">const</span> <span class="keywordtype">double</span> limit;
+<a name="l00038"></a>00038         std::vector&lt;PointIdx&gt; dataset;
+<a name="l00039"></a>00039         std::vector&lt;double&gt; nndists;
+<a name="l00040"></a>00040 
+<a name="l00041"></a>00041         <span class="keywordtype">bool</span> is_nearer(<span class="keywordtype">double</span> &amp;d2min, <span class="keywordtype">int</span> &amp;idxmin, <span class="keyword">const</span> Point &amp;q, <span class="keyword">const</span> PointIdx &amp;pi) <span class="keyword">const</span>;
+<a name="l00042"></a>00042 
+<a name="l00043"></a>00043         <span class="keywordtype">bool</span> insert_neighbour(<span class="keyword">const</span> Point &amp;q, <span class="keyword">const</span> PointIdx &amp;pi, 
+<a name="l00044"></a>00044                 std::vector&lt;double&gt; &amp;nndists, std::vector&lt;int&gt; &amp;idx);
+<a name="l00045"></a>00045 
+<a name="l00046"></a>00046         <span class="keyword">static</span> <span class="keywordtype">bool</span> yorder (<span class="keyword">const</span> PointIdx&amp; p, <span class="keyword">const</span> PointIdx&amp; q) {
+<a name="l00047"></a>00047                 <span class="keywordflow">return</span> p.p.y &lt; q.p.y;
+<a name="l00048"></a>00048         }
+<a name="l00049"></a>00049 };
+<a name="l00050"></a>00050 
+<a name="l00051"></a>00051 }
+<a name="l00052"></a>00052 
+<a name="l00053"></a>00053 <span class="preprocessor">#endif</span>
+</pre></div><hr size="1"><address style="align: right;"><small>Generated on Tue Aug 8 12:54:32 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/structlaser__range-members.html b/sm/drivers/playerstage/mricp/doc/html/structlaser__range-members.html
new file mode 100644
index 0000000000000000000000000000000000000000..d3db508b6f3bf83732da8af7f9adfb5ee0d8a1b6
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/structlaser__range-members.html
@@ -0,0 +1,27 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: Member List</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li><a href="modules.html"><span>Modules</span></a></li>
+    <li id="current"><a href="annotated.html"><span>Classes</span></a></li>
+    <li><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<div class="tabs">
+  <ul>
+    <li><a href="annotated.html"><span>Class&nbsp;List</span></a></li>
+    <li><a href="hierarchy.html"><span>Class&nbsp;Hierarchy</span></a></li>
+  </ul></div>
+<h1>laser_range Member List</h1>This is the complete list of members for <a class="el" href="structlaser__range.html">laser_range</a>, including all inherited members.<p><table>
+  <tr bgcolor="#f0f0f0"><td><b>begins</b> (defined in <a class="el" href="structlaser__range.html">laser_range</a>)</td><td><a class="el" href="structlaser__range.html">laser_range</a></td><td></td></tr>
+  <tr bgcolor="#f0f0f0"><td><b>ends</b> (defined in <a class="el" href="structlaser__range.html">laser_range</a>)</td><td><a class="el" href="structlaser__range.html">laser_range</a></td><td></td></tr>
+</table><hr size="1"><address style="align: right;"><small>Generated on Tue Aug 8 12:54:32 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/structlaser__range.html b/sm/drivers/playerstage/mricp/doc/html/structlaser__range.html
new file mode 100644
index 0000000000000000000000000000000000000000..3c431f3467b0f158831d2a470d02f5c7558aa292
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/structlaser__range.html
@@ -0,0 +1,42 @@
+<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
+<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
+<title>MapReferenceICP: laser_range Struct Reference</title>
+<link href="doxygen.css" rel="stylesheet" type="text/css">
+<link href="tabs.css" rel="stylesheet" type="text/css">
+</head><body>
+<!-- Generated by Doxygen 1.4.6 -->
+<div class="tabs">
+  <ul>
+    <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
+    <li><a href="modules.html"><span>Modules</span></a></li>
+    <li id="current"><a href="annotated.html"><span>Classes</span></a></li>
+    <li><a href="files.html"><span>Files</span></a></li>
+  </ul></div>
+<div class="tabs">
+  <ul>
+    <li><a href="annotated.html"><span>Class&nbsp;List</span></a></li>
+    <li><a href="hierarchy.html"><span>Class&nbsp;Hierarchy</span></a></li>
+  </ul></div>
+<h1>laser_range Struct Reference</h1><!-- doxytag: class="laser_range" -->MRICP DRIVER Class ///.  
+<a href="#_details">More...</a>
+<p>
+<a href="structlaser__range-members.html">List of all members.</a><table border="0" cellpadding="0" cellspacing="0">
+<tr><td></td></tr>
+<tr><td colspan="2"><br><h2>Public Attributes</h2></td></tr>
+<tr><td class="memItemLeft" nowrap align="right" valign="top"><a class="anchor" name="6d19c28cf50bc01258b27a26e8550905"></a><!-- doxytag: member="laser_range::begins" ref="6d19c28cf50bc01258b27a26e8550905" args="" -->
+int&nbsp;</td><td class="memItemRight" valign="bottom"><b>begins</b></td></tr>
+
+<tr><td class="memItemLeft" nowrap align="right" valign="top"><a class="anchor" name="ab835a735606458fa850e3bf9fbaf81c"></a><!-- doxytag: member="laser_range::ends" ref="ab835a735606458fa850e3bf9fbaf81c" args="" -->
+int&nbsp;</td><td class="memItemRight" valign="bottom"><b>ends</b></td></tr>
+
+</table>
+<hr><a name="_details"></a><h2>Detailed Description</h2>
+MRICP DRIVER Class ///. 
+<p>
+<hr>The documentation for this struct was generated from the following file:<ul>
+<li>src/mricp_driver.cpp</ul>
+<hr size="1"><address style="align: right;"><small>Generated on Tue Aug 8 12:54:32 2006 for MapReferenceICP by&nbsp;
+<a href="http://www.doxygen.org/index.html">
+<img src="doxygen.png" alt="doxygen" align="middle" border="0"></a> 1.4.6 </small></address>
+</body>
+</html>
diff --git a/sm/drivers/playerstage/mricp/doc/html/tab_b.gif b/sm/drivers/playerstage/mricp/doc/html/tab_b.gif
new file mode 100644
index 0000000000000000000000000000000000000000..0d623483ffdf5f9f96900108042a7ab0643fe2a3
Binary files /dev/null and b/sm/drivers/playerstage/mricp/doc/html/tab_b.gif differ
diff --git a/sm/drivers/playerstage/mricp/doc/html/tab_l.gif b/sm/drivers/playerstage/mricp/doc/html/tab_l.gif
new file mode 100644
index 0000000000000000000000000000000000000000..9b1e6337c9299a700401a2a78a2c6ffced475216
Binary files /dev/null and b/sm/drivers/playerstage/mricp/doc/html/tab_l.gif differ
diff --git a/sm/drivers/playerstage/mricp/doc/html/tab_r.gif b/sm/drivers/playerstage/mricp/doc/html/tab_r.gif
new file mode 100644
index 0000000000000000000000000000000000000000..ce9dd9f533cb5486d6941844f442b59d4a9e9175
Binary files /dev/null and b/sm/drivers/playerstage/mricp/doc/html/tab_r.gif differ
diff --git a/sm/drivers/playerstage/mricp/doc/html/tabs.css b/sm/drivers/playerstage/mricp/doc/html/tabs.css
new file mode 100644
index 0000000000000000000000000000000000000000..a61552a67ad217c0d67da13e4d3f24c9d2c87119
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doc/html/tabs.css
@@ -0,0 +1,102 @@
+/* tabs styles, based on http://www.alistapart.com/articles/slidingdoors */
+
+DIV.tabs
+{
+   float            : left;
+   width            : 100%;
+   background       : url("tab_b.gif") repeat-x bottom;
+   margin-bottom    : 4px;
+}
+
+DIV.tabs UL
+{
+   margin           : 0px;
+   padding-left     : 10px;
+   list-style       : none;
+}
+
+DIV.tabs LI, DIV.tabs FORM
+{
+   display          : inline;
+   margin           : 0px;
+   padding          : 0px;
+}
+
+DIV.tabs FORM
+{
+   float            : right;
+}
+
+DIV.tabs A
+{
+   float            : left;
+   background       : url("tab_r.gif") no-repeat right top;
+   border-bottom    : 1px solid #84B0C7;
+   font-size        : x-small;
+   font-weight      : bold;
+   text-decoration  : none;
+}
+
+DIV.tabs A:hover
+{
+   background-position: 100% -150px;
+}
+
+DIV.tabs A:link, DIV.tabs A:visited,
+DIV.tabs A:active, DIV.tabs A:hover
+{
+       color: #1A419D;
+}
+
+DIV.tabs SPAN
+{
+   float            : left;
+   display          : block;
+   background       : url("tab_l.gif") no-repeat left top;
+   padding          : 5px 9px;
+   white-space      : nowrap;
+}
+
+DIV.tabs INPUT
+{
+   float            : right;
+   display          : inline;
+   font-size        : 1em;
+}
+
+DIV.tabs TD
+{
+   font-size        : x-small;
+   font-weight      : bold;
+   text-decoration  : none;
+}
+
+
+
+/* Commented Backslash Hack hides rule from IE5-Mac \*/
+DIV.tabs SPAN {float : none;}
+/* End IE5-Mac hack */
+
+DIV.tabs A:hover SPAN
+{
+   background-position: 0% -150px;
+}
+
+DIV.tabs LI#current A
+{
+   background-position: 100% -150px;
+   border-width     : 0px;
+}
+
+DIV.tabs LI#current SPAN
+{
+   background-position: 0% -150px;
+   padding-bottom   : 6px;
+}
+
+DIV.nav
+{
+   background       : none;
+   border           : none;
+   border-bottom    : 1px solid #84B0C7;
+}
diff --git a/sm/drivers/playerstage/mricp/doxy.Doxyfile b/sm/drivers/playerstage/mricp/doxy.Doxyfile
new file mode 100644
index 0000000000000000000000000000000000000000..b1998b21c4fcb242ff2b79b51cf24b100cf4f2a3
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/doxy.Doxyfile
@@ -0,0 +1,1237 @@
+# Doxyfile 1.4.6
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project
+#
+# All text after a hash (#) is considered a comment and will be ignored
+# The format is:
+#       TAG = value [value, ...]
+# For lists items can also be appended using:
+#       TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (" ")
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded 
+# by quotes) that should identify the project.
+
+PROJECT_NAME = Map Reference ICP
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. 
+# This could be handy for archiving the generated documentation or 
+# if some version control system is used.
+
+PROJECT_NUMBER = $VERSION$
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) 
+# base path where the generated documentation will be put. 
+# If a relative path is entered, it will be relative to the location 
+# where doxygen was started. If left blank the current directory will be used.
+
+OUTPUT_DIRECTORY = doc
+
+# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create 
+# 4096 sub-directories (in 2 levels) under the output directory of each output 
+# format and will distribute the generated files over these directories. 
+# Enabling this option can be useful when feeding doxygen a huge amount of 
+# source files, where putting all generated files in the same directory would 
+# otherwise cause performance problems for the file system.
+
+CREATE_SUBDIRS = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all 
+# documentation generated by doxygen is written. Doxygen will use this 
+# information to generate all constant output in the proper language. 
+# The default language is English, other supported languages are: 
+# Brazilian, Catalan, Chinese, Chinese-Traditional, Croatian, Czech, Danish, 
+# Dutch, Finnish, French, German, Greek, Hungarian, Italian, Japanese, 
+# Japanese-en (Japanese with English messages), Korean, Korean-en, Norwegian, 
+# Polish, Portuguese, Romanian, Russian, Serbian, Slovak, Slovene, Spanish, 
+# Swedish, and Ukrainian.
+
+OUTPUT_LANGUAGE = English
+
+# This tag can be used to specify the encoding used in the generated output. 
+# The encoding is not always determined by the language that is chosen, 
+# but also whether or not the output is meant for Windows or non-Windows users. 
+# In case there is a difference, setting the USE_WINDOWS_ENCODING tag to YES 
+# forces the Windows encoding (this is the default for the Windows binary), 
+# whereas setting the tag to NO uses a Unix-style encoding (the default for 
+# all platforms other than Windows).
+
+USE_WINDOWS_ENCODING = NO
+
+# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will 
+# include brief member descriptions after the members that are listed in 
+# the file and class documentation (similar to JavaDoc). 
+# Set to NO to disable this.
+
+BRIEF_MEMBER_DESC = YES
+
+# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend 
+# the brief description of a member or function before the detailed description. 
+# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the 
+# brief descriptions will be completely suppressed.
+
+REPEAT_BRIEF = YES
+
+# This tag implements a quasi-intelligent brief description abbreviator 
+# that is used to form the text in various listings. Each string 
+# in this list, if found as the leading text of the brief description, will be 
+# stripped from the text and the result after processing the whole list, is 
+# used as the annotated text. Otherwise, the brief description is used as-is. 
+# If left blank, the following values are used ("$name" is automatically 
+# replaced with the name of the entity): "The $name class" "The $name widget" 
+# "The $name file" "is" "provides" "specifies" "contains" 
+# "represents" "a" "an" "the"
+
+ABBREVIATE_BRIEF = 
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then 
+# Doxygen will generate a detailed section even if there is only a brief 
+# description.
+
+ALWAYS_DETAILED_SEC = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all 
+# inherited members of a class in the documentation of that class as if those 
+# members were ordinary class members. Constructors, destructors and assignment 
+# operators of the base classes will not be shown.
+
+INLINE_INHERITED_MEMB = NO
+
+# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full 
+# path before files name in the file list and in the header files. If set 
+# to NO the shortest path that makes the file name unique will be used.
+
+FULL_PATH_NAMES = YES
+
+# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag 
+# can be used to strip a user-defined part of the path. Stripping is 
+# only done if one of the specified strings matches the left-hand part of 
+# the path. The tag can be used to show relative paths in the file list. 
+# If left blank the directory from which doxygen is run is used as the 
+# path to strip.
+
+STRIP_FROM_PATH = 
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of 
+# the path mentioned in the documentation of a class, which tells 
+# the reader which header file to include in order to use a class. 
+# If left blank only the name of the header file containing the class 
+# definition is used. Otherwise one should specify the include paths that 
+# are normally passed to the compiler using the -I flag.
+
+STRIP_FROM_INC_PATH = 
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter 
+# (but less readable) file names. This can be useful is your file systems 
+# doesn't support long names like on DOS, Mac, or CD-ROM.
+
+SHORT_NAMES = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen 
+# will interpret the first line (until the first dot) of a JavaDoc-style 
+# comment as the brief description. If set to NO, the JavaDoc 
+# comments will behave just like the Qt-style comments (thus requiring an 
+# explicit @brief command for a brief description.
+
+JAVADOC_AUTOBRIEF = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen 
+# treat a multi-line C++ special comment block (i.e. a block of //! or /// 
+# comments) as a brief description. This used to be the default behaviour. 
+# The new default is to treat a multi-line C++ comment block as a detailed 
+# description. Set this tag to YES if you prefer the old behaviour instead.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# If the DETAILS_AT_TOP tag is set to YES then Doxygen 
+# will output the detailed description near the top, like JavaDoc.
+# If set to NO, the detailed description appears after the member 
+# documentation.
+
+DETAILS_AT_TOP = NO
+
+# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented 
+# member inherits the documentation from any documented member that it 
+# re-implements.
+
+INHERIT_DOCS = YES
+
+# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce 
+# a new page for each member. If set to NO, the documentation of a member will 
+# be part of the file/class/namespace that contains it.
+
+SEPARATE_MEMBER_PAGES = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. 
+# Doxygen uses this value to replace tabs by spaces in code fragments.
+
+TAB_SIZE = 8
+
+# This tag can be used to specify a number of aliases that acts 
+# as commands in the documentation. An alias has the form "name=value". 
+# For example adding "sideeffect=\par Side Effects:\n" will allow you to 
+# put the command \sideeffect (or @sideeffect) in the documentation, which 
+# will result in a user-defined paragraph with heading "Side Effects:". 
+# You can put \n's in the value part of an alias to insert newlines.
+
+ALIASES = 
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C 
+# sources only. Doxygen will then generate output that is more tailored for C. 
+# For instance, some of the names that are used will be different. The list 
+# of all members will be omitted, etc.
+
+OPTIMIZE_OUTPUT_FOR_C = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java 
+# sources only. Doxygen will then generate output that is more tailored for Java. 
+# For instance, namespaces will be presented as packages, qualified scopes 
+# will look different, etc.
+
+OPTIMIZE_OUTPUT_JAVA = NO
+
+# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want to 
+# include (a tag file for) the STL sources as input, then you should 
+# set this tag to YES in order to let doxygen match functions declarations and 
+# definitions whose arguments contain STL classes (e.g. func(std::string); v.s. 
+# func(std::string) {}). This also make the inheritance and collaboration 
+# diagrams that involve STL classes more complete and accurate.
+
+BUILTIN_STL_SUPPORT = NO
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC 
+# tag is set to YES, then doxygen will reuse the documentation of the first 
+# member in the group (if any) for the other members of the group. By default 
+# all members of a group must be documented explicitly.
+
+DISTRIBUTE_GROUP_DOC = NO
+
+# Set the SUBGROUPING tag to YES (the default) to allow class member groups of 
+# the same type (for instance a group of public functions) to be put as a 
+# subgroup of that type (e.g. under the Public Functions section). Set it to 
+# NO to prevent subgrouping. Alternatively, this can be done per class using 
+# the \nosubgrouping command.
+
+SUBGROUPING = YES
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in 
+# documentation are documented, even if no documentation was available. 
+# Private class members and static file members will be hidden unless 
+# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES
+
+EXTRACT_ALL = NO
+
+# If the EXTRACT_PRIVATE tag is set to YES all private members of a class 
+# will be included in the documentation.
+
+EXTRACT_PRIVATE = NO
+
+# If the EXTRACT_STATIC tag is set to YES all static members of a file 
+# will be included in the documentation.
+
+EXTRACT_STATIC = NO
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) 
+# defined locally in source files will be included in the documentation. 
+# If set to NO only classes defined in header files are included.
+
+EXTRACT_LOCAL_CLASSES = YES
+
+# This flag is only useful for Objective-C code. When set to YES local 
+# methods, which are defined in the implementation section but not in 
+# the interface are included in the documentation. 
+# If set to NO (the default) only methods in the interface are included.
+
+EXTRACT_LOCAL_METHODS = NO
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all 
+# undocumented members of documented classes, files or namespaces. 
+# If set to NO (the default) these members will be included in the 
+# various overviews, but no documentation section is generated. 
+# This option has no effect if EXTRACT_ALL is enabled.
+
+HIDE_UNDOC_MEMBERS = NO
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all 
+# undocumented classes that are normally visible in the class hierarchy. 
+# If set to NO (the default) these classes will be included in the various 
+# overviews. This option has no effect if EXTRACT_ALL is enabled.
+
+HIDE_UNDOC_CLASSES = NO
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all 
+# friend (class|struct|union) declarations. 
+# If set to NO (the default) these declarations will be included in the 
+# documentation.
+
+HIDE_FRIEND_COMPOUNDS = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any 
+# documentation blocks found inside the body of a function. 
+# If set to NO (the default) these blocks will be appended to the 
+# function's detailed documentation block.
+
+HIDE_IN_BODY_DOCS = NO
+
+# The INTERNAL_DOCS tag determines if documentation 
+# that is typed after a \internal command is included. If the tag is set 
+# to NO (the default) then the documentation will be excluded. 
+# Set it to YES to include the internal documentation.
+
+INTERNAL_DOCS = NO
+
+# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate 
+# file names in lower-case letters. If set to YES upper-case letters are also 
+# allowed. This is useful if you have classes or files whose names only differ 
+# in case and if your file system supports case sensitive file names. Windows 
+# and Mac users are advised to set this option to NO.
+
+CASE_SENSE_NAMES = YES
+
+# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen 
+# will show members with their full class and namespace scopes in the 
+# documentation. If set to YES the scope will be hidden.
+
+HIDE_SCOPE_NAMES = NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen 
+# will put a list of the files that are included by a file in the documentation 
+# of that file.
+
+SHOW_INCLUDE_FILES = YES
+
+# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] 
+# is inserted in the documentation for inline members.
+
+INLINE_INFO = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen 
+# will sort the (detailed) documentation of file and class members 
+# alphabetically by member name. If set to NO the members will appear in 
+# declaration order.
+
+SORT_MEMBER_DOCS = YES
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the 
+# brief documentation of file, namespace and class members alphabetically 
+# by member name. If set to NO (the default) the members will appear in 
+# declaration order.
+
+SORT_BRIEF_DOCS = NO
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be 
+# sorted by fully-qualified names, including namespaces. If set to 
+# NO (the default), the class list will be sorted only by class name, 
+# not including the namespace part. 
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the 
+# alphabetical list.
+
+SORT_BY_SCOPE_NAME = NO
+
+# The GENERATE_TODOLIST tag can be used to enable (YES) or 
+# disable (NO) the todo list. This list is created by putting \todo 
+# commands in the documentation.
+
+GENERATE_TODOLIST = YES
+
+# The GENERATE_TESTLIST tag can be used to enable (YES) or 
+# disable (NO) the test list. This list is created by putting \test 
+# commands in the documentation.
+
+GENERATE_TESTLIST = YES
+
+# The GENERATE_BUGLIST tag can be used to enable (YES) or 
+# disable (NO) the bug list. This list is created by putting \bug 
+# commands in the documentation.
+
+GENERATE_BUGLIST = YES
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or 
+# disable (NO) the deprecated list. This list is created by putting 
+# \deprecated commands in the documentation.
+
+GENERATE_DEPRECATEDLIST = YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional 
+# documentation sections, marked by \if sectionname ... \endif.
+
+ENABLED_SECTIONS = 
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines 
+# the initial value of a variable or define consists of for it to appear in 
+# the documentation. If the initializer consists of more lines than specified 
+# here it will be hidden. Use a value of 0 to hide initializers completely. 
+# The appearance of the initializer of individual variables and defines in the 
+# documentation can be controlled using \showinitializer or \hideinitializer 
+# command in the documentation regardless of this setting.
+
+MAX_INITIALIZER_LINES = 30
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated 
+# at the bottom of the documentation of classes and structs. If set to YES the 
+# list will mention the files that were used to generate the documentation.
+
+SHOW_USED_FILES = YES
+
+# If the sources in your project are distributed over multiple directories 
+# then setting the SHOW_DIRECTORIES tag to YES will show the directory hierarchy 
+# in the documentation. The default is NO.
+
+SHOW_DIRECTORIES = NO
+
+# The FILE_VERSION_FILTER tag can be used to specify a program or script that 
+# doxygen should invoke to get the current version for each file (typically from the 
+# version control system). Doxygen will invoke the program by executing (via 
+# popen()) the command <command> <input-file>, where <command> is the value of 
+# the FILE_VERSION_FILTER tag, and <input-file> is the name of an input file 
+# provided by doxygen. Whatever the program writes to standard output 
+# is used as the file version. See the manual for examples.
+
+FILE_VERSION_FILTER = 
+
+#---------------------------------------------------------------------------
+# configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated 
+# by doxygen. Possible values are YES and NO. If left blank NO is used.
+
+QUIET = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are 
+# generated by doxygen. Possible values are YES and NO. If left blank 
+# NO is used.
+
+WARNINGS = YES
+
+# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings 
+# for undocumented members. If EXTRACT_ALL is set to YES then this flag will 
+# automatically be disabled.
+
+WARN_IF_UNDOCUMENTED = YES
+
+# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for 
+# potential errors in the documentation, such as not documenting some 
+# parameters in a documented function, or documenting parameters that 
+# don't exist or using markup commands wrongly.
+
+WARN_IF_DOC_ERROR = YES
+
+# This WARN_NO_PARAMDOC option can be abled to get warnings for 
+# functions that are documented, but have no documentation for their parameters 
+# or return value. If set to NO (the default) doxygen will only warn about 
+# wrong or incomplete parameter documentation, but not about the absence of 
+# documentation.
+
+WARN_NO_PARAMDOC = NO
+
+# The WARN_FORMAT tag determines the format of the warning messages that 
+# doxygen can produce. The string should contain the $file, $line, and $text 
+# tags, which will be replaced by the file and line number from which the 
+# warning originated and the warning text. Optionally the format may contain 
+# $version, which will be replaced by the version of the file (if it could 
+# be obtained via FILE_VERSION_FILTER)
+
+WARN_FORMAT = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning 
+# and error messages should be written. If left blank the output is written 
+# to stderr.
+
+WARN_LOGFILE = 
+
+#---------------------------------------------------------------------------
+# configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag can be used to specify the files and/or directories that contain 
+# documented source files. You may enter file names like "myfile.cpp" or 
+# directories like "/usr/src/myproject". Separate the files or directories 
+# with spaces.
+
+INPUT = src include 
+
+# If the value of the INPUT tag contains directories, you can use the 
+# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp 
+# and *.h) to filter out the source-files in the directories. If left 
+# blank the following patterns are tested: 
+# *.c *.cc *.cxx *.cpp *.c++ *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh *.hxx 
+# *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.py
+
+FILE_PATTERNS = 
+
+# The RECURSIVE tag can be used to turn specify whether or not subdirectories 
+# should be searched for input files as well. Possible values are YES and NO. 
+# If left blank NO is used.
+
+RECURSIVE = NO
+
+# The EXCLUDE tag can be used to specify files and/or directories that should 
+# excluded from the INPUT source files. This way you can easily exclude a 
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+
+EXCLUDE = 
+
+# The EXCLUDE_SYMLINKS tag can be used select whether or not files or 
+# directories that are symbolic links (a Unix filesystem feature) are excluded 
+# from the input.
+
+EXCLUDE_SYMLINKS = NO
+
+# If the value of the INPUT tag contains directories, you can use the 
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude 
+# certain files from those directories. Note that the wildcards are matched 
+# against the file with absolute path, so to exclude all test directories 
+# for example use the pattern */test/*
+
+EXCLUDE_PATTERNS = 
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or 
+# directories that contain example code fragments that are included (see 
+# the \include command).
+
+EXAMPLE_PATH = 
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the 
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp 
+# and *.h) to filter out the source-files in the directories. If left 
+# blank all files are included.
+
+EXAMPLE_PATTERNS = 
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be 
+# searched for input files to be used with the \include or \dontinclude 
+# commands irrespective of the value of the RECURSIVE tag. 
+# Possible values are YES and NO. If left blank NO is used.
+
+EXAMPLE_RECURSIVE = NO
+
+# The IMAGE_PATH tag can be used to specify one or more files or 
+# directories that contain image that are included in the documentation (see 
+# the \image command).
+
+IMAGE_PATH = 
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should 
+# invoke to filter for each input file. Doxygen will invoke the filter program 
+# by executing (via popen()) the command <filter> <input-file>, where <filter> 
+# is the value of the INPUT_FILTER tag, and <input-file> is the name of an 
+# input file. Doxygen will then use the output that the filter program writes 
+# to standard output.  If FILTER_PATTERNS is specified, this tag will be 
+# ignored.
+
+INPUT_FILTER = 
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern 
+# basis.  Doxygen will compare the file name with each pattern and apply the 
+# filter if there is a match.  The filters are a list of the form: 
+# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further 
+# info on how filters are used. If FILTER_PATTERNS is empty, INPUT_FILTER 
+# is applied to all files.
+
+FILTER_PATTERNS = 
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using 
+# INPUT_FILTER) will be used to filter the input files when producing source 
+# files to browse (i.e. when SOURCE_BROWSER is set to YES).
+
+FILTER_SOURCE_FILES = NO
+
+#---------------------------------------------------------------------------
+# configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will 
+# be generated. Documented entities will be cross-referenced with these sources. 
+# Note: To get rid of all source code in the generated output, make sure also 
+# VERBATIM_HEADERS is set to NO.
+
+SOURCE_BROWSER = NO
+
+# Setting the INLINE_SOURCES tag to YES will include the body 
+# of functions and classes directly in the documentation.
+
+INLINE_SOURCES = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct 
+# doxygen to hide any special comment blocks from generated source code 
+# fragments. Normal C and C++ comments will always remain visible.
+
+STRIP_CODE_COMMENTS = YES
+
+# If the REFERENCED_BY_RELATION tag is set to YES (the default) 
+# then for each documented function all documented 
+# functions referencing it will be listed.
+
+REFERENCED_BY_RELATION = YES
+
+# If the REFERENCES_RELATION tag is set to YES (the default) 
+# then for each documented function all documented entities 
+# called/used by that function will be listed.
+
+REFERENCES_RELATION = YES
+
+# If the USE_HTAGS tag is set to YES then the references to source code 
+# will point to the HTML generated by the htags(1) tool instead of doxygen 
+# built-in source browser. The htags tool is part of GNU's global source 
+# tagging system (see http://www.gnu.org/software/global/global.html). You 
+# will need version 4.8.6 or higher.
+
+USE_HTAGS = NO
+
+# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen 
+# will generate a verbatim copy of the header file for each class for 
+# which an include is specified. Set to NO to disable this.
+
+VERBATIM_HEADERS = YES
+
+#---------------------------------------------------------------------------
+# configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index 
+# of all compounds will be generated. Enable this if the project 
+# contains a lot of classes, structs, unions or interfaces.
+
+ALPHABETICAL_INDEX = NO
+
+# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then 
+# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns 
+# in which this list will be split (can be a number in the range [1..20])
+
+COLS_IN_ALPHA_INDEX = 5
+
+# In case all classes in a project start with a common prefix, all 
+# classes will be put under the same header in the alphabetical index. 
+# The IGNORE_PREFIX tag can be used to specify one or more prefixes that 
+# should be ignored while generating the index headers.
+
+IGNORE_PREFIX = 
+
+#---------------------------------------------------------------------------
+# configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES (the default) Doxygen will 
+# generate HTML output.
+
+GENERATE_HTML = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. 
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
+# put in front of it. If left blank `html' will be used as the default path.
+
+HTML_OUTPUT = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for 
+# each generated HTML page (for example: .htm,.php,.asp). If it is left blank 
+# doxygen will generate files with .html extension.
+
+HTML_FILE_EXTENSION = .html
+
+# The HTML_HEADER tag can be used to specify a personal HTML header for 
+# each generated HTML page. If it is left blank doxygen will generate a 
+# standard header.
+
+HTML_HEADER = 
+
+# The HTML_FOOTER tag can be used to specify a personal HTML footer for 
+# each generated HTML page. If it is left blank doxygen will generate a 
+# standard footer.
+
+HTML_FOOTER = 
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading 
+# style sheet that is used by each HTML page. It can be used to 
+# fine-tune the look of the HTML output. If the tag is left blank doxygen 
+# will generate a default style sheet. Note that doxygen will try to copy 
+# the style sheet file to the HTML output directory, so don't put your own 
+# stylesheet in the HTML output directory as well, or it will be erased!
+
+HTML_STYLESHEET = 
+
+# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, 
+# files or namespaces will be aligned in HTML using tables. If set to 
+# NO a bullet list will be used.
+
+HTML_ALIGN_MEMBERS = YES
+
+# If the GENERATE_HTMLHELP tag is set to YES, additional index files 
+# will be generated that can be used as input for tools like the 
+# Microsoft HTML help workshop to generate a compressed HTML help file (.chm) 
+# of the generated HTML documentation.
+
+GENERATE_HTMLHELP = NO
+
+# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can 
+# be used to specify the file name of the resulting .chm file. You 
+# can add a path in front of the file if the result should not be 
+# written to the html output directory.
+
+CHM_FILE = 
+
+# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can 
+# be used to specify the location (absolute path including file name) of 
+# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run 
+# the HTML help compiler on the generated index.hhp.
+
+HHC_LOCATION = 
+
+# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag 
+# controls if a separate .chi index file is generated (YES) or that 
+# it should be included in the master .chm file (NO).
+
+GENERATE_CHI = NO
+
+# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag 
+# controls whether a binary table of contents is generated (YES) or a 
+# normal table of contents (NO) in the .chm file.
+
+BINARY_TOC = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members 
+# to the contents of the HTML help documentation and to the tree view.
+
+TOC_EXPAND = NO
+
+# The DISABLE_INDEX tag can be used to turn on/off the condensed index at 
+# top of each HTML page. The value NO (the default) enables the index and 
+# the value YES disables it.
+
+DISABLE_INDEX = NO
+
+# This tag can be used to set the number of enum values (range [1..20]) 
+# that doxygen will group on one line in the generated HTML documentation.
+
+ENUM_VALUES_PER_LINE = 4
+
+# If the GENERATE_TREEVIEW tag is set to YES, a side panel will be
+# generated containing a tree-like index structure (just like the one that 
+# is generated for HTML Help). For this to work a browser that supports 
+# JavaScript, DHTML, CSS and frames is required (for instance Mozilla 1.0+, 
+# Netscape 6.0+, Internet explorer 5.0+, or Konqueror). Windows users are 
+# probably better off using the HTML help feature.
+
+GENERATE_TREEVIEW = NO
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be 
+# used to set the initial width (in pixels) of the frame in which the tree 
+# is shown.
+
+TREEVIEW_WIDTH = 250
+
+#---------------------------------------------------------------------------
+# configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will 
+# generate Latex output.
+
+GENERATE_LATEX = YES
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. 
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
+# put in front of it. If left blank `latex' will be used as the default path.
+
+LATEX_OUTPUT = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be 
+# invoked. If left blank `latex' will be used as the default command name.
+
+LATEX_CMD_NAME = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to 
+# generate index for LaTeX. If left blank `makeindex' will be used as the 
+# default command name.
+
+MAKEINDEX_CMD_NAME = makeindex
+
+# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact 
+# LaTeX documents. This may be useful for small projects and may help to 
+# save some trees in general.
+
+COMPACT_LATEX = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used 
+# by the printer. Possible values are: a4, a4wide, letter, legal and 
+# executive. If left blank a4wide will be used.
+
+PAPER_TYPE = a4wide
+
+# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX 
+# packages that should be included in the LaTeX output.
+
+EXTRA_PACKAGES = 
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for 
+# the generated latex document. The header should contain everything until 
+# the first chapter. If it is left blank doxygen will generate a 
+# standard header. Notice: only use this tag if you know what you are doing!
+
+LATEX_HEADER = 
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated 
+# is prepared for conversion to pdf (using ps2pdf). The pdf file will 
+# contain links (just like the HTML output) instead of page references 
+# This makes the output suitable for online browsing using a pdf viewer.
+
+PDF_HYPERLINKS = NO
+
+# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of 
+# plain latex in the generated Makefile. Set this option to YES to get a 
+# higher quality PDF documentation.
+
+USE_PDFLATEX = NO
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. 
+# command to the generated LaTeX files. This will instruct LaTeX to keep 
+# running if errors occur, instead of asking the user for help. 
+# This option is also used when generating formulas in HTML.
+
+LATEX_BATCHMODE = NO
+
+# If LATEX_HIDE_INDICES is set to YES then doxygen will not 
+# include the index chapters (such as File Index, Compound Index, etc.) 
+# in the output.
+
+LATEX_HIDE_INDICES = NO
+
+#---------------------------------------------------------------------------
+# configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output 
+# The RTF output is optimized for Word 97 and may not look very pretty with 
+# other RTF readers or editors.
+
+GENERATE_RTF = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. 
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
+# put in front of it. If left blank `rtf' will be used as the default path.
+
+RTF_OUTPUT = rtf
+
+# If the COMPACT_RTF tag is set to YES Doxygen generates more compact 
+# RTF documents. This may be useful for small projects and may help to 
+# save some trees in general.
+
+COMPACT_RTF = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated 
+# will contain hyperlink fields. The RTF file will 
+# contain links (just like the HTML output) instead of page references. 
+# This makes the output suitable for online browsing using WORD or other 
+# programs which support those fields. 
+# Note: wordpad (write) and others do not support links.
+
+RTF_HYPERLINKS = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's 
+# config file, i.e. a series of assignments. You only have to provide 
+# replacements, missing definitions are set to their default value.
+
+RTF_STYLESHEET_FILE = 
+
+# Set optional variables used in the generation of an rtf document. 
+# Syntax is similar to doxygen's config file.
+
+RTF_EXTENSIONS_FILE = 
+
+#---------------------------------------------------------------------------
+# configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES (the default) Doxygen will 
+# generate man pages
+
+GENERATE_MAN = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. 
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
+# put in front of it. If left blank `man' will be used as the default path.
+
+MAN_OUTPUT = man
+
+# The MAN_EXTENSION tag determines the extension that is added to 
+# the generated man pages (default is the subroutine's section .3)
+
+MAN_EXTENSION = .3
+
+# If the MAN_LINKS tag is set to YES and Doxygen generates man output, 
+# then it will generate one additional man file for each entity 
+# documented in the real man page(s). These additional files 
+# only source the real man page, but without them the man command 
+# would be unable to find the correct page. The default is NO.
+
+MAN_LINKS = NO
+
+#---------------------------------------------------------------------------
+# configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES Doxygen will 
+# generate an XML file that captures the structure of 
+# the code including all documentation.
+
+GENERATE_XML = NO
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. 
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be 
+# put in front of it. If left blank `xml' will be used as the default path.
+
+XML_OUTPUT = xml
+
+# The XML_SCHEMA tag can be used to specify an XML schema, 
+# which can be used by a validating XML parser to check the 
+# syntax of the XML files.
+
+XML_SCHEMA = 
+
+# The XML_DTD tag can be used to specify an XML DTD, 
+# which can be used by a validating XML parser to check the 
+# syntax of the XML files.
+
+XML_DTD = 
+
+# If the XML_PROGRAMLISTING tag is set to YES Doxygen will 
+# dump the program listings (including syntax highlighting 
+# and cross-referencing information) to the XML output. Note that 
+# enabling this will significantly increase the size of the XML output.
+
+XML_PROGRAMLISTING = YES
+
+#---------------------------------------------------------------------------
+# configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will 
+# generate an AutoGen Definitions (see autogen.sf.net) file 
+# that captures the structure of the code including all 
+# documentation. Note that this feature is still experimental 
+# and incomplete at the moment.
+
+GENERATE_AUTOGEN_DEF = NO
+
+#---------------------------------------------------------------------------
+# configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES Doxygen will 
+# generate a Perl module file that captures the structure of 
+# the code including all documentation. Note that this 
+# feature is still experimental and incomplete at the 
+# moment.
+
+GENERATE_PERLMOD = NO
+
+# If the PERLMOD_LATEX tag is set to YES Doxygen will generate 
+# the necessary Makefile rules, Perl scripts and LaTeX code to be able 
+# to generate PDF and DVI output from the Perl module output.
+
+PERLMOD_LATEX = NO
+
+# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be 
+# nicely formatted so it can be parsed by a human reader.  This is useful 
+# if you want to understand what is going on.  On the other hand, if this 
+# tag is set to NO the size of the Perl module output will be much smaller 
+# and Perl will parse it just the same.
+
+PERLMOD_PRETTY = YES
+
+# The names of the make variables in the generated doxyrules.make file 
+# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. 
+# This is useful so different doxyrules.make files included by the same 
+# Makefile don't overwrite each other's variables.
+
+PERLMOD_MAKEVAR_PREFIX = 
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor   
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will 
+# evaluate all C-preprocessor directives found in the sources and include 
+# files.
+
+ENABLE_PREPROCESSING = YES
+
+# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro 
+# names in the source code. If set to NO (the default) only conditional 
+# compilation will be performed. Macro expansion can be done in a controlled 
+# way by setting EXPAND_ONLY_PREDEF to YES.
+
+MACRO_EXPANSION = NO
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES 
+# then the macro expansion is limited to the macros specified with the 
+# PREDEFINED and EXPAND_AS_DEFINED tags.
+
+EXPAND_ONLY_PREDEF = NO
+
+# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files 
+# in the INCLUDE_PATH (see below) will be search if a #include is found.
+
+SEARCH_INCLUDES = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that 
+# contain include files that are not input files but should be processed by 
+# the preprocessor.
+
+INCLUDE_PATH = 
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard 
+# patterns (like *.h and *.hpp) to filter out the header-files in the 
+# directories. If left blank, the patterns specified with FILE_PATTERNS will 
+# be used.
+
+INCLUDE_FILE_PATTERNS = 
+
+# The PREDEFINED tag can be used to specify one or more macro names that 
+# are defined before the preprocessor is started (similar to the -D option of 
+# gcc). The argument of the tag is a list of macros of the form: name 
+# or name=definition (no spaces). If the definition and the = are 
+# omitted =1 is assumed. To prevent a macro definition from being 
+# undefined via #undef or recursively expanded use the := operator 
+# instead of the = operator.
+
+PREDEFINED = 
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then 
+# this tag can be used to specify a list of macro names that should be expanded. 
+# The macro definition that is found in the sources will be used. 
+# Use the PREDEFINED tag if you want to use a different macro definition.
+
+EXPAND_AS_DEFINED = 
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then 
+# doxygen's preprocessor will remove all function-like macros that are alone 
+# on a line, have an all uppercase name, and do not end with a semicolon. Such 
+# function macros are typically used for boiler-plate code, and will confuse 
+# the parser if not removed.
+
+SKIP_FUNCTION_MACROS = YES
+
+#---------------------------------------------------------------------------
+# Configuration::additions related to external references   
+#---------------------------------------------------------------------------
+
+# The TAGFILES option can be used to specify one or more tagfiles. 
+# Optionally an initial location of the external documentation 
+# can be added for each tagfile. The format of a tag file without 
+# this location is as follows: 
+#   TAGFILES = file1 file2 ... 
+# Adding location for the tag files is done as follows: 
+#   TAGFILES = file1=loc1 "file2 = loc2" ... 
+# where "loc1" and "loc2" can be relative or absolute paths or 
+# URLs. If a location is present for each tag, the installdox tool 
+# does not have to be run to correct the links.
+# Note that each tag file must have a unique name
+# (where the name does NOT include the path)
+# If a tag file is not located in the directory in which doxygen 
+# is run, you must also specify the path to the tagfile here.
+
+TAGFILES = 
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create 
+# a tag file that is based on the input files it reads.
+
+GENERATE_TAGFILE = 
+
+# If the ALLEXTERNALS tag is set to YES all external classes will be listed 
+# in the class index. If set to NO only the inherited external classes 
+# will be listed.
+
+ALLEXTERNALS = NO
+
+# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed 
+# in the modules index. If set to NO, only the current project's groups will 
+# be listed.
+
+EXTERNAL_GROUPS = YES
+
+# The PERL_PATH should be the absolute path and name of the perl script 
+# interpreter (i.e. the result of `which perl').
+
+PERL_PATH = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool   
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will 
+# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base 
+# or super classes. Setting the tag to NO turns the diagrams off. Note that 
+# this option is superseded by the HAVE_DOT option below. This is only a 
+# fallback. It is recommended to install and use dot, since it yields more 
+# powerful graphs.
+
+CLASS_DIAGRAMS = YES
+
+# If set to YES, the inheritance and collaboration graphs will hide 
+# inheritance and usage relations if the target is undocumented 
+# or is not a class.
+
+HIDE_UNDOC_RELATIONS = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is 
+# available from the path. This tool is part of Graphviz, a graph visualization 
+# toolkit from AT&T and Lucent Bell Labs. The other options in this section 
+# have no effect if this option is set to NO (the default)
+
+HAVE_DOT = NO
+
+# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen 
+# will generate a graph for each documented class showing the direct and 
+# indirect inheritance relations. Setting this tag to YES will force the 
+# the CLASS_DIAGRAMS tag to NO.
+
+CLASS_GRAPH = YES
+
+# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen 
+# will generate a graph for each documented class showing the direct and 
+# indirect implementation dependencies (inheritance, containment, and 
+# class references variables) of the class with other documented classes.
+
+COLLABORATION_GRAPH = YES
+
+# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen 
+# will generate a graph for groups, showing the direct groups dependencies
+
+GROUP_GRAPHS = YES
+
+# If the UML_LOOK tag is set to YES doxygen will generate inheritance and 
+# collaboration diagrams in a style similar to the OMG's Unified Modeling 
+# Language.
+
+UML_LOOK = NO
+
+# If set to YES, the inheritance and collaboration graphs will show the 
+# relations between templates and their instances.
+
+TEMPLATE_RELATIONS = NO
+
+# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT 
+# tags are set to YES then doxygen will generate a graph for each documented 
+# file showing the direct and indirect include dependencies of the file with 
+# other documented files.
+
+INCLUDE_GRAPH = YES
+
+# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and 
+# HAVE_DOT tags are set to YES then doxygen will generate a graph for each 
+# documented header file showing the documented files that directly or 
+# indirectly include this file.
+
+INCLUDED_BY_GRAPH = YES
+
+# If the CALL_GRAPH and HAVE_DOT tags are set to YES then doxygen will 
+# generate a call dependency graph for every global function or class method. 
+# Note that enabling this option will significantly increase the time of a run. 
+# So in most cases it will be better to enable call graphs for selected 
+# functions only using the \callgraph command.
+
+CALL_GRAPH = NO
+
+# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen 
+# will graphical hierarchy of all classes instead of a textual one.
+
+GRAPHICAL_HIERARCHY = YES
+
+# If the DIRECTORY_GRAPH, SHOW_DIRECTORIES and HAVE_DOT tags are set to YES 
+# then doxygen will show the dependencies a directory has on other directories 
+# in a graphical way. The dependency relations are determined by the #include
+# relations between the files in the directories.
+
+DIRECTORY_GRAPH = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images 
+# generated by dot. Possible values are png, jpg, or gif
+# If left blank png will be used.
+
+DOT_IMAGE_FORMAT = png
+
+# The tag DOT_PATH can be used to specify the path where the dot tool can be 
+# found. If left blank, it is assumed the dot tool can be found in the path.
+
+DOT_PATH = 
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that 
+# contain dot files that are included in the documentation (see the 
+# \dotfile command).
+
+DOTFILE_DIRS = 
+
+# The MAX_DOT_GRAPH_WIDTH tag can be used to set the maximum allowed width 
+# (in pixels) of the graphs generated by dot. If a graph becomes larger than 
+# this value, doxygen will try to truncate the graph, so that it fits within 
+# the specified constraint. Beware that most browsers cannot cope with very 
+# large images.
+
+MAX_DOT_GRAPH_WIDTH = 1024
+
+# The MAX_DOT_GRAPH_HEIGHT tag can be used to set the maximum allows height 
+# (in pixels) of the graphs generated by dot. If a graph becomes larger than 
+# this value, doxygen will try to truncate the graph, so that it fits within 
+# the specified constraint. Beware that most browsers cannot cope with very 
+# large images.
+
+MAX_DOT_GRAPH_HEIGHT = 1024
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the 
+# graphs generated by dot. A depth value of 3 means that only nodes reachable 
+# from the root by following a path via at most 3 edges will be shown. Nodes 
+# that lay further from the root node will be omitted. Note that setting this 
+# option to 1 or 2 may greatly reduce the computation time needed for large 
+# code bases. Also note that a graph may be further truncated if the graph's 
+# image dimensions are not sufficient to fit the graph (see MAX_DOT_GRAPH_WIDTH 
+# and MAX_DOT_GRAPH_HEIGHT). If 0 is used for the depth value (the default), 
+# the graph is not depth-constrained.
+
+MAX_DOT_GRAPH_DEPTH = 0
+
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent 
+# background. This is disabled by default, which results in a white background. 
+# Warning: Depending on the platform used, enabling this option may lead to 
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to 
+# read).
+
+DOT_TRANSPARENT = NO
+
+# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output 
+# files in one run (i.e. multiple -o and -T options on the command line). This 
+# makes dot run faster, but since only newer versions of dot (>1.8.10) 
+# support this, this feature is disabled by default.
+
+DOT_MULTI_TARGETS = NO
+
+# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will 
+# generate a legend page explaining the meaning of the various boxes and 
+# arrows in the dot generated graphs.
+
+GENERATE_LEGEND = YES
+
+# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will 
+# remove the intermediate dot files that are used to generate 
+# the various graphs.
+
+DOT_CLEANUP = YES
+
+#---------------------------------------------------------------------------
+# Configuration::additions related to the search engine   
+#---------------------------------------------------------------------------
+
+# The SEARCHENGINE tag specifies whether or not a search engine should be 
+# used. If set to NO the values of all tags below this one will be ignored.
+
+SEARCHENGINE = NO
diff --git a/sm/drivers/playerstage/mricp/include/Timer.h b/sm/drivers/playerstage/mricp/include/Timer.h
new file mode 100644
index 0000000000000000000000000000000000000000..29a338fbe98d71a9d8aba68acf5a04b64148898e
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/include/Timer.h
@@ -0,0 +1,52 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Tarek Taha                                      *
+ *   tataha@eng.uts.edu.au                                                 *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifndef TIMER_H_
+#define TIMER_H_
+
+#if defined WIN32
+  #include <replace.h>
+  #include <Winsock2.h>  // For struct timeval
+#else
+  #include <sys/time.h>
+#endif
+#include <iostream>
+
+
+class MricpTimer
+{
+	private:
+		struct timeval start_time,end_time;
+		double time_diff;
+	public:
+		MricpTimer();
+		double TimeElapsed(); // time elapsed in usec since last call
+		void Reset(); 				// resets timer
+		virtual ~MricpTimer();
+		/* Synchronize the loop within a period
+		 * To use this u will have to initialize the timer
+		 * reset the timer at the beginning of the loop
+		 * and call the Synch function at the end of the loop
+		 */
+		void Synch(double period); // period should be in msec
+};
+
+#endif /*TIMER_H_*/
+
diff --git a/sm/drivers/playerstage/mricp/include/geometry2D.h b/sm/drivers/playerstage/mricp/include/geometry2D.h
new file mode 100644
index 0000000000000000000000000000000000000000..5aec54c1a8e5f12ceb8295ad432e659ff50d0faf
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/include/geometry2D.h
@@ -0,0 +1,172 @@
+/*
+Copyright (c) 2004, Tim Bailey
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+    * Redistributions of source code must retain the above copyright notice,
+      this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright notice,
+      this list of conditions and the following disclaimer in the documentation
+      and/or other materials provided with the distribution.
+    * Neither the name of the Player Project nor the names of its contributors
+      may be used to endorse or promote products derived from this software
+      without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+/* Simple 2D geometric operations with points, poses, and lines.
+ *
+ * These algorithms were worked out by me from first principles several years
+ * ago. They work ok, but are not particularly good implementations. I recommend
+ * the geometric source-code and resources by David Eberly on the "Magic Software"
+ * website: www.magic-software.com.
+ *
+ * Tim Bailey 2004.
+ */
+#ifndef GEOMETRY_2D_H_
+#define GEOMETRY_2D_H_
+
+#include <vector>
+
+namespace Geom2D {
+
+//
+// Basic Structures
+//
+
+struct Point 
+{
+	double x;
+	double y;
+	short int laser_index;
+};
+
+struct Pose 
+{
+	Point p;
+	double phi;
+};
+
+struct Line {
+	Point first;
+	Point second;
+};
+
+//
+// Utility functions
+//
+
+const double PI = 3.14159265358979;
+
+inline 
+double sqr(double x) { return x*x; }
+
+inline 
+double abs(double x) { return (x<0.) ? -x : x; }
+
+inline
+double round(double x) { 
+	return (x<0.) ? -static_cast<int>(0.5-x) : static_cast<int>(0.5+x); 
+}
+/*
+template<class T>
+inline
+void swap(T& a, T& b) 
+{
+	T tmp(a);
+	a = b;
+	b = tmp;
+}
+*/
+inline
+double pi_to_pi(double angle) { // normalise an angle to within +/- PI
+	while (angle < -PI)
+		angle += 2.*PI;
+	while (angle > PI)
+		angle -= 2.*PI;
+	return angle;
+}
+
+//
+// Point and Pose algorithms
+//
+
+inline 
+double dist_sqr(const Point& p, const Point& q) { // squared distance between two Points
+	return (sqr(p.x-q.x) + sqr(p.y-q.y));
+}
+
+double dist(const Point& p, const Point& q);
+Pose compute_relative_pose(const std::vector<Point>& a, const std::vector<Point>& b);
+
+//
+// Line algorithms
+//
+
+bool intersection_line_line (Point& p, const Line& l, const Line& m);
+double distance_line_point (const Line& lne, const Point& p);
+void intersection_line_point(Point& p, const Line& l, const Point& q);
+
+//
+// Basic transformations on 2-D Points (x,y) and Poses (x,y,phi).
+//
+
+class Transform2D {
+public:
+	Transform2D(const Pose& ref);
+
+	void transform_to_relative(Point &p);
+	void transform_to_relative(Pose &p);
+	void transform_to_global(Point &p);
+	void transform_to_global(Pose &p);
+
+private:
+	const Pose base;
+	double c;
+	double s;
+};
+
+inline
+void Transform2D::transform_to_relative(Point &p) {
+	p.x -= base.p.x; 
+	p.y -= base.p.y;
+	double t(p.x);
+	p.x = p.x*c + p.y*s;
+	p.y = p.y*c -   t*s;
+}
+
+inline
+void Transform2D::transform_to_global(Point &p) {
+	double t(p.x); 
+	p.x = base.p.x + c*p.x - s*p.y;
+	p.y = base.p.y + s*t   + c*p.y;
+}
+
+inline
+void Transform2D::transform_to_relative(Pose &p) {
+	transform_to_relative(p.p);
+	p.phi= pi_to_pi(p.phi-base.phi);
+}
+
+inline
+void Transform2D::transform_to_global(Pose &p) {
+	transform_to_global(p.p);
+	p.phi= pi_to_pi(p.phi+base.phi);
+}
+
+} // namespace Geom2D
+
+#endif
diff --git a/sm/drivers/playerstage/mricp/include/icp.h b/sm/drivers/playerstage/mricp/include/icp.h
new file mode 100644
index 0000000000000000000000000000000000000000..7b7652d598988d1a61741481f8f35eee49866c9c
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/include/icp.h
@@ -0,0 +1,82 @@
+/*
+Copyright (c) 2004, Tim Bailey
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+    * Redistributions of source code must retain the above copyright notice,
+      this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright notice,
+      this list of conditions and the following disclaimer in the documentation
+      and/or other materials provided with the distribution.
+    * Neither the name of the Player Project nor the names of its contributors
+      may be used to endorse or promote products derived from this software
+      without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+/* Iterated closest point (ICP) algorithm.
+ * 
+ * This is a very simple implementation of ICP, with one simple extension where the
+ * association may be chosen as an interpolation between two nearest neighbours rather
+ * than just point-to-point with the nearest neighbour.
+ *
+ * A lot of extensions to this basic algorithm are possible. For example,
+ *	1. Record at each iteration the set of NN associations for each observation. 
+ *	If these do not change, then ICP has converged completely.
+ *	
+ *	2. Various speed ups given in the following papers:
+ *
+ *	(a) P.J. Besl and N.D. McKay. A method for registration of 3-D shapes. IEEE 
+ *	Transactions on Pattern Analysis and Machine Intelligence, 14(2):239256, 1992.
+ *
+ *	(b) F. Lu and E. Milios. Robot pose estimation in unknown environments by matching
+ *	2D range scans. Journal of Intelligent and Robotic Systems, 18:249275, 1997.
+ *
+ *	(c) S. Rusinkiewicz and M. Levoy. Efficient variants of the ICP algorithm. In Third
+ *	International Conference on 3D Digital Imaging and Modeling, pages 145152, 2001.
+ *
+ *	3. Methods for estimating the error introduced by point-wise correspondence in 
+ *	the paper (b) above and also:
+ *
+ *	S.T. P?ster, K.L. Kriechbaum, S.I. Roumeliotis, and J.W. Burdick. Weighted range
+ *	sensor matching algorithms for mobile robot displacement estimation. In IEEE 
+ *	International Conference on Robotics and Automation, 2002.
+ *
+ * Tim Bailey 2004.
+ */
+#include <vector>
+#include "geometry2D.h"
+#include "nn.h"
+#include <memory>
+using namespace std;
+namespace Geom2D {
+
+class ICP {
+public:
+	ICP();
+	~ICP();
+	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;
+	vector<Point> a;
+	vector<Point> b;
+	vector<int> index;
+};
+
+} // namespace Geom2D 
diff --git a/sm/drivers/playerstage/mricp/include/lasermodel.h b/sm/drivers/playerstage/mricp/include/lasermodel.h
new file mode 100644
index 0000000000000000000000000000000000000000..43846e724eac9b61acdfc831af97e837191e1c5f
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/include/lasermodel.h
@@ -0,0 +1,51 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Tarek Taha                                      *
+ *   tataha@eng.uts.edu.au                                                 *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifndef LASERMODEL_H_
+#define LASERMODEL_H_
+// Info for a single range measurement
+typedef struct
+{
+	double range, bearing;
+}	laser_range_t;
+
+class LaserModel
+{
+	private:
+	  	mapgrid * * map; 	// Pointer to the OG map
+	  	double range_cov;	// Covariance in the range reading
+	  	double range_bad;	// Probability of spurious range readings
+	  	// Pre-computed laser sensor model
+	  	int lut_size;
+	  	double  lut_res;
+	  	double *lut_probs;
+	  	int range_count;
+	  	laser_range_t *ranges;
+	public :
+		void 	ClearRanges();
+		void 	AddRange(double,double);
+		void 	PreCompute();
+		double 	RangeProb(double,double);
+		double 	PoseProb();
+				LaserModel();
+		 		~LaserModel();
+				LaserModel(mapgrid * * );
+};
+#endif /*LASERMODEL_H_*/
diff --git a/sm/drivers/playerstage/mricp/include/map.h b/sm/drivers/playerstage/mricp/include/map.h
new file mode 100644
index 0000000000000000000000000000000000000000..91edf6f555a484fb44b6131df98237b800f42cac
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/include/map.h
@@ -0,0 +1,60 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Tarek Taha                                      *
+ *   tataha@eng.uts.edu.au                                                 *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#ifndef map_HEADER
+#define map_HEADER
+
+#include <gdk-pixbuf/gdk-pixbuf.h>
+#include <gtk/gtk.h>
+#include <glib/gprintf.h>
+
+#include <sys/stat.h>
+class MapInfo
+{
+public :
+	int height;
+	int width;
+	double pixel_size;
+};
+typedef struct mapgrid
+{
+	double prob_occ,prob_free;
+	bool added;
+} mapgrid_t;
+class MAP
+{
+	private:
+    		double pixel_size;
+    		int negate;
+  			GdkPixbuf * pixbuf;
+	public:
+	   		int size_x, size_y;
+			mapgrid * * occ_grid;
+    		char * Mapname;
+			MapInfo    GetMapInfo();
+			void SavePixelBufferToFile();
+			void DrawPixel(int,int,int,int,int);
+			void ClearData();
+			void ResetProb();
+			int  SavePgm();
+			GdkPixbuf * CreateMap();
+			~MAP();
+			MAP(char*, double,double);
+};
+#endif
diff --git a/sm/drivers/playerstage/mricp/include/nn.h b/sm/drivers/playerstage/mricp/include/nn.h
new file mode 100644
index 0000000000000000000000000000000000000000..2abd9683c87b6b6c23971a9fce478ebb0f279f19
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/include/nn.h
@@ -0,0 +1,82 @@
+/*
+Copyright (c) 2004, Tim Bailey
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+    * Redistributions of source code must retain the above copyright notice,
+      this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright notice,
+      this list of conditions and the following disclaimer in the documentation
+      and/or other materials provided with the distribution.
+    * Neither the name of the Player Project nor the names of its contributors
+      may be used to endorse or promote products derived from this software
+      without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+/* Nearest-neighbours algorithm for 2D point-sets.
+ * Compute the single nearest-neighbour of a point, or the set of k-nearest neighbours.
+ *
+ * This is a very simple algorithm that is reasonably efficient for planar
+ * points. However much faster algorithms are possible.
+ *
+ * Tim Bailey 2004.
+ */
+
+#ifndef NN_HEADER
+#define NN_HEADER
+
+#include "geometry2D.h"
+#include <vector>
+
+namespace Geom2D {
+
+// Simple 2D k-nearest-neighbours search
+//
+class SweepSearch {
+public:
+	enum { NOT_FOUND = -1 };
+
+	SweepSearch(const std::vector<Point> &p, double dmax);
+
+	int query(const Point &q) const;
+	std::vector<double>& query(const Point &q, std::vector<int> &idx);
+
+private:	
+	struct PointIdx { 
+		PointIdx() {}
+		PointIdx(const Point &p_, const int& i_) : p(p_), i(i_) {}
+		Point p;
+		int i; 
+	};
+
+	const double limit;
+	std::vector<PointIdx> dataset;
+	std::vector<double> nndists;
+
+	bool is_nearer(double &d2min, int &idxmin, const Point &q, const PointIdx &pi) const;
+
+	bool insert_neighbour(const Point &q, const PointIdx &pi, 
+		std::vector<double> &nndists, std::vector<int> &idx);
+
+	static bool yorder (const PointIdx& p, const PointIdx& q) {
+		return p.p.y < q.p.y;
+	}
+};
+
+}
+
+#endif
diff --git a/sm/drivers/playerstage/mricp/src/Timer.cpp b/sm/drivers/playerstage/mricp/src/Timer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6bf898f3b761c6171895bac44266bb4d346aeedb
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/src/Timer.cpp
@@ -0,0 +1,57 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Tarek Taha                                      *
+ *   tataha@eng.uts.edu.au                                                 *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#include "Timer.h"
+#include <ctime>
+#include <cstddef>
+
+MricpTimer::MricpTimer()
+{
+	gettimeofday(&start_time,NULL);
+}
+double MricpTimer::TimeElapsed() // return in usec
+{
+	gettimeofday(&end_time,NULL);
+	time_diff = ((double) end_time.tv_sec*1000000 + (double)end_time.tv_usec) -
+	            ((double) start_time.tv_sec*1000000 + (double)start_time.tv_usec);
+	return  time_diff;
+}
+MricpTimer::~MricpTimer()
+{
+}
+void MricpTimer::Reset()
+{
+	gettimeofday(&start_time,NULL);
+}
+void MricpTimer::Synch(double period)
+{
+	struct timespec ts;
+	int us;
+
+	double time_elapsed = this->TimeElapsed();
+	if (time_elapsed < (period*1000))
+	{
+		us = static_cast<int>(period*1000-time_elapsed);
+		ts.tv_sec = us/1000000;
+		ts.tv_nsec = (us%1000000)*1000;
+		nanosleep(&ts, NULL);
+	}
+}
+
diff --git a/sm/drivers/playerstage/mricp/src/geometry2D.cpp b/sm/drivers/playerstage/mricp/src/geometry2D.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ecf39f319bbb8db1eff038b2005a447e314135a2
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/src/geometry2D.cpp
@@ -0,0 +1,188 @@
+/*
+Copyright (c) 2004, Tim Bailey
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+    * Redistributions of source code must retain the above copyright notice,
+      this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright notice,
+      this list of conditions and the following disclaimer in the documentation
+      and/or other materials provided with the distribution.
+    * Neither the name of the Player Project nor the names of its contributors
+      may be used to endorse or promote products derived from this software
+      without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+/* Simple 2D geometric operations with points, poses, and lines.
+ * Tim Bailey 2004.
+ */
+
+#include "geometry2D.h"
+#include <cmath>
+#include <cassert>
+#include <stdio.h>
+#include <iostream>
+
+using namespace std;
+
+namespace Geom2D {
+
+#if 0
+double pi_to_pi(double angle) 
+// An alternative implementation that uses fmod() rather than while-loops.
+{
+	angle = fmod(angle, 2.*PI);
+	if (angle < -PI)
+		angle += 2.*PI;
+	else if (angle > PI)
+		angle -= 2.*PI;
+	return angle;
+}
+#endif
+
+double dist(const Point& p, const Point& q) 
+// Distance between two points.
+{ 
+	return sqrt(dist_sqr(p,q)); 
+}
+
+Pose compute_relative_pose(const vector<Point>& a, const vector<Point>& b)
+// Determine the relative Pose that best aligns two point-sets.
+// INPUTS: a, b are two equal-length sets of points, such that a[i] aligns with b[i].
+// OUTPUT: Best-fit alignment.
+//
+// Closed-form algorithm from Appendix C of:
+// F. Lu and E. Milios. Robot pose estimation in unknown environments by matching
+// 2D range scans. Journal of Intelligent and Robotic Systems, 18:249�275, 1997.
+{
+	assert(a.size() == b.size() && a.size() != 0);
+
+	int n= a.size();
+	double x1,x2,y1,y2,xx,yy,xy,yx;
+
+	x1=x2=y1=y2=xx=yy=xy=yx=0.0;
+	for (int i=0; i<n; ++i) { // calculate sums
+		const Point& p1 = a[i];
+		const Point& p2 = b[i];
+
+		x1 += p1.x;
+		x2 += p2.x;
+		y1 += p1.y;
+		y2 += p2.y;
+		xx += p1.x*p2.x;
+		yy += p1.y*p2.y;
+		xy += p1.x*p2.y;
+		yx += p1.y*p2.x;
+	}
+
+	double N = static_cast<double>(n);
+
+	double Sxx = xx - x1*x2/N; // calculate S
+	double Syy = yy - y1*y2/N;
+	double Sxy = xy - x1*y2/N;
+	double Syx = yx - y1*x2/N;
+
+	double xm1 = x1/N; // calculate means
+	double xm2 = x2/N; 
+	double ym1 = y1/N; 
+	double ym2 = y2/N; 
+
+	double phi = atan2(Sxy-Syx, Sxx+Syy); 
+	
+	Pose pse; // calculate pose
+	pse.p.x = xm2 - (xm1*cos(phi) - ym1*sin(phi));
+	pse.p.y = ym2 - (xm1*sin(phi) + ym1*cos(phi));
+	pse.phi = phi;
+
+	return pse;
+}
+
+Transform2D::Transform2D(const Pose& ref) : base(ref) 
+{
+	c = cos(ref.phi);
+	s = sin(ref.phi);
+}
+
+bool intersection_line_line (Point& p, const Line& l, const Line& m) 
+// Compute the intersection point of two lines.
+// Returns false for parallel lines.
+{
+	double gl, gm, bl, bm;
+	bool lVert = true, mVert = true;
+
+	// calculate gradients 
+	if ((gl = l.second.x - l.first.x) != 0.0) {
+		gl = (l.second.y - l.first.y)/gl;
+		lVert = false;
+	}
+	if ((gm = m.second.x - m.first.x) != 0.0) {
+		gm = (m.second.y - m.first.y)/gm;
+		mVert = false;
+	}
+
+	if (lVert == mVert) { // check for parallelism 
+		if (gl == gm)
+			return false;
+	}
+
+	bl = l.first.y - gl*l.first.x; // calculate y intercepts 
+	bm = m.first.y - gm*m.first.x;
+
+	if (lVert) { // calculate intersection 
+		p.x = l.first.x;
+		p.y = gm*p.x + bm;
+	}
+	else if (mVert) {
+		p.x = m.first.x;
+		p.y = gl*p.x + bl;
+	}
+	else {
+		p.x = (bm - bl)/(gl - gm);
+		p.y = gm*p.x + bm;
+	}
+
+	return true;
+}
+
+double distance_line_point (const Line& lne, const Point& p)
+// Note: distance is -ve if point is on left of line and +ve if it is on 
+// the right (when looking from first to second). 
+{
+	Point v;
+	v.x= lne.second.x - lne.first.x;
+	v.y= lne.second.y - lne.first.y;
+
+	return ((lne.second.y - p.y)*v.x 
+		  - (lne.second.x - p.x)*v.y) 
+		  / sqrt(v.x*v.x + v.y*v.y);
+}
+
+void intersection_line_point(Point& p, const Line& l, const Point& q) 
+// Compute the perpendicular intersection from a point to a line
+{
+	Line m; 	// create line through q perpendicular to l
+	m.first = q;
+	m.second.x = q.x + (l.second.y - l.first.y);
+	m.second.y = q.y - (l.second.x - l.first.x);
+
+	bool not_parallel = intersection_line_line(p, l, m);
+	//assert(not_parallel);
+	if (not_parallel == 0)
+		printf("\n Lines are parallel !!!");
+}
+
+} // namespace Geom2D
diff --git a/sm/drivers/playerstage/mricp/src/icp.cpp b/sm/drivers/playerstage/mricp/src/icp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fa0730d6a852247603a29b5ffd0778bffdc15552
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/src/icp.cpp
@@ -0,0 +1,118 @@
+/*
+Copyright (c) 2004, Tim Bailey
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+    * Redistributions of source code must retain the above copyright notice,
+      this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright notice,
+      this list of conditions and the following disclaimer in the documentation
+      and/or other materials provided with the distribution.
+    * Neither the name of the Player Project nor the names of its contributors
+      may be used to endorse or promote products derived from this software
+      without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+/* Iterated closest point (ICP) algorithm.
+ * Tim Bailey 2004.
+ */
+#include "icp.h"
+#include <cassert>
+#include <iostream>
+using namespace std;
+
+namespace Geom2D
+{
+	ICP::ICP()
+		: warning_misalign(true)
+		{
+			index.push_back(0);
+			index.push_back(0);
+		};
+	ICP::~ICP()
+		{
+			a.clear();
+			b.clear();
+			index.clear();
+			ref.clear();
+		}
+
+Pose ICP::align(vector<Point> reference, vector<Point> obs,Pose init, double gate, int nits, bool interp)
+{
+	ref = reference;
+	nn = new SweepSearch(reference, gate);
+	Pose pse = init;
+	double gate_sqr = sqr(gate);
+	int size_obs = obs.size();
+
+	 // used if interp == true
+	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)
+		{
+			Point p = obs[i];
+			tr.transform_to_global(p); // transform obs[i] to estimated ref coord-frame
+
+			Point q;
+			// simple ICP
+			if (interp == false)
+			{
+				int idx = nn->query(p);
+				if (idx == SweepSearch::NOT_FOUND)
+					continue;
+
+				q = ref[idx];
+			}
+			// ICP with interpolation between 2 nearest points in ref
+			else
+			{
+				(void) nn->query(p, index);
+				assert(index.size() == 2);
+				if (index[1] == SweepSearch::NOT_FOUND)
+					continue;
+
+				Line lne;
+				lne.first  = ref[index[0]];
+				lne.second = ref[index[1]];
+				intersection_line_point(q, lne, p);
+			}
+			if (dist_sqr(p,q) < gate_sqr) // check if NN is close enough
+			{
+				a.push_back(obs[i]);
+				b.push_back(q);
+			}
+		}
+		//cout<<"\nObs size:"<<obs.size()<<" Ref:"<<ref.size()<<" Paired set size:"<<a.size();
+		//If Less than half of the Observation is paired => not good alignment
+		if( a.size() < obs.size()/2.0)
+		{
+			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;
+		}
+		if (a.size() > 2)
+			pse = compute_relative_pose(a, b); // new iteration result
+	}
+	delete nn;
+	return pse;
+}
+
+} // namespace Geom2D
diff --git a/sm/drivers/playerstage/mricp/src/lasermodel.cpp b/sm/drivers/playerstage/mricp/src/lasermodel.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c05dd8f31e5101dc3f6bbe74e7c46d439d4bd5ba
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/src/lasermodel.cpp
@@ -0,0 +1,140 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Tarek Taha                                      *
+ *   tataha@eng.uts.edu.au                                                 *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#include <assert.h>
+#include <math.h>
+#include <stdlib.h>
+#include "map.h"
+#include "lasermodel.h"
+#define LASER_MAX_RANGES 401
+// Create an sensor model
+LaserModel::LaserModel()
+{
+	//Empty Constructor
+};
+LaserModel::LaserModel(mapgrid * * map)
+{
+  	this->map = map;
+  	this->range_cov = 0.10 * 0.10;
+  	this->range_bad = 0.50;
+  	PreCompute();  
+  	this->range_count = 0;
+  	this->ranges = (laser_range_t *)calloc(LASER_MAX_RANGES, sizeof(laser_range_t));
+};
+// Free an sensor model
+LaserModel::~LaserModel()
+{
+	if(this->lut_probs)
+  		free(this->lut_probs);
+  	if(this->ranges)
+  		free(this->ranges);
+  	return;
+}
+// Clear all existing range readings
+void LaserModel::ClearRanges()
+{
+  	this->range_count = 0;
+  	return;
+}
+// Set the laser range readings that will be used.
+void LaserModel::AddRange(double range, double bearing)
+{
+  	laser_range_t *beam;
+  	assert(this->range_count < LASER_MAX_RANGES);
+  	beam = this->ranges + this->range_count++;
+  	beam->range = range;
+  	beam->bearing = bearing;
+  	return;
+}
+// Pre-compute the range sensor probabilities.
+// We use a two-dimensional array over the O.G
+void LaserModel::PreCompute()
+{
+  	double max;
+  	double c, z, p;
+  	double mrange, orange;
+  	int i, j;
+  	// Laser max range and resolution
+  	max = 8.00;
+  	this->lut_res = 0.01;
+  	this->lut_size = (int) ceil(max / this->lut_res);
+  	this->lut_probs = (double *)malloc(this->lut_size * this->lut_size * sizeof(this->lut_probs[0]));
+  	for (i = 0; i < this->lut_size; i++)
+  	{
+    	mrange = i * this->lut_res;
+   		for (j = 0; j < this->lut_size; j++)
+    	{
+      		orange = j * this->lut_res;
+	      	// TODO: proper sensor model (using Kolmagorov?)
+	      	// Simple gaussian model
+	      	c = this->range_cov;
+	      	z = orange - mrange;
+	      	p = this->range_bad + (1 - this->range_bad) * exp(-(z * z) / (2 * c));
+	      	//printf("%f %f %f\n", orange, mrange, p);
+	      	//assert(p >= 0 && p <= 1.0);
+	      	this->lut_probs[i + j * this->lut_size] = p;
+    	}
+    //printf("\n");
+  	}
+  	return;
+}
+// Determine the probability for the given range reading
+double LaserModel::RangeProb(double obs_range, double map_range)
+{
+  	int i, j;
+  	double p;
+  	i = (int) (map_range / this->lut_res + 0.5);
+  	j = (int) (obs_range / this->lut_res + 0.5);
+  	assert(i >= 0);
+  	if (i >= this->lut_size)
+    	i = this->lut_size - 1;
+  	assert(j >= 0);
+  	if (j >= this->lut_size)
+    	j = this->lut_size - 1;
+  	p = this->lut_probs[i + j * this->lut_size];
+  	//assert(p >= 0 && p <= 1.0);
+  	return p;
+}
+// Determine the probability for the given pose
+double LaserModel::PoseProb()
+{
+  	int i;
+  	double p;
+  	double map_range;
+  	laser_range_t *obs;
+  	p = 1.0; 
+  	for (i = 0; i < this->range_count; i++)
+  	{
+    	obs = this->ranges + i;
+    	map_range = 1; //TODO
+    	if (obs->range >= 8.0 && map_range >= 8.0)
+      		p *= 1.0;
+    	else if (obs->range >= 8.0 && map_range < 8.0)
+      		p *= this->range_bad;
+    	else if (obs->range < 8.0 && map_range >= 8.0)
+      		p *= this->range_bad;
+   	 	else
+      		p *= RangeProb(obs->range, map_range);
+  	}
+  //printf("%e\n", p);
+  assert(p >= 0);
+  return p;
+}
+
diff --git a/sm/drivers/playerstage/mricp/src/map.cpp b/sm/drivers/playerstage/mricp/src/map.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5102954df74270a241e7163f23ce0823144a1e8b
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/src/map.cpp
@@ -0,0 +1,197 @@
+/***************************************************************************
+ *   Copyright (C) 2005 by Tarek Taha                                      *
+ *   tataha@eng.uts.edu.au                                                 *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#include "map.h"
+#include <math.h>
+#include <stdlib.h>
+#include <cassert>
+#include <iostream>
+using namespace std;
+
+#if defined WIN32
+  #define snprintf _snprintf
+#endif
+
+MAP::MAP(char * Mapname, double pixel_size, double map_size)
+{
+  	this->Mapname = Mapname;
+  	this->pixel_size = pixel_size;
+  	this->negate   = TRUE;
+  	this->occ_grid = NULL;
+	this->pixbuf = NULL;
+  	this->size_x =  (int)ceil(map_size/pixel_size); // Square MAP
+  	this->size_y = this->size_x + 1;
+}
+void MAP::ResetProb()
+{
+	if (!this->occ_grid)
+	{
+		cout<<"\n Occupancy Grid not Allocated Yet!!!";
+		return;
+	}
+	for (int i=0; i < this->size_x; i++)
+    	for(int j =0; j < this->size_y;j++)
+    	{
+    		occ_grid[i][j].prob_occ = occ_grid[i][j].prob_free = double(1)/3;
+    		occ_grid[i][j].added =false;
+    	}
+}
+GdkPixbuf * MAP::CreateMap()
+{
+  	g_type_init();// Initialize glib
+	if (this->occ_grid)
+	{
+		for (int i=0; i < this->size_x; i++)
+    		delete  [] this->occ_grid[i];
+		delete [] this->occ_grid;
+	}
+	if (!this->pixbuf)
+	{
+  		cout<<"\n	--->>> Creating Image Pixel Buffer:"<<this->Mapname;
+  		fflush(stdout);
+  		if(!(this->pixbuf = gdk_pixbuf_new (GDK_COLORSPACE_RGB, FALSE, 8, this->size_x,this->size_y))) 	// Read the image
+  		{
+    			printf("\nfailed to Create Map Buffer %s", this->Mapname);
+    			return(NULL);
+  		}
+  		gdk_pixbuf_fill (pixbuf, 0x7f7f7f00);
+	}
+  	assert(this->size_x == gdk_pixbuf_get_width(this->pixbuf));
+	assert(this->size_y == gdk_pixbuf_get_height(this->pixbuf));
+	if (!(this->occ_grid= new mapgrid * [size_x]))
+		perror("\nCould not allocated the requested memory space");
+	for(int m=0;m<size_x;m++)
+	{
+		if(!(this->occ_grid[m] = new mapgrid [size_y]))
+			perror("\nCould not allocated the requested memory space");
+	}
+	ClearData();
+  	printf("\n	--->>> MAP Created with Height=%d Width=%d Resolution=%.3f <<<---",this->size_y, this->size_x, this->pixel_size);
+  	fflush(stdout);
+  	return this->pixbuf;
+}
+void MAP::ClearData()
+{
+	for (int i=0; i < this->size_x; i++)
+    	for(int j =0; j < this->size_y;j++)
+    	{
+    		this->occ_grid[i][j].prob_occ = this->occ_grid[i][j].prob_free = 1/3;
+    	}
+    if(pixbuf)
+   		gdk_pixbuf_fill (pixbuf, 0x7f7f7f00);
+}
+MAP::~ MAP()
+{
+	for (int i=0; i < this->size_x; i++)
+        	delete  [] this->occ_grid[i];
+    delete [] this->occ_grid;
+    if(this->pixbuf)
+    	gdk_pixbuf_unref(this->pixbuf);
+    cout <<"\n	<<<--- IMAGE DATA FREED <<<---";
+    fflush(stdout);
+};
+MapInfo MAP::GetMapInfo()
+{
+	MapInfo mapinfo={0};
+	if(this->occ_grid == NULL)
+		{
+		mapinfo.pixel_size=-1;
+		return mapinfo;
+		}
+  	mapinfo.width = this->size_x;
+  	mapinfo.height =this->size_y;
+	mapinfo.pixel_size=this->pixel_size;
+	return mapinfo;
+};
+void MAP::DrawPixel (int red,int green,int blue,int i,int j)
+{
+	int rowstride=0, n_channels, bps;
+  	guchar *pixels;
+  	guchar * p;
+  	rowstride = gdk_pixbuf_get_rowstride(this->pixbuf);
+  	bps = gdk_pixbuf_get_bits_per_sample(this->pixbuf)/8;
+  	n_channels = gdk_pixbuf_get_n_channels(this->pixbuf);
+  	pixels = gdk_pixbuf_get_pixels(this->pixbuf);
+  	if(gdk_pixbuf_get_has_alpha(this->pixbuf))
+	  	n_channels++;
+  	p= pixels +j*rowstride + i*n_channels;
+  	p[0]=red;
+  	p[1]=green;
+  	p[2]=blue;
+  	//p[3]=;
+	  return;
+}
+void MAP::SavePixelBufferToFile()
+{
+	char command[40],filename[40];
+	struct stat stat_buf;
+  	if(!this->pixbuf)
+	{
+		cout<<"		--->>> Nothing To SAVE Buffer Empty !!! ";
+		return;
+	}
+  	snprintf(filename,40, "%s%s",this->Mapname,".png");
+	// Delete the file if it exists
+ 	if (stat(filename,&stat_buf) != 0 || (stat_buf.st_mode & S_IFMT) == S_IFREG)
+	{
+		snprintf(command,40,"%s%s","rm -f -r ",filename);
+		if(system(command)==-1)
+		{
+			perror("\nError Happened while trying to Delete Existing File");
+			exit(1);
+		}
+		else
+			cout<<"\n	--->>> Map already existed with the same name : Deleted Successfully";
+	}
+	cout<<"\n	--->>> Saving the map into: "<<filename; fflush(stdout);
+	// Save the file
+	gdk_pixbuf_save(this->pixbuf,filename,"png",NULL,NULL);
+  	cout<<"\n	--->>> PIXEL BUFFER SAVED <<<---	"; fflush(stdout);
+};
+// Save as PGM format
+int MAP::SavePgm()
+{
+  char filename[40];
+  int i, j;
+  signed char c;
+  unsigned char d;
+  FILE *file;
+  snprintf(filename,40,"%s%s",this->Mapname,".pgm");
+  file = fopen(filename, "w+");
+  if (file == NULL)
+  {
+    fprintf(stderr, "error writing to %s ", filename);
+    return -1;
+  }
+
+  fprintf(file, "P5 %d %d 255\n",this->size_x , this->size_y -1);
+
+  for (j = 0; j < this->size_y-1; j++)
+  {
+    for (i = 0; i < this->size_x; i++)
+    {
+	    c = (int)(occ_grid[i][j].prob_occ * 255.0);
+	    d = (unsigned char) (255 - c);
+        fwrite(&d, 1, 1,  file);
+    }
+  }
+  cout<<"\n		--->>> Pgm Map Saved <<<--- "; fflush(stdout);
+  fclose(file);
+  return 0;
+};
diff --git a/sm/drivers/playerstage/mricp/src/mricp_driver.cpp b/sm/drivers/playerstage/mricp/src/mricp_driver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9a4826f8772bae09d537eef94f46e03c59c5fd3c
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/src/mricp_driver.cpp
@@ -0,0 +1,1411 @@
+/*
+ *  Player - One Hell of a Robot Server
+ *  Copyright (C) 2000-2003
+ *     Andras Szekely, Aaron Skelsey, Kent Williams
+ *
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+/*
+ * mricp_driver.cpp, v3.0 21/12/2007
+ * This is a Map Reference ICP plugin Driver for real time Map building and
+ * Localization using Iterative Closest Point laser scan matching and
+ * odom correction. Currently The driver is in stable release stage, more modifications
+ * might be added later on.
+ *           By Tarek Taha and Jonathan Paxman / Centre of Autonomous Systems University of Technology Sydney
+ */
+#include <sys/types.h>
+#include <sys/stat.h>
+#if defined WIN32
+  #define snprintf _snprintf
+#else
+  #include <unistd.h>
+  #include <sys/time.h>
+  #include <termios.h>
+  #include <netinet/in.h>
+#endif
+#include <fcntl.h>
+#include <cstdio>
+#include <cstdlib>
+#include <cstddef>
+#include <cerrno>
+#include <cstring>
+#include <string>
+#include <pthread.h>
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <csignal>
+#include <vector>
+// Player includes
+#include <libplayercore/playercore.h>
+
+#include <cassert>
+#include "csmpp.h"
+#include "icp.h"
+#include "map.h"
+#include "lasermodel.h"
+#include "Timer.h"
+
+using namespace std;
+using namespace Geom2D;
+
+#define MAP_IDX(mf, i, j) ((mf->map_size) * (j) + (i))// compute linear index for given map coords
+#define MAP_VALID(mf, i, j) ((i >= 0) && (i < mf->map_size*2/mf->map_resolution) && (j >= 0) && (j <= mf->map_size*2/mf->map_resolution))
+#define MAXLASERS 4
+#define MAXRANGES 10
+/** @ingroup drivers */
+/** @{ */
+/** @defgroup ComponentNavigator MRICP
+ * @brief Map Reference ICP
+
+This is a Map Reference ICP plugin Driver for real time Map building and
+Localization using Iterative Closest Point laser scan matching and
+odom correction. Currently The driver is in stable release stage, more modifications
+might be added later on.
+
+This driver is created to support UTS CAS mobile Platforms. This driver can be quite
+usefull in a lot of applications where odom correction or matching laser scans is
+essential. I would appreciate any feedback and recommendations that can lead to improving
+the performance of this driver.
+
+@par Compile-time dependencies
+
+- none
+
+@par Provides
+
+The MRICP driver provides the following device interfaces, some of
+them named:
+
+- "Position" @ref player_interface_position
+  - This interface returns odometry data.
+- "Opaque"   @ref player_icp_plugin_interface
+  - This is a dummy interface supporting requests and commands to the ICP / TODO
+- "Map"   @ref player_map_interface
+  - This interface supports map data requests / TODO
+
+@par Supported configuration requests
+
+- "Position" @ref player_interface_position:
+  - PLAYER_POSITION_SET_ODOM_REQ
+  - PLAYER_POSITION_RESET_ODOM_REQ
+  - PLAYER_POSITION_GET_GEOM_REQ
+  - "Map" @ref player_interface_map:
+  - PLAYER_POSITION_GET_GEOM_DATA
+- @ref player_icp_plugin_interface (not clear now, i will know it later)
+  - PLAYER_ICP_X_REQ
+  - PLAYER_ICP_Y_REQ
+
+
+@par Configuration file options
+
+- MAXR (double)
+  - Default: "7.8"
+  - Maximium Laser Range
+- MINR (double)
+  - Default: "0.05"
+  - Minimium Laser Range
+- period (double)
+  - Default: "0.5"
+  - Time in sec between scans to be matched.
+- map_resolution (double)
+  - Default: "0.05"
+  - Pixel resolution in meter of the map to be build
+- map_size (double)
+  - Default: 20
+  - This is defined from the origin to the boundary, so is actually half the size of the
+    map PATCH, not the whole map.
+- interpolate (bool)
+  - Default "1"
+  - 0 - Simple ICP, 1 - ICP with interpolation
+- NIT (int)
+  - Default "10"
+  - Number of iterations for each scan-matching.
+- gate1 (float)
+  - Default "0.5"
+  - 1st data association gate for each point in scan
+- gate2 (float)
+  - Default "0.05"
+  - 2nd data association gate for each point in scan
+- debug (bool)
+  - 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)
+- map_path(string)
+  - Default: "maps/"
+  - Specifies the locations where patches and logs are to be saved
+- start_in(int)
+  - Default : 2
+  - Delay Before starting, unit is in seconds
+- robot_id(int)
+  - Default : 0
+  - The Robot id assigned for map identification
+- number_of_laser(int)
+  - Default : 1
+  - The number of lasers to be used in the scan matching (index starts from 0) all lasers
+    should be declared in the requires section
+- playerv_debug (bool)
+  - Default : 0
+  - If enabled, the map occupancy will be represented by +1, 0, -1 (occupied, unoccupied,
+    unknown), other wise, the probability will be scaled from 0 - 255
+- laserX_ranges tuple of (int) where X is an int
+  - Default : [-90 90]
+  - Determines the acceptable laser scan ranges, even number of elements should exist in
+    the tuple, smaller range should be first followed by the larger range.
+    You will have to manually assign the value of X:
+    eg. in ur configuration file u should have something like this for 2 lasers:
+        number_of_lasers 2
+        laser0_ranges [-90 -70 -50 -30 -10 90]
+        laser1_ranges [-120 120]
+    - this represent the following acceptable ranges:
+      for Laser:0 [-90 -70] [-50 -30] [-10 90]
+      for laser:1 [-120 120]
+- use_max_range (float)
+  - Default: 0
+  - Specify if you want to use ranges more than the max to update empty spaces in
+    Occupancy grid map, if it's not zero , then the range specified will be used to
+    update the cells within that range only (usefull to reduce the effect of false returns)
+- sparse_scans_rate (int)
+  - Default: 1
+  - Specifies the number of laser scan samples resolution, 1 means all beams, 2 means every
+    take one every 2 and so on. (OG will not be affected by this, all samples will be use for OG)
+    it reduces the CPU usage.
+- use_odom (bool)
+  - Default: 0
+  - Specify if you want to use the underlying poisition driver's odom in laser scan correction
+    the existance of an underlying position driver no longer means that u are using the odom
+    automatically since it can be used for passing velocity commands.
+- free_space_prob (float) between 0 and 1
+  - Default: 0.4
+  - The probability to be used for updating the free space , lower values will help in reducing
+    the false readings effect.
+- map_saving_period (float)
+  - Default : 10 sec
+  - Determines how long time we should wait before saving the map.
+@par Example
+
+@verbatim
+driver
+(
+  name "mricp"
+  provides ["position2d:1" "map:0"]
+  requires ["position2d:0" "laser:0"]
+  number_of_lasers 1
+  laser0_ranges [-120 120]
+  playerv_debug 0
+  period 0.2
+  MAXR 3.9
+  MINR 0.05
+  map_resolution 0.05
+  map_saving_period 5
+  map_size 10
+  use_max_range 4
+  sparse_scans_rate 3
+  map_path "logs/"
+  debug 0
+  warning 1
+  alwayson 1
+  log 1
+)
+@endverbatim
+
+@par Authors
+
+Tarek Taha - Centre of Autonomous Systems - University of Technology Sydney
+*/
+/** @} */
+  /////////////////////////////////////////////////////////////
+ ///                   MRICP DRIVER Class                  ///
+/////////////////////////////////////////////////////////////
+typedef struct laser_range
+{
+	int begins;
+	int ends;
+} laser_ranges_t;
+bool is_file(string fname)
+{
+	//cout<<fname<<endl;
+	struct stat stat_buf;
+ 	if (stat(fname.c_str(),&stat_buf) != 0) return false;
+  	return (stat_buf.st_mode & S_IFMT) == S_IFREG;
+}
+bool is_directory(string fname)
+{
+	struct stat stat_buf;
+  	if (stat(fname.c_str(),&stat_buf) != 0) return false;
+  	return (stat_buf.st_mode & S_IFMT) == S_IFDIR;
+}
+class MrIcp : public ThreadedDriver
+ {
+	// Must implement the following methods.
+  	public :
+	    virtual int MainSetup();
+	    virtual void MainQuit();
+	    virtual int ProcessMessage(QueuePointer& resp_queue, player_msghdr * hdr, void * data);
+	// Constructor
+	public:  	MrIcp(ConfigFile* cf, int section);
+	// Main function for device thread.
+    private:
+    		virtual void Main();
+			int  HandleConfigs(QueuePointer& resp_queue,player_msghdr * hdr,void * data);
+			int  HandleCommands(QueuePointer& resp_queue,player_msghdr * hdr,void * data);
+			int  HandleData(QueuePointer& resp_queue, player_msghdr * hdr, void * data);
+ 		 	void RefreshData();     //refreshs and sends data
+ 		 	void AddToMap(vector <Point> points_to_add,Pose p); // Add points to Map
+ 		 	void ResetMap();		// Reset the Map and empty all the point cloud
+ 		 	Point TransformToGlobal(Point ,Pose p);
+ 		 	Pose  TransformToGlobal(Pose ,Pose p);
+			Point ConvertToPixel(Point p);
+			Point ConvertPixel(Point p);
+ 		 	int  InRange(double angle,int laser_index);
+ 		 	void BuildMap();
+ 		 	int SetupLaser(int);
+			void SetupPositionDriver();
+			void GenerateLocalMap(Pose pse);
+			void ConnectPatches();
+			mapgrid_t ComputeRangeProb(double range,bool);
+			int ProcessMapInfoReq(QueuePointer& resp_queue,player_msghdr * hdr,void * data);
+			int ProcessMapDataReq(QueuePointer& resp_queue,player_msghdr * hdr,void * data);
+	// Position interface / IN
+  	private:
+  			player_devaddr_t          position_in_addr;
+			player_position2d_geom_t  geom;
+			Device  			     *position_device;
+	// Position interface / OUT
+  	private:
+  			player_devaddr_t         position_out_addr;
+  		 	player_position2d_data_t position_out_data;
+	// Lasers interfaces
+  	private:
+			// Supports MAXLASERS Lasers
+  			player_devaddr_t       laser_addr[MAXLASERS];
+			player_laser_geom_t    *laser_geom;
+			// Used to communicate with the laser Driver
+			Device                 *laser_device[MAXLASERS];
+	// Map interface
+  	private:
+  			player_devaddr_t 	   map_addr;
+  		 	player_map_data_t 	   map_data;
+			player_map_info_t      map_info;
+	// Variables
+	public :
+			FILE *file,*config_file;
+			char * map_path;
+			int   robot_id,scan_count,nit,nu,map_number,number_of_lasers,
+				  range_count[MAXLASERS],sparse_scans_rate;
+			// defines a set of ranges (max 10) for each attached laser
+		 	laser_range range[MAXLASERS][MAXRANGES];
+			float maxr,minr,period,map_resolution,gate1,gate2,map_size,map_saving_period,
+				  use_max_range,local_map_margine;
+		    float PoseX,PoseY,PoseTheta; // Laser Pose
+			double px, py, pa,speed,turn_rate,delta_time,start_in,free_space_prob;
+			bool log,debug,interpolate,sample_initialized,
+			     playerv_debug,position_in_exists,use_odom,reset_timer,warning_misalign;
+			struct timeval last_time[MAXLASERS],current_time,laser_timestamp,position_timestamp,
+				   loop_start,loop_end,map_timestamp,last_delta,map_current,map_saved;
+			ICP icp;
+			Pose laser_pose[MAXLASERS],pose_1,pose_2,delta_pose,global_pose,global_pose_prev,
+				 global_diff,relative_pose,P;
+			vector<Point> laser_set,laser_set_1,laser_set_2,local_map,occ_laser_set,map_points;
+			MAP *map;
+			MricpTimer delta_t_estimation;
+                        CanonicalScanMatcher csm;
+};
+Driver* MrIcp_Init(ConfigFile* cf, int section) // Create and return a new instance of this driver
+{
+  	return ((Driver*) (new MrIcp(cf, section)));
+}
+
+void mricp_Register(DriverTable* table)
+{
+  	table->AddDriver("mricp", MrIcp_Init);
+}
+
+/* need for this driver to be a plugin  */
+extern "C"
+{
+  int player_driver_init(DriverTable* table)
+  {
+    puts("	--->>>Initializing Pluggin Driver ==>  MRICP Driver ...");
+    mricp_Register(table);
+    return(0);
+  }
+}
+
+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->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);
+	this->use_max_range =    cf->ReadFloat(section,  "use_max_range",0);
+	this->sparse_scans_rate= cf->ReadInt(section,  "sparse_scans_rate",1);
+	this->free_space_prob =  cf->ReadFloat(section,"free_space_prob",0.4);
+	this->map_saving_period= cf->ReadFloat(section,"map_saving_period",10);
+
+	if (sparse_scans_rate <= 0 )
+	{
+		cout <<"\nSparse Scans Rate should be positive integer > 0";
+		exit(1);
+	}
+	if(free_space_prob < 0 || free_space_prob >1)
+	{
+		cout <<"\nFree space probability should be between 0 and 1";
+		exit(1);
+	}
+	for(int k = 0; k < number_of_lasers ; k++)
+	{
+		snprintf(config_temp,40,"%s%d%s","laser",k,"_ranges");
+		// Expects you to provide "laserx_ranges" (where x is the laser index)
+		// in the configuration file
+		this->range_count[k] =	 cf->GetTupleCount(section,config_temp);
+		if ((range_count[k]%2)!=0)
+		{
+			cout<<"\n ERROR: Number of tuples in the ranges for Laser:"<<k<< "should be even !!!";
+			exit(1);
+		}
+		int index=0;
+		for(int i=0;i<range_count[k];i+=2)
+		{
+			this->range[k][index].begins =	 cf->ReadTupleInt(section,config_temp,i ,-90);
+			this->range[k][index].ends   =	 cf->ReadTupleInt(section,config_temp,i+1,90);
+			if(this->range[k][index].begins > this->range[k][index].ends)
+			{
+				cout<<"\n ERROR: the beginning range SHOULd be less than the end range !!!";
+				exit(1);
+			}
+			cout<<"\n Laser:"<<k<<" Range:"<<index<<" begins:"<<range[k][index].begins;
+			cout<<" ends:"<<range[k][index].ends;
+			index ++;
+		}
+		range_count[k]/=2;
+	}
+    // Adding position interface
+    // Do we create a robot position interface?
+    if(cf->ReadDeviceAddr(&(this->position_out_addr), section, "provides", PLAYER_POSITION2D_CODE, -1, NULL) == 0)
+    {
+		if (this->AddInterface(this->position_out_addr))
+	  	{
+	    	this->SetError(-1);
+	    	return;
+	  	}
+  	  	if(this->debug)
+			cout<<"\n Position out Interface Loaded";
+  	}
+  	// Adding MAP interface
+  	// Do we create a MAP interface?
+    if(cf->ReadDeviceAddr(&(this->map_addr), section, "provides", PLAYER_MAP_CODE, -1, NULL) == 0)
+    {
+		if (this->AddInterface(this->map_addr))
+	  	{
+	    	this->SetError(-1);
+	    	return;
+	  	}
+		if(this->debug)
+			cout<<"\n MAP Interface Loaded";
+  	}
+  	// Adding LASER interfaces
+    if(this->debug)
+    	cout<<"N of Lasers:"<<number_of_lasers;
+	for(int i=0; i<this->number_of_lasers;i++)
+	{
+  		if(cf->ReadDeviceAddr(&this->laser_addr[i], section, "requires", PLAYER_LASER_CODE,-1, NULL) == 0)
+  		{
+			SetupLaser(i); // Here we initialize the talk to the laser driver
+			cout<<"\n LASER Interface Loaded Success index:"<<i; fflush(stdout);
+	  		if(this->debug)
+				cout<<"\n LASER Interface Loaded";
+  		}
+  		else
+		{
+			cout<<"\n Error Reading Laser on index:"<<i;
+			fflush(stdout);
+	    	this->SetError(-1);
+	    	return;
+		}
+	}
+   // Adding position interface
+   // Do we create a position interface?
+	if (cf->ReadDeviceAddr(&this->position_in_addr, section, "requires",PLAYER_POSITION2D_CODE, -1, NULL) == 0)
+	{
+		SetupPositionDriver();
+		if(this->debug)
+			cout<<"\n Position IN Interface Loaded";
+	}
+  	else
+  	{
+  		if (this->use_odom)
+  		  		cout<<"\n Can't use odom when you don't have an underlying position driver !!!";
+  		position_in_exists = false;
+  		this->position_device = NULL;
+  	}
+	if(this->debug)
+		cout<<"\n	--->>>Gate1 ="<<gate1<<" Gate2="<<gate2<<" NIT="<<nit<<" MAXR="<<maxr<<" MINR="<<minr<<endl;
+  	return;
+}
+
+int MrIcp::MainSetup()
+{
+	char filename[40],command[40];
+	printf("\n- Setting UP MRICP Plugin Driver.");
+	if(!is_directory(map_path))
+	{
+		snprintf(command,40,"%s%s","mkdir ",map_path);
+		if(system(command)==-1)
+		{
+			perror("\n Serious Error Happened while trying to create the folder");
+			exit(1);
+		}
+		else
+			cout<<"\nFolder Created Successfully";
+	}
+	// allocate space for map cells
+  	//assert(this->map_data = (char*)malloc(sizeof(char)*this->map_size*this->map_size));
+  	for(int i=0;i<number_of_lasers;i++)
+		gettimeofday(&last_time[i],NULL);
+	this->global_pose.p.x = this->global_pose.p.y = this->global_pose.phi = 0;
+	// what extra area(margine) in the stored map around the local map should be included in the ICP
+	this->local_map_margine = 0.5;
+	this->px=0;
+	this->py=0;
+	this->pa=0;
+	nu = 0; map_number = 1;
+	gchar * g_filename=g_strdup_printf("%sMAP_PATCH0",map_path);
+	this->map = new MAP(g_filename,this->map_resolution,this->map_size*2);
+	this->map->CreateMap();
+	this->map->ResetProb();
+	snprintf(filename,40,"%spatch_config.txt",map_path);
+	config_file = fopen(filename,"wb");
+	// Initial Patch Settings
+	fprintf(config_file,"%s %.3f %.3f %.3f\n","MAP_PATCH0",0.0,0.0,0.0);
+	delta_pose.p.x=0;
+	delta_pose.p.y=0;
+	delta_pose.phi=0;
+	reset_timer= true;
+	sample_initialized = FALSE;
+	if(log)
+	{
+        snprintf(filename,40, "%sicplog.txt",map_path);
+		file=fopen(filename,"wb");
+	}
+	usleep((int)(this->start_in*1e6));
+	return(0);
+};
+void MrIcp::SetupPositionDriver()
+{
+	Pose initial_pose={{0,0,0}};
+	// Subscribe to the underlyin odometry device
+	if(!(this->position_device = deviceTable->GetDevice(this->position_in_addr)))
+	{
+		PLAYER_ERROR("unable to locate suitable position device");
+	    return ;
+	}
+	if(this->position_device->Subscribe(this->InQueue) != 0)
+	{
+		PLAYER_ERROR("unable to subscribe to position device");
+	    return ;
+	}
+	position_in_exists = true;
+ 	// Get the odometry geometry
+  	Message* msg;
+  	if(!(msg = this->position_device->Request(this->InQueue,PLAYER_MSGTYPE_REQ,PLAYER_POSITION2D_REQ_GET_GEOM,NULL, 0, NULL,false))
+  	  ||(msg->GetHeader()->size != sizeof(player_position2d_geom_t)))
+  	{
+    	PLAYER_ERROR("failed to get geometry of underlying position device");
+    	if(msg)
+      		delete msg;
+    	return;
+  	}
+  	memcpy(&geom,(player_position2d_geom_t *)msg->GetPayload(),sizeof(geom));
+// 	geom = (player_position2d_geom_t *)msg->GetPayload();
+//	initial_pose = GetOdomReading();
+	this->px = initial_pose.p.x;
+	this->py = initial_pose.p.y;
+	this->pa = initial_pose.phi;
+};
+
+int MrIcp::SetupLaser(int index)
+{
+	// Subscribe to the Laser device
+  	if (!(this->laser_device[index] = deviceTable->GetDevice(this->laser_addr[index])))
+  	{
+    	PLAYER_ERROR("unable to locate suitable laser device");
+    	return -1;
+  	}
+  	if (this->laser_device[index]->Subscribe(this->InQueue) != 0)
+  	{
+    	PLAYER_ERROR("unable to subscribe to laser device");
+    	return -1;
+  	}
+  	// Ask for the laser's geometry
+  	Message* msg;
+  	if((msg = laser_device[index]->Request(this->InQueue,PLAYER_MSGTYPE_REQ,PLAYER_LASER_REQ_GET_GEOM,NULL, 0, NULL,false)))
+  	{
+    	laser_geom = (player_laser_geom_t *)msg->GetPayload();
+	  	// Get the laser pose relative to the robot center of Rotation
+  		laser_pose[index].p.x = laser_geom->pose.px;
+  		laser_pose[index].p.y = laser_geom->pose.py;
+  		laser_pose[index].phi = laser_geom->pose.pyaw;
+	  	//if (this->debug)
+		  	cout<<"\n Laser["<<index<<"] Pose --> X="<<laser_pose[index].p.x<<" Y="<<laser_pose[index].p.y<<" Theta="<<laser_pose[index].phi;
+    	delete msg;
+    	return 0;
+  	}
+  	return -1;
+};
+void MrIcp::ResetMap()
+{
+	char filename[40];
+	gchar * savefile;
+	sample_initialized = FALSE;
+	laser_set_1.clear();
+	laser_set_2.clear();
+	map_points.clear();
+	//map->SavePixelBufferToFile();
+	map->SavePgm();
+	map->ClearData();
+	snprintf(filename,40,"MAP_PATCH%d",map_number++);
+  	savefile=g_strdup_printf("%s%s",map_path,filename);
+  	map->Mapname = (char *)realloc(map->Mapname,strlen(savefile)*(sizeof(savefile[0])));
+ 	strcpy(map->Mapname,savefile);
+	this->map->ResetProb();
+	// Save Patch's Name + origin in terms of the previous Patch
+	fprintf(config_file,"%s %.3f %.3f %.3f\n",filename,global_pose.p.x,global_pose.p.y,global_pose.phi);
+	this->global_pose.p.x = this->global_pose.p.y = this->global_pose.phi = 0;
+}
+void MrIcp::MainQuit()
+{
+	// Stop and join the driver thread
+	cout<<"\n- Shutting Down MRICP Driver - Cleaning up Mess ..\n"; fflush(stdout);
+	for(int i=0;i<number_of_lasers;i++)
+	{
+    	this->laser_device[i]->Unsubscribe(this->InQueue);
+    	this->laser_device[i] = NULL;
+	}
+	if(position_in_exists)
+		this->position_device->Unsubscribe(this->InQueue);
+    //this->map->SavePixelBufferToFile();
+    this->map->SavePgm();
+    delete this->map;
+	cout<<"\n	--->>> MAP Buffer Deleted->"; fflush(stdout);
+	this->occ_laser_set.clear();
+    this->map_points.clear();
+    this->laser_set_1.clear();
+    this->laser_set_2.clear();
+    this->local_map.clear();
+	cout<<" Vectors Cleared ->"; fflush(stdout);
+	if(log)
+		fclose(file);
+	fclose(config_file);
+	cout<<" Files Closed ->"; fflush(stdout);
+	//ConnectPatches();
+	cout<<" Thread Killed ->"; fflush(stdout);
+	cout<<" ... ShutDown FINISED\n"; fflush(stdout);
+
+};
+// this function will run in a separate thread
+void MrIcp::Main()
+{
+	MricpTimer loop_timer,map_timer,test;
+	double time_elapsed;
+	// Synchronously cancelable thread.
+	//pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);
+
+	/* To use the MricpTimer.Synch() in a loop you will have to
+	 * reset the timer at the beginning of the loop then call
+	 * the MricpTimer.Synch(period) at the end of the loop with
+	 * period representing the synchronization time in msec,
+	 * this method will check what is the total time spend in
+	 * this loop and then sleep the remaining period time
+	 * (if the time taken by the loop is less than the allowed period)
+	 */
+	while(1)
+	{
+		loop_timer.Reset();
+		test.Reset();
+		pthread_testcancel();     // Thread cancellation point.
+	    this->ProcessMessages();
+		BuildMap();		          // Launch the MrICP on two laser scans
+		RefreshData();            // Update Data
+		time_elapsed = loop_timer.TimeElapsed();
+		if(this->debug)
+			cout<<"\n Min Loop took="<<time_elapsed/1e3<<"usec";
+		loop_timer.Synch(this->period*1e3);
+		time_elapsed = map_timer.TimeElapsed();
+		if( time_elapsed >= this->map_saving_period*1e6)
+		{
+		    this->map->SavePgm();
+			map_timer.Reset();
+		}
+		time_elapsed = loop_timer.TimeElapsed();
+		if(this->debug)
+			cout<<"\n Time Synch="<<time_elapsed/1e3<<"usec";
+	}
+	pthread_exit(NULL);
+}
+/*! Forwards the Messeges  from the messege queue to their
+ *  specific handler
+ */
+int MrIcp::ProcessMessage(QueuePointer& resp_queue, player_msghdr * hdr, void * data)
+{
+  	// Forward the Messages
+  	switch (hdr->type)
+  	{
+  		case PLAYER_MSGTYPE_REQ:
+	    	return(this->HandleConfigs(resp_queue,hdr,data));
+	    case PLAYER_MSGTYPE_CMD:
+	    	return(this->HandleCommands(resp_queue,hdr,data));
+	    case PLAYER_MSGTYPE_DATA:
+	    	return(this->HandleData(resp_queue,hdr,data));
+	    default:
+	    	return -1;
+  	}
+};
+/*! Gets the Laser and Position Data from the underlying Devices
+ *  If they are provided.
+ */
+int MrIcp::HandleData(QueuePointer& resp_queue, player_msghdr * hdr, void * idata)
+{
+  	struct timeval currtime;
+  	double t1,t2, min_angle, scan_res,r,b,time_diff;
+	Point p;
+	//Clear the previous Laser set used for the occupance grid generation
+	occ_laser_set.clear();
+	laser_set.clear();
+  	if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,PLAYER_POSITION2D_DATA_STATE, this->position_in_addr))
+  	{
+  		player_position2d_data_t* data = reinterpret_cast<player_position2d_data_t*> (idata);
+  		// Compute new robot pose from Underlying Position Interface.
+  		P.p.x = data->pos.px;
+  		P.p.y = data->pos.py;
+  		P.phi = data->pos.pa;
+		if(this->debug)
+			cout<<"\n	--->>> Odom pose from Position:0 XYTheta=["<<P.p.x<<"]["<<P.p.y<<"]["<<P.phi<<"]";
+		return 0;
+  	}
+  	for(int index=0;index<number_of_lasers;index++)
+  	{
+		if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,PLAYER_LASER_DATA_SCAN, this->laser_addr[index]))
+		{
+			gettimeofday(&currtime,NULL);
+			t1 = ((double) currtime.tv_sec  + (double) currtime.tv_usec/1e6);
+			t2 = ((double) last_time[index].tv_sec + (double) last_time[index].tv_usec/1e6);
+		    time_diff = t1 -t2;
+		  	if (time_diff < 0.1)
+		  	{
+		  		if(this->debug)
+		  			cout<<"\n	--->>> Time Since Last Read is= "<<(t1-t2)<<" msec";
+		    	continue;
+		  	}
+		  	this->last_time[index] = currtime;
+	  		player_laser_data_t* data = reinterpret_cast<player_laser_data_t*> (idata);
+	  		min_angle = data->min_angle;
+	  		scan_res  = data->resolution;
+	  		this->scan_count = data->ranges_count;
+		    if(this->debug)
+		    	cout<<"\n Scan Count="<<this->scan_count;
+		  	for(int i=0;i < scan_count ;i++)
+		  	{
+		    	// convert to mm, then m, according to given range resolution
+			    r = data->ranges[i];
+		    	b = min_angle + (i * scan_res); 		    // compute bearing
+		    	//cout<<"\n Bearing:"<<RTOD(b);
+		    	if(InRange(b,index)!=0)
+		    		continue;
+			    // Transfer from Polar to Cartesian coordinates
+		    	p.x = r * cos(b);
+		    	p.y = r * sin(b);
+		    	p.laser_index = index;
+		    	// Transfer all the laser Reading into the Robot Coordinate System
+		    	p = TransformToGlobal(p,laser_pose[index]);
+				// Filter max ranges for alignment and use all for Occ-grid
+		    	if (this->use_max_range!=0)
+		    	{
+			    	if (r >= this->minr && r <= this->maxr && (i%sparse_scans_rate)==0)
+		  				laser_set.push_back(p);
+		  			occ_laser_set.push_back(p);
+		  		}
+		  		else
+		  		{
+		    		// Use only informative data for the scan allignement and Occ-grid
+			    	if (r >= this->minr && r <= this->maxr)
+			    	{
+			    		// Get samples according to sparse resolution
+			    		if ((i%sparse_scans_rate)==0)
+		  					laser_set.push_back(p);
+		  				// Use All the samples for OG-Map
+		  				occ_laser_set.push_back(p);
+			    	}
+		  		}
+		  	}
+			return 0;
+		}
+  	}
+  	return -1;
+};
+int MrIcp::HandleConfigs(QueuePointer &resp_queue,player_msghdr * hdr,void * data)
+{
+	// Handle Position REQ
+	// I didn't like the stupid MessageMatch Method
+	// check for position config requests
+	if(
+	   (hdr->type == (uint8_t)PLAYER_MSGTYPE_REQ) && (hdr->addr.host   == position_out_addr.host)   &&
+	   (hdr->addr.robot  == position_out_addr.robot) &&  (hdr->addr.interf == position_out_addr.interf) &&
+       (hdr->addr.index  == position_out_addr.index)
+       )
+    {
+		switch (hdr->subtype)
+		{
+		case PLAYER_POSITION2D_REQ_GET_GEOM:
+			/* Return the robot geometry. */
+		    if(hdr->size != 0)
+		    {
+		      PLAYER_WARN("Arg get robot geom is wrong size; ignoring");
+		      return(-1);
+			}
+	  		if(position_in_exists)
+	  		{
+		    	this->Publish(this->position_out_addr, resp_queue,PLAYER_MSGTYPE_RESP_ACK,PLAYER_POSITION2D_REQ_GET_GEOM, (void*)&geom);
+			    return 0;
+	  		}
+	  		else // Default Value if Position:0 not found
+	  		{
+			    geom.pose.px = 0.3;
+			    geom.pose.py = 0.0;
+			    geom.pose.pyaw = 0.0;
+		    	geom.size.sl = 1.2;
+		    	geom.size.sw = 0.65;
+		    	this->Publish(this->position_out_addr, resp_queue,PLAYER_MSGTYPE_RESP_ACK,PLAYER_POSITION2D_REQ_GET_GEOM,
+			    			  (void*)&geom, sizeof(geom), NULL);
+			    return 0;
+	  		}
+			return -1;
+		case PLAYER_POSITION2D_REQ_SET_ODOM:
+		    if(hdr->size != sizeof(player_position2d_set_odom_req_t))
+		    {
+		      puts("Arg get robot geom is wrong size; ignoring");
+		      return(-1);
+			}
+	    	player_position2d_set_odom_req_t * set_odom_req;
+	    	set_odom_req = (player_position2d_set_odom_req_t*) data;
+	    	this->Publish(this->position_in_addr, resp_queue,PLAYER_MSGTYPE_REQ,PLAYER_POSITION2D_REQ_SET_ODOM,
+		    			  (void*)set_odom_req, sizeof(set_odom_req), NULL);
+			break;
+		case PLAYER_POSITION2D_REQ_RESET_ODOM:
+		    	/* reset position to 0,0,0: no args */
+			    if(hdr->size != 0)
+			    {
+			      PLAYER_WARN("Arg to reset position request is wrong size; ignoring");
+			      return(-1);
+			    }
+				else
+				ResetMap();
+			    this->Publish(this->position_out_addr, resp_queue,PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_RESET_ODOM);
+			    return(0);
+		case PLAYER_POSITION2D_REQ_MOTOR_POWER:
+			    /* motor state change request
+			     *   1 = enable motors
+			     *   0 = disable motors (default)
+			     */
+			    if(hdr->size != sizeof(player_position2d_power_config_t))
+			    {
+			      PLAYER_WARN("Arg to motor state change request wrong size; ignoring");
+			      return(-1);
+			    }
+		    	this->Publish(this->position_in_addr, resp_queue,PLAYER_MSGTYPE_REQ,PLAYER_POSITION2D_REQ_SET_ODOM,
+		    			  (void*)data, sizeof(data), NULL);
+
+	        	break;
+		default:
+				PLAYER_WARN("\nreceived unknown config type ");
+	    		return -1;
+		}
+  	}
+	// check for MAP config requests
+	if(
+	   (hdr->type == (uint8_t)PLAYER_MSGTYPE_REQ) && (hdr->addr.host   == map_addr.host)   &&
+	   (hdr->addr.robot  == map_addr.robot) &&  (hdr->addr.interf == map_addr.interf) &&
+       (hdr->addr.index  == map_addr.index)
+       )
+    {
+    	switch(hdr->subtype)
+  		{
+    		case PLAYER_MAP_REQ_GET_INFO:
+      			return ProcessMapInfoReq(resp_queue,hdr,data);
+		    case PLAYER_MAP_REQ_GET_DATA:
+		      	return ProcessMapDataReq(resp_queue,hdr,data);
+		    default:
+		    	return -1;
+  		}
+  	}
+  	return -1;
+}
+int  MrIcp::HandleCommands(QueuePointer& resp_queue,player_msghdr * hdr,void * data)
+{
+  	if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_CMD,PLAYER_POSITION2D_CMD_VEL,this->position_out_addr))
+  	{
+	 	if (!this->position_in_exists)
+	 	{
+	  		cout<<"\n	--->>>No Input Position Driver Initialized";
+	  		fflush(stdout);
+	  		return -1;
+	 	}
+	 	player_position2d_cmd_vel_t* cmd = reinterpret_cast<player_position2d_cmd_vel_t *> (data);
+	 	// cout<<"\n	--->>Me Strong, ME handle commands :)";
+        this->position_device->PutMsg(this->InQueue,
+                             PLAYER_MSGTYPE_CMD,
+                             PLAYER_POSITION2D_CMD_VEL,
+                             (void*)cmd,sizeof(cmd),NULL);
+		return 0;
+	}
+  	return -1;
+}
+// Handle map info request
+int MrIcp::ProcessMapInfoReq(QueuePointer& resp_queue,player_msghdr * hdr,void * data)
+{
+  	// Is it a request for map meta-data?
+	cout<<"\n Processing Map Info request!!!"; fflush(stdout);
+  	if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_MAP_REQ_GET_INFO,this->device_addr) && this->map->occ_grid)
+  	{
+  		cout<<"\n Processing Map Info request Inside!!!"; fflush(stdout);
+    	if(hdr->size != 0)
+    	{
+	      	PLAYER_ERROR2("request is wrong length (%d != %d); ignoring",hdr->size, sizeof(player_laser_config_t));
+	      	return(-1);
+    	}
+    	map_info.scale = this->map_resolution;
+    	map_info.width = ((uint32_t) (int)ceil(2*map_size/map_resolution));
+    	map_info.height =((uint32_t) (int)ceil(2*map_size/map_resolution + 1));
+	    // Did the user specify an origin?
+    	map_info.origin.px = -map_size;
+    	map_info.origin.py = -map_size;
+    	map_info.origin.pa = 0.0;
+	    this->Publish(this->map_addr, resp_queue,PLAYER_MSGTYPE_RESP_ACK,PLAYER_MAP_REQ_GET_INFO,
+	                  (void*)&map_info, sizeof(map_info), NULL);
+	    return(0);
+  }
+  return -1;
+}
+
+// Handle map data request
+int MrIcp::ProcessMapDataReq(QueuePointer& resp_queue,player_msghdr * hdr,void * data)
+{
+	if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+                           PLAYER_MAP_REQ_GET_DATA,
+                           this->device_addr))
+  	{
+    	player_map_data_t* mapreq = (player_map_data_t*)data;
+
+	    player_map_data_t mapresp ;
+
+	    int i, j;
+	    int oi, oj, si, sj,last_row = map->size_y -1;;
+		int16_t temp;
+	  	double prob;
+	    // Construct reply
+	    oi = mapresp.col = mapreq->col;
+	    oj = mapresp.row = mapreq->row;
+	    si = mapresp.width = mapreq->width;
+	    sj = mapresp.height = mapreq->height;
+		long int tile_size = si*sj;
+	    mapresp.data = (int8_t*)calloc(1,tile_size);
+
+  		// Grab the Information from the occupancy data
+	  	for(j = oj; j < (sj+oj); j++)
+	  	{
+	  		// Proccess Last Row with the patch data
+	  		if(j == last_row)
+	  		{
+	  			if(this->playerv_debug)
+	  				continue;
+	  			// Saving Creation Map Time Stamp
+	 			gettimeofday(&map_timestamp,NULL);
+				// Storing Map ID can be between -127 and 127 (assumed +ve all the time)
+				mapresp.data[0 + (j-oj) * si] = map_number;
+				// Storing Robot ID can be between -127 and 127 (assumed +ve all the time)
+				mapresp.data[1 + (j-oj) * si] = robot_id;
+				/* Storing Pose X */
+				int8_t* offset = mapresp.data + (j-oj)*si+2;
+				temp = (int)(global_pose.p.x*1e3);
+				memcpy(offset,&temp, sizeof(temp));
+				offset += sizeof(temp);
+				// Storing Pose Y
+				temp = (int)(global_pose.p.y*1e3);
+				memcpy(offset,&temp, sizeof(temp));
+				offset += sizeof(temp);
+				// Storing Pose Phi
+				temp = (int)(global_pose.phi*1e3);
+				memcpy(offset,&temp, sizeof(temp));
+				offset += sizeof(temp);
+				/* Encapsulating Map time stamp
+				 * Timeval consists of two value, tv_sec and tv_usec both of
+				 * long unsigned int unit32_t (4 bytes), representing time since
+				 * the epoch.
+				 */
+				uint32_t time_temp;
+				// Seconds
+				time_temp = map_timestamp.tv_sec;
+				memcpy(offset,&time_temp, sizeof(time_temp));
+				offset += sizeof(time_temp);
+				// usec
+				time_temp = map_timestamp.tv_usec;
+				memcpy(offset,&time_temp, sizeof(time_temp));
+				offset += sizeof(time_temp);
+				/* The Rest of the Row is ignored for now, more data will
+				 * be encapsulated later on
+				 */
+	  			continue;
+	  		}
+	    	for(i= oi; i < (si+oi); i++)
+	    	{
+	      		if(((i-oi) * (j-oj)) <= tile_size )
+	      		{
+	        		if(MAP_VALID(this, i, j))
+	        		{
+	       				prob = map->occ_grid[i][j].prob_occ;
+	       				//cout<<"\n prob ="<<prob<<" found occupied"<<found_occ;
+	       				if(this->playerv_debug)
+	       				{
+		        			if(prob > 0.9)
+		        				mapresp.data[(i-oi) + (j-oj) * si] = +1;
+		        			else if(prob < 0.1)
+		        				mapresp.data[(i-oi) + (j-oj) * si] = -1;
+		        			else
+		        				mapresp.data[(i-oi) + (j-oj) * si] =  0;
+	       				}
+	       				else
+	       				{
+	       					uint8_t value = (uint8_t)(double(255)*prob);
+	       					memcpy(mapresp.data + (i-oi) + (j-oj)*si,&value, sizeof(value));
+	       				}
+	        		}
+	        		else
+	        		{
+	          			PLAYER_WARN2("requested cell (%d,%d) is offmap", i+oi, j+oj);
+	          			mapresp.data[i + j * si] = 0;
+		        		cout<<"\nData Sent";fflush(stdout);
+	        		}
+	      		}
+	     		else
+	      		{
+	        		PLAYER_WARN("requested tile is too large; truncating");
+	        		cout<<"\nMap Too Large";fflush(stdout);
+	        		if(i == 0)
+	        		{
+	          			mapresp.width = (si-1);
+	          			mapresp.height = (j-1);
+	        		}
+	        		else
+	       			{
+	          			mapresp.width = (i);
+	          			mapresp.height = (j);
+	        		}
+	      		}
+	    	}
+	  }
+       	mapresp.data_count = mapresp.width * mapresp.height;
+	mapresp.data_range = 1;
+    	cout<<"\n	--->>> Columns="<<oi<<" Rows="<<oj<<" width="<<si<<" height="<<sj;
+       	this->Publish(this->device_addr, resp_queue,PLAYER_MSGTYPE_RESP_ACK,PLAYER_MAP_REQ_GET_DATA,(void*)(&mapresp), sizeof(mapresp), NULL);
+		free(mapresp.data);
+       	return(0);
+	}
+  	return -1;
+};
+
+void MrIcp::RefreshData()
+{
+  	// Write position data//
+	this->position_out_data.pos.px = this->px;
+	this->position_out_data.pos.py = this->py;
+	this->position_out_data.pos.pa = this->pa;
+	this->position_out_data.vel.px = this->speed;
+ 	this->position_out_data.vel.py = this->turn_rate;
+    Publish(this->position_out_addr, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE,(void*)&position_out_data);
+	return;
+};
+// Check if the laser beam is in the allowed range
+// This is useful if you have something or part of the robot obstructing the field of view
+// of the laser
+int MrIcp::InRange(double angle,int laser_index)
+{
+	for(int i=0;i<range_count[laser_index];i++)
+	{
+		if(range[laser_index][i].begins <= RTOD(angle) && RTOD(angle) <= range[laser_index][i].ends)
+			return 0;
+	}
+	return -1; // Beam to be ignored
+};
+Point MrIcp::TransformToGlobal(Point p,Pose pose)
+{
+	// Rotate + Translate
+	Point temp = p;
+	p.x = temp.x*cos(pose.phi) - temp.y*sin(pose.phi) + pose.p.x ;
+	p.y = temp.x*sin(pose.phi) + temp.y*cos(pose.phi) + pose.p.y ;
+	return p;
+};
+Pose MrIcp::TransformToGlobal(Pose p,Pose pose)
+{
+	// Rotate + Translate
+	Point temp = p.p;
+	p.p.x = temp.x*cos(pose.phi) - temp.y*sin(pose.phi) + pose.p.x ;
+	p.p.y = temp.x*sin(pose.phi) + temp.y*cos(pose.phi) + pose.p.y ;
+	p.phi = NORMALIZE(p.phi + pose.phi);
+	return p;
+};
+// transfers from Pixel to the Map coordinate
+Point MrIcp :: ConvertPixel(Point  p)
+{
+	p.x = ( p.x*this->map_resolution - this->map_size) ;
+	p.y = (-p.y*this->map_resolution + this->map_size) ;
+	return p;
+};
+// transfers from Map into the Pixel Coordinate
+Point MrIcp :: ConvertToPixel(Point p)
+{
+	//  This is a NxN Map with N = 2*map_size
+	p.x = rint (( p.x + this->map_size)/this->map_resolution);
+	p.y = rint ((-p.y + this->map_size)/this->map_resolution);
+	return p;
+};
+mapgrid_t MrIcp::ComputeRangeProb(double range,bool free)
+{
+	double bad_range=0.;
+	mapgrid_t prob;
+	// 2 , 3 or 5 cm error based on the laser specifications
+	// for the hokouo this should be modified to a constant of 2 cm
+	if (range <=2)
+		bad_range = 0.02;
+	else if (range>2 && range<=4)
+		bad_range = 0.03;
+	else if (range>4)
+		bad_range = 0.05;
+	if (free)
+	{
+		prob.prob_free = range /(range + bad_range);
+		prob.prob_occ  =  1 - prob.prob_free;
+	}
+	else
+	{
+		prob.prob_occ  = range /(range + bad_range);
+		prob.prob_free = 1 - prob.prob_occ;
+	}
+	if(range == 0)
+	{
+		prob.prob_free = 1;
+		prob.prob_occ  = 0;
+	}
+	return prob;
+}
+void MrIcp::AddToMap(vector<Point> laser_data,Pose pose)
+{
+	Point p,pixel_point,d,in_p;
+	Pose relative_laser_pose;
+	double dist,color_prob,gradient;
+	double x_free,x_occ,x_unknown,range,normalizer;
+  	mapgrid_t sensor_prob;
+	int steps;
+	for(unsigned int i=0;i<laser_data.size();i++)
+	{
+		p = TransformToGlobal(laser_data[i],pose);
+		pixel_point = ConvertToPixel(p);
+		if(pixel_point.x > (map_size*2.0/map_resolution)-1 || pixel_point.y >(2.0*map_size/map_resolution) - 2 // -2 because the last row will hold the meta data
+		|| pixel_point.x < 0 || pixel_point.y < 0)
+		{
+			//cout<<"\n	--->>> Map Size Limitations Exceeded Creating New Patch <<<---";
+			cout<<"\n	--->>> Map Size Limitations , New Data Ignored <<<---";
+			//ResetMap();
+			fflush(stdout);
+			return;
+		}
+		// Round the pose to the closest resolution
+//		p.x = map_resolution*rint(p.x/map_resolution);
+//		p.y = map_resolution*rint(p.y/map_resolution);
+		// line parameters
+		relative_laser_pose = TransformToGlobal(laser_pose[laser_data[i].laser_index],pose);
+		d.x = p.x - relative_laser_pose.p.x; // Ray distance X in meters
+		d.y = p.y - relative_laser_pose.p.y; // Ray distance Y in meters
+		// Ray Total length meters
+		dist = sqrt(d.x * d.x + d.y * d.y);
+		steps = (int) floor(dist / map_resolution) + 1;
+		d.x /= (steps);
+		d.y /= (steps);
+		// Traverse the line segment and update the grid
+		Point temp;
+		// The free Cells
+		for (int j = 0; j < steps; j++)
+		{
+		  in_p.x = relative_laser_pose.p.x + d.x * j;
+		  in_p.y = relative_laser_pose.p.y + d.y * j;
+		  temp = ConvertToPixel(in_p);
+		  // Prior Probability Knowledge
+		  x_free = this->map->occ_grid[(int)(temp.x)][(int)(temp.y)].prob_free;
+		  x_occ  = this->map->occ_grid[(int)(temp.x)][(int)(temp.y)].prob_occ;
+		  x_unknown = 1 - (x_free + x_occ);
+		  range = sqrt(pow(in_p.x - relative_laser_pose.p.x,2)+pow(in_p.y - relative_laser_pose.p.y,2));
+		  // Range Sensor Probability
+		  // Bayesian Update
+		  if (dist > this->maxr)
+		  {
+		  	if(range > this->use_max_range)
+		  		break;
+		  	sensor_prob.prob_free = free_space_prob;
+		  	sensor_prob.prob_occ  = (1 - free_space_prob)/2.0;
+		  }
+		  else
+		  {
+		 	sensor_prob = ComputeRangeProb(range,1);
+		  }
+ 	  	  normalizer  = x_free*sensor_prob.prob_free + x_occ*sensor_prob.prob_occ +
+		  			    x_unknown*(1-(sensor_prob.prob_free + sensor_prob.prob_occ ));
+		  this->map->occ_grid[(int)(temp.x)][(int)(temp.y)].prob_free = x_free*sensor_prob.prob_free/normalizer;
+		  this->map->occ_grid[(int)(temp.x)][(int)(temp.y)].prob_occ  = x_occ *sensor_prob.prob_occ/normalizer;
+		  //cout<<"\n Prob OCC ="<<x_occ *sensor_prob.prob_occ/normalizer<<" Prob Free="<<x_free*sensor_prob.prob_free/normalizer;
+		  // Draw the Probability Gradient into the buffer
+		  color_prob =  this->map->occ_grid[(int)(temp.x)][(int)(temp.y)].prob_occ;
+		  if(color_prob>1 || color_prob<0)
+		  {
+		  	cout <<"\n WTF : UNEXPECTED Probability !!! "<< color_prob;
+		  }
+		  gradient = 255.0 - color_prob * 255.0;
+		  map->DrawPixel((int)(gradient),(int)(gradient),(int)(gradient),(int)temp.x,(int)temp.y);
+		}
+		// The end point is occupied
+		if (dist < this->maxr)
+		{
+		  	in_p.x = p.x;
+		  	in_p.y = p.y;
+		  	temp = in_p;
+		  	temp = ConvertToPixel(in_p);
+		  	// Prior Probability
+		  	x_free = this->map->occ_grid[(int)(temp.x)][(int)(temp.y)].prob_free;
+		  	x_occ  = this->map->occ_grid[(int)(temp.x)][(int)(temp.y)].prob_occ;
+		  	x_unknown = 1 - (x_free + x_occ);
+		  	range = sqrt(pow(in_p.x - relative_laser_pose.p.x,2)+pow(in_p.y - relative_laser_pose.p.y,2));
+		  	// Range Sensor Probability
+		  	sensor_prob = ComputeRangeProb(range,0);
+		  	normalizer = x_free*sensor_prob.prob_free + x_occ*sensor_prob.prob_occ +
+		  			   x_unknown*(1-(sensor_prob.prob_free + sensor_prob.prob_occ ));
+		  	// Bayesian Update
+		  	this->map->occ_grid[(int)(temp.x)][(int)(temp.y)].prob_free = x_free*sensor_prob.prob_free/normalizer;
+		  	this->map->occ_grid[(int)(temp.x)][(int)(temp.y)].prob_occ  = x_occ *sensor_prob.prob_occ/normalizer;
+			//cout<<"\n Prob OCC ="<<x_occ *sensor_prob.prob_occ/normalizer<<" Prob Free="<<x_free*sensor_prob.prob_free/normalizer;
+		  	// Draw the Probability Gradient into the buffer
+			color_prob =  this->map->occ_grid[(int)(temp.x)][(int)(temp.y)].prob_occ;
+			gradient = 255.0 - color_prob * 255.0;
+			//cout<<" Gradient= "<<gradient;
+			map->DrawPixel((int)(gradient),(int)(gradient),(int)(gradient),(int)temp.x,(int)temp.y);
+			// Add the Point to the global set
+			if (! this->map->occ_grid[(int)(pixel_point.x)][(int)(pixel_point.y)].added)
+			{
+				this->map->occ_grid[(int)(pixel_point.x)][(int)(pixel_point.y)].added = true;
+				this->map_points.push_back(p);
+			}
+		}
+	}
+};
+// Only get the Existing Map points that are useful for Allignement
+void MrIcp::GenerateLocalMap(Pose pse)
+{
+	double farest_laser_dist=0,dist,num_pixels;
+	Point location,grid_start,temp;
+	location.x = pse.p.x;
+	location.y = pse.p.y;
+	local_map.clear();
+	for(int i=0;i<this->number_of_lasers;i++)
+	{
+		// Get the distance from the Robot's Origin to the Laser Position
+		dist = sqrt(pow(laser_pose[i].p.x,2)+pow(laser_pose[i].p.y,2));
+		if( dist > farest_laser_dist )
+			farest_laser_dist = dist;
+	}
+//	for (unsigned int i=0;i<map_points.size();i++)
+//	{
+//		if (sqrt (pow(pse.p.x - map_points[i].x,2) + pow(pse.p.y - map_points[i].y,2)) <= (maxr + farest_laser_dist + local_map_margine))
+//			local_map.push_back(map_points[i]);
+//	}
+	num_pixels = (farest_laser_dist + this->maxr + local_map_margine) /this->map_resolution;
+	location = ConvertToPixel(location);
+	grid_start.x = location.x - num_pixels;
+	if(grid_start.x < 0)
+		grid_start.x = 0;
+	grid_start.y = location.y - num_pixels;
+	if(grid_start.y < 0)
+		grid_start.y = 0;
+    //cout<<"\nStart grid: "<<grid_start.x<<" y:"<<grid_start.y<<" pixels:"<<num_pixels; fflush(stdout);
+	for(int i= (int)(grid_start.x) ; i< (2*num_pixels + grid_start.x); i++)
+		for(int j=(int)(grid_start.y);j<(2*num_pixels + grid_start.y); j++)
+		{
+			 // y is -2 because last row is meta data
+			if(i<(map->size_x - 1)  && j<(map->size_y - 2))
+				if (map->occ_grid[i][j].prob_occ > 0.9)
+				{
+					temp.x = i;
+					temp.y = j;
+					local_map.push_back(ConvertPixel(temp));
+				}
+		}
+};
+void MrIcp::BuildMap()
+{
+//	double estimated_delta_d,estimated_delta_phi;
+	this->delta_time = delta_t_estimation.TimeElapsed()/1e6;
+	if(this->reset_timer)
+		delta_t_estimation.Reset();
+	if (!sample_initialized)
+	{
+		laser_set_1 = laser_set;
+		if (laser_set_1.size() != 0)
+			sample_initialized = TRUE;
+		else
+			return;
+		// Read Pose if postion driver exists
+		if(this->position_device)	pose_1 = P;
+		global_pose.p.x = global_pose.p.y = global_pose.phi = 0;
+		AddToMap(laser_set_1,global_pose);
+		gettimeofday(&last_delta,NULL);
+		return;
+	}
+	laser_set_2 =  laser_set;
+	if (laser_set_2.size() == 0 || laser_set_1.size() == 0)
+		return;
+
+	if(this->debug)
+		cout<<"\n Laser Set 1 Size:"<<laser_set_1.size()<<" Laser Set 2 Size:"<<laser_set_2.size();
+
+	// Read Pose if position driver exists
+	if(this->use_odom)
+	{
+		pose_2 = P;
+		delta_pose.phi = NORMALIZE(pose_2.phi - pose_1.phi);
+		delta_pose.p.x =  (pose_2.p.x - pose_1.p.x)*cos(pose_1.phi) + (pose_2.p.y - pose_1.p.y)*sin(pose_1.phi) ;
+		delta_pose.p.y = -(pose_2.p.x - pose_1.p.x)*sin(pose_1.phi) + (pose_2.p.y - pose_1.p.y)*cos(pose_1.phi) ;
+	}
+	else
+	{
+		delta_pose.phi = 0; delta_pose.p.x = 0; delta_pose.p.y = 0;
+	}
+	if (this->debug)
+	{
+		cout<<"\n POSE 1 XYQ["<<pose_1.p.x<<"]["<<pose_1.p.y<<"]["<<pose_1.phi<<"]  ";
+		cout<<" POSE 2 XYQ["<<pose_2.p.x<<"]["<<pose_2.p.y<<"]["<<pose_2.phi<<"]";
+	}
+	// Estimate the movement based on the last linear and angular velocity
+	//estimated_delta_d  = this->delta_time * this->speed;
+	//estimated_delta_phi= this->delta_time * this->turn_rate;
+	//cout<<"\n Delta t:"<<delta_time<<" Estimeted d:"<<estimated_delta_d<<" Estimated Phi:"<<estimated_delta_phi;
+
+	// Check what is the displacement estimated by ICP
+	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)
+	{
+		if (this->warning_misalign)
+			cout <<"\nWARNING: possible misalignment ICP: 1 - skipping scan";
+		//laser_set_1 = laser_set_2;
+		return;
+	}
+	global_pose = TransformToGlobal(delta_pose,global_pose);
+	// Is the ICP estimation more than what we excpect ?? If yes then ignore this set
+	/*if(sqrt(pow(delta_pose.p.x,2)+pow(delta_pose.p.y,2)) > (estimated_delta_d +0.1 ) || abs(delta_pose.phi) > abs(estimated_delta_phi + DTOR(5)))
+	{
+		cout<<"\n Delta Pose:1 x:"<<delta_pose.p.x <<" Y="<<delta_pose.p.y<<" Phi"<<delta_pose.phi;
+		cout <<"\nWARNING: possible misalignment - skipping scan";
+		this->reset_timer = false;
+		return;
+	}*/
+	this->reset_timer = true;
+	if (!this->map_points.size())
+	{
+		sample_initialized = false;
+		return;
+	}
+	this->speed = sqrt(pow(delta_pose.p.x,2) + pow(delta_pose.p.y,2))/this->delta_time;
+	this->turn_rate = delta_pose.phi/this->delta_time;
+
+	GenerateLocalMap(global_pose);
+	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)
+	{
+		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;
+		//laser_set_1 = laser_set_2;
+		return;
+	}
+	if(this->debug)
+		cout<<"\n Delta Pose:2 x:"<<global_pose.p.x<<" y:"<<global_pose.p.x<<" phi:"<<global_pose.phi;
+	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)
+	{
+		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;
+		//laser_set_1 = laser_set_2;
+		return;
+	}
+	if(this->debug)
+		cout<<"\n Delta Pose:3 x:"<<global_pose.p.x<<" y:"<<global_pose.p.x<<" phi:"<<global_pose.phi;
+
+	// Serve Data to Position Interface
+	this->global_pose_prev.p.x = this->px = global_pose.p.x;
+	this->global_pose_prev.p.y = this->py = global_pose.p.y;
+	this->global_pose_prev.phi = this->pa = NORMALIZE(global_pose.phi);
+	// Use ALL the Laser data for the occupancy grid update
+	AddToMap(occ_laser_set,global_pose);
+
+	// Perform the ICP on Next Laser Scan
+	laser_set_1 = laser_set_2;
+	if(this->position_device)
+		pose_1 = pose_2;
+};
+
+void MrIcp::ConnectPatches()
+{
+	int patch_number=0;
+	gchar * patch;
+	patch = g_strdup_printf("%sMAP_PATCH%d.png",map_path,patch_number++);
+	while(is_file(patch))
+	{
+		cout<<patch<<endl; fflush(stdout);
+		patch = g_strdup_printf("%sMAP_PATCH%d.png",map_path,patch_number++);
+	}
+}
diff --git a/sm/drivers/playerstage/mricp/src/nn.cpp b/sm/drivers/playerstage/mricp/src/nn.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..44f65f9e1f150f6afa79bcc4308aa37b206681cd
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/src/nn.cpp
@@ -0,0 +1,164 @@
+/*
+Copyright (c) 2004, Tim Bailey
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+    * Redistributions of source code must retain the above copyright notice,
+      this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright notice,
+      this list of conditions and the following disclaimer in the documentation
+      and/or other materials provided with the distribution.
+    * Neither the name of the Player Project nor the names of its contributors
+      may be used to endorse or promote products derived from this software
+      without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+/* K-nearest-neighbours for 2D point-sets.
+ * Tim Bailey 2004.
+ */
+#include "nn.h"
+#include "geometry2D.h"
+#include <algorithm>
+#include <cmath>
+#include <cassert>
+
+using namespace Geom2D;
+using namespace std;
+	
+SweepSearch::SweepSearch(const vector<Point> &p, double lim) : limit(lim)
+// Input: set of points, p, and maximum search distance, lim.
+// Function: create data structure for nearest-neighbour search.
+{
+					// store points and their order
+	dataset.reserve(p.size());
+	for (int i=0; i < (int)p.size(); ++i)
+		dataset.push_back(PointIdx(p[i], i));
+
+	// sort points
+	sort(dataset.begin(), dataset.end(), yorder);
+}
+
+int SweepSearch::query(const Point& q) const
+// Input: query point.
+// Output: index of nearest-neighbour.
+{
+	double d2min = sqr(limit);
+	int idxmin = NOT_FOUND;
+
+					// get upper bound
+	PointIdx qi(q, 0);
+	vector<PointIdx>::const_iterator upper = 
+		upper_bound(dataset.begin(), dataset.end(), qi, yorder);
+	vector<PointIdx>::const_iterator i; 
+
+					// search backwards
+	double min_y = q.y - limit;
+	for (i = upper-1; i >= dataset.begin() && i->p.y > min_y; --i) 
+		if (is_nearer(d2min, idxmin, q, *i))
+			min_y = q.y - sqrt(d2min);
+
+					// search forward
+	double max_y = q.y+sqrt(d2min);
+	for (i = upper; i < dataset.end() && i->p.y < max_y; ++i) 
+		if (is_nearer(d2min, idxmin, q, *i))
+			max_y = q.y+sqrt(d2min);
+
+	return idxmin;
+}
+
+inline 
+bool SweepSearch::is_nearer(double &d2min, int &idxmin, const Point &q, const PointIdx &pi) const
+// Check whether new point is closer than previous points. If so, update d2min and idxmin.
+{
+	double d2 = dist_sqr(q, pi.p);
+	if (d2 < d2min) {
+		d2min = d2;
+		idxmin = pi.i;
+		return true;
+	}
+	return false;
+}
+
+std::vector<double>& SweepSearch::query(const Point &q, std::vector<int> &idx) 
+// Input: query point and vector<int> of size k (number of neighbours)
+// Output: vector<int> indices of nearest-neighbours.
+// Returns: set of distances to k nn's.
+{
+					// can't have more nn's than reference points
+	assert(idx.size() > 0);
+	int size = idx.size();
+	if ((int)dataset.size() < size) {
+		size = dataset.size();
+		idx.resize(size);
+	}
+	if ((int)nndists.size() != size)
+		nndists.resize(size);
+
+					// initialise set
+	for (int j=0; j<size; ++j) {
+		idx[j] = NOT_FOUND;
+		nndists[j] = sqr(limit);
+	}
+					// get upper bound
+	PointIdx qi(q, 0);
+	vector<PointIdx>::const_iterator di = 
+		upper_bound(dataset.begin(), dataset.end(), qi, yorder);
+	vector<PointIdx>::const_iterator i;
+
+					// sweep forwards
+	double max_y = q.y + limit;
+	for (i = di; i < dataset.end() && i->p.y < max_y; ++i) 	
+		if (insert_neighbour(q, *i, nndists, idx))
+			max_y = q.y + sqrt(nndists[size-1]);
+
+					// sweep backwards
+	double min_y = q.y - sqrt(nndists[size-1]);
+	for (i = di-1; i >= dataset.begin() && i->p.y > min_y; --i) 
+		if (insert_neighbour(q, *i, nndists, idx))
+			min_y = q.y - sqrt(nndists[size-1]);
+
+					// convert square distances to distances
+	{ for (int i=0; i<size; ++i)
+		nndists[i] = sqrt(nndists[i]);
+	}
+	return nndists;
+}
+
+bool SweepSearch::insert_neighbour(const Point &q, const PointIdx &pi, 
+		std::vector<double> &nndists, std::vector<int> &idx)
+// Check if new point is closer than any of the existing near-neighbours. If so,
+// place the new point into near-neighbour set (in ascending order).
+{
+	const int size = nndists.size();
+	double d2 = dist_sqr(q, pi.p);
+
+	if (d2 >= nndists[size-1]) // no improvement possible
+		return false;
+
+	int i;
+	for (i=0; d2 >= nndists[i]; ++i) // find location for new neighbour
+		;
+
+	for (int j = size-1; j > i; --j) { // shuffle in order
+		nndists[j] = nndists[j-1];
+		idx[j] = idx[j-1];
+	}
+	nndists[i] = d2;
+	idx[i] = pi.i;
+
+	return true;
+}
diff --git a/sm/drivers/playerstage/mricp/stage_test/casarea.jpeg b/sm/drivers/playerstage/mricp/stage_test/casarea.jpeg
new file mode 100644
index 0000000000000000000000000000000000000000..2fdae98593d4c52e2e23f82f66922d700f4130e4
Binary files /dev/null and b/sm/drivers/playerstage/mricp/stage_test/casarea.jpeg differ
diff --git a/sm/drivers/playerstage/mricp/stage_test/homer.inc b/sm/drivers/playerstage/mricp/stage_test/homer.inc
new file mode 100644
index 0000000000000000000000000000000000000000..80238a53021c1e08a9e747f954d3287c6bbc74c9
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/stage_test/homer.inc
@@ -0,0 +1,45 @@
+
+# Desc: Device definitions for Activemedia robots.
+# Author: Andrew Howard, Richard Vaughan
+# Date: 10 Jun 2002
+# CVS: $Id: homer.inc 4251 2007-11-21 10:51:57Z thjc $
+
+define homer position
+(
+  # actual size
+  size    [0.37 0.37 1]
+  origin  [-0.1 0 0 0] # the wheelchairs's center of rotation is offset from its center of area / I considered it to be 30 cm from the mid Axes
+  velocity [0 0 0]    # Initial Robot Velocity
+  color "gray" # body colorx
+  
+  # GUI properties
+  gui_nose 1 # draw a nose on the robot so we can see which way it points
+  #gui_grid 1
+  gui_movemask 1
+  
+  # estimated mass in KG
+  mass 15.0 
+  
+  # determine how the model appears in various sensors
+  obstacle_return 1
+  laser_return 1
+  ranger_return 1
+  fiducial_return 1
+  # differential steering model
+  drive "diff"
+
+  #body shape
+  block
+  (
+	    points 4
+	    point[0] [ 0.185 -0.185 ]
+	    point[1] [-0.185 -0.185 ]
+	    point[2] [-0.185  0.185 ]
+	    point[3] [ 0.185  0.185 ]
+	    #z[0 0.3]
+  )
+
+  #bitmap ""
+  #bitmap_resolution 0	
+) 
+
diff --git a/sm/drivers/playerstage/mricp/stage_test/libmricp.so b/sm/drivers/playerstage/mricp/stage_test/libmricp.so
new file mode 120000
index 0000000000000000000000000000000000000000..0ace03146b47ff21a69e2c0a044716f28be9517c
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/stage_test/libmricp.so
@@ -0,0 +1 @@
+../libmricp.so
\ No newline at end of file
diff --git a/sm/drivers/playerstage/mricp/stage_test/map.inc b/sm/drivers/playerstage/mricp/stage_test/map.inc
new file mode 100644
index 0000000000000000000000000000000000000000..401bcc63168a7e09a6e155945ea545c3829669a3
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/stage_test/map.inc
@@ -0,0 +1,8 @@
+
+define map model
+(
+  color "black"
+  gui_nose 0
+  gui_grid 0
+  gui_movemask 0
+)
diff --git a/sm/drivers/playerstage/mricp/stage_test/pioneer.inc b/sm/drivers/playerstage/mricp/stage_test/pioneer.inc
new file mode 100644
index 0000000000000000000000000000000000000000..d7a7a83e512c1ead751369fa04c9f7aabd99fcab
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/stage_test/pioneer.inc
@@ -0,0 +1,438 @@
+# Desc: Device definitions for Activemedia robots.
+# Author: Richard Vaughan, Andrew Howard,  Luis Riazuelo
+# Date: 10 Jun 2002
+# CVS: $Id: pioneer.inc,v 1.30 2008-01-15 01:25:42 rtv Exp $
+
+# The Pioneer2DX sonar array
+define p2dx_sonar ranger
+(
+  scount 16 # the number of transducers
+
+  # define the pose of each transducer [xpos ypos heading]
+  spose[0] [ 0.075 0.130 90 ]
+  spose[1] [ 0.115 0.115 50 ]
+  spose[2] [ 0.150 0.080 30 ]
+  spose[3] [ 0.170 0.025 10 ]
+  spose[4] [ 0.170 -0.025 -10 ]
+  spose[5] [ 0.150 -0.080 -30 ]
+  spose[6] [ 0.115 -0.115 -50 ]
+  spose[7] [ 0.075 -0.130 -90 ]
+  spose[8] [ -0.155 -0.130 -90 ]
+  spose[9] [ -0.195 -0.115 -130 ]
+  spose[10] [ -0.230 -0.080 -150 ]
+  spose[11] [ -0.250 -0.025 -170 ]
+  spose[12] [ -0.250 0.025 170 ]
+  spose[13] [ -0.230 0.080 150 ]
+  spose[14] [ -0.195 0.115 130 ]
+  spose[15] [ -0.155 0.130 90 ]	
+		
+  # define the field of view of each transducer [range_min range_max view_angle]
+  sview [0 5.0 15]
+
+  # define the size of each transducer [xsize ysize] in meters
+  ssize [0.01 0.05]
+)
+
+define p2dx_sonar_front ranger
+(
+  scount 8 # the number of transducers
+
+  # define the pose of each transducer [xpos ypos heading]
+  spose[0] [ 0.075 0.130 90 ]
+  spose[1] [ 0.115 0.115 50 ]
+  spose[2] [ 0.150 0.080 30 ]
+  spose[3] [ 0.170 0.025 10 ]
+  spose[4] [ 0.170 -0.025 -10 ]
+  spose[5] [ 0.150 -0.080 -30 ]
+  spose[6] [ 0.115 -0.115 -50 ]
+  spose[7] [ 0.075 -0.130 -90 ]
+
+  # define the field of view of each transducer [range_min range_max view_angle]
+  sview [0 5.0 15]
+
+  # define the size of each transducer [xsize ysize] in meters
+  ssize [0.01 0.05]
+)
+
+
+# The Pioneer3DX sonar array
+define p3dx_sonar ranger
+(
+  scount 16
+
+  # define the pose of each transducer [xpos ypos heading]
+  spose[0] [ 0.069 0.136 90 ]
+  spose[1] [ 0.114 0.119 50 ]
+  spose[2] [ 0.148 0.078 30 ]
+  spose[3] [ 0.166 0.027 10 ]
+  spose[4] [ 0.166 -0.027 -10 ]
+  spose[5] [ 0.148 -0.078 -30 ]
+  spose[6] [ 0.114 -0.119 -50 ]
+  spose[7] [ 0.069 -0.136 -90 ]
+  spose[8] [ -0.157 -0.136 -90 ]
+  spose[9] [ -0.203 -0.119 -130 ]
+  spose[10] [ -0.237 -0.078 -150 ]
+  spose[11] [ -0.255 -0.027 -170 ]
+  spose[12] [ -0.255 0.027 170 ]
+  spose[13] [ -0.237 0.078 150 ]
+  spose[14] [ -0.103 0.119 130 ]
+  spose[15] [ -0.157 0.136 90 ]	
+		
+  # define the field of view of each transducer [range_min range_max view_angle]
+  sview [0.1 5.0 30]  # min (m), max (m), field of view (deg)
+
+  # define the size of each transducer [xsize ysize] in meters
+  ssize [0.01 0.04]
+)
+
+# The Pioneer3AT sonar array
+define p3at_sonar ranger
+(
+  scount 16
+
+  # define the pose of each transducer [xpos ypos heading]
+  spose[0] [0.147 0.136 90]
+  spose[1] [0.193 0.119 50]
+  spose[2] [0.227 0.079 30]
+  spose[3] [0.245 0.027 10]
+  spose[4] [0.245 -0.027 -10]
+  spose[5] [0.227 -0.079 -30]
+  spose[6] [0.193 -0.119 -50]
+  spose[7] [0.147 -0.136 -90]
+  spose[8] [-0.144 -0.136 -90]
+  spose[9] [-0.189 -0.119 -130]
+  spose[10] [-0.223 -0.079 -150]
+  spose[11] [-0.241 -0.027 -170]
+  spose[12] [-0.241 0.027 170]
+  spose[13] [-0.223 0.079 150]
+  spose[14] [-0.189 0.119 130]
+  spose[15] [-0.144 0.136 90]
+
+  # define the field of view of each transducer [range_min range_max view_angle]
+  sview [0.1 5.0 30]  # min (m), max (m), field of view (deg)
+
+  # define the size of each transducer [xsize ysize] in meters
+  ssize [0.01 0.04]
+)
+
+define pioneer_base position 
+(
+  color "red"			# Default color.
+  drive "diff"		 	# Differential steering model.
+  gui_nose 1                  	# Draw a nose on the robot so we can see which way it points
+  obstacle_return 1           	# Can hit things.
+  laser_return 1                 # reflects laser beams
+  ranger_return 1             	# reflects sonar beams
+  blob_return 1               	# Seen by blobfinders  
+  fiducial_return 1           	# Seen as "1" fiducial finders
+
+  localization "gps"         	  
+  localization_origin [0 0 0 0] 	# Start odometry at (0, 0, 0).
+
+  # alternative odometric localization with simple error model
+  # localization "odom"         		# Change to "gps" to have impossibly perfect, global odometry
+  #  odom_error [ 0.05 0.05 0.1 ]   	# Odometry error or slip in X, Y and Theta
+                                   	# (Uniform random distribution)   
+)
+
+
+
+define pioneer2dx_base_no_sonar pioneer_base
+(
+  # actual size
+  size [0.44 0.38 0.22] # sizes from MobileRobots' web site
+
+  # the pioneer's center of rotation is offset from its center of area
+  origin [-0.04 0 0 0]
+
+  # draw a nose on the robot so we can see which way it points
+  gui_nose 1
+
+  # estimated mass in KG
+  mass 23.0 
+  # differential steering model
+  drive "diff"
+ )
+
+
+define pioneer2dx_base pioneer2dx_base_no_sonar
+(
+  # use the sonar array defined above with a small vertical offset to
+  # drop the sensors into the robot body
+  p2dx_sonar( pose [0 0 -0.03 0] ) 
+)
+
+define pioneer2dx_base_front_sonar pioneer2dx_base_no_sonar
+(
+  # use the sonar array defined above with a small vertical offset to
+  # drop the sensors into the robot body
+  p2dx_sonar_front( pose [0 0 -0.03 0] ) 
+)
+
+
+define pioneer2dx pioneer2dx_base
+(  
+  # simplified Body shape:
+  block( 
+    points 8
+    point[0] [-0.2 0.12]
+    point[1] [-0.2 -0.12]
+    point[2] [-0.12 -0.2555]
+    point[3] [0.12 -0.2555]
+    point[4] [0.2 -0.12]
+    point[5] [0.2 0.12]
+    point[6] [0.12 0.2555]
+    point[7] [-0.12 0.2555]
+    z [0 0.22]
+  )
+)
+
+# as above, but with front sonar only
+define pioneer2dx_front_sonar pioneer2dx_base_front_sonar
+(  
+  # simplified Body shape:
+  block( 
+    points 8
+    point[0] [-0.2 0.12]
+    point[1] [-0.2 -0.12]
+    point[2] [-0.12 -0.2555]
+    point[3] [0.12 -0.2555]
+    point[4] [0.2 -0.12]
+    point[5] [0.2 0.12]
+    point[6] [0.12 0.2555]
+    point[7] [-0.12 0.2555]
+    z [0 0.22]
+  )
+)
+
+
+# a Pioneer 2 or 3 in standard configuration
+define fancypioneer2dx pioneer2dx_base
+(
+  # this set of blocks approximates the shape of a real Pioneer
+
+  #  The geometry is from the Webots v5.3.0 manual. Thanks to Webots
+  #  and Olivier Michel. If Stage or Gazebo do not do what you want,
+  #  take a look at Webots. It's a very nice commercial simulator.
+ 
+  # main body
+  block 
+  (
+    points 8
+    point[0] [ -0.185 -0.135 ] 
+    point[1] [  0.095 -0.135 ] 
+    point[2] [  0.11  -0.08  ] 
+    point[3] [  0.11   0.08  ] 
+    point[4] [  0.095  0.135 ] 
+    point[5] [ -0.185  0.135 ]
+    point[6] [ -0.215  0.1   ]
+    point[7] [ -0.215 -0.1   ] 
+    z [ 0.059 0.234 ]
+  )
+
+  # sonar case
+  block
+  (
+    points 9
+    point[0]  [ -0.135  0.136 ]
+    point[1]  [ -0.185  0.136 ]
+    point[2]  [ -0.223  0.101 ]
+    point[3]  [ -0.248  0.054 ]
+    point[4]  [ -0.258  0     ]
+    point[5]  [ -0.248 -0.054 ]
+    point[6]  [ -0.223 -0.101 ]
+    point[7]  [ -0.185 -0.136 ]
+    point[8]  [ -0.135 -0.136 ]
+    z [ 0.184 0.234 ]
+  )
+
+  # sonar case
+  block
+  (
+    points 9
+    point[0]  [ 0.046 -0.136 ]
+    point[1]  [ 0.096 -0.136 ]
+    point[2]  [ 0.134 -0.101 ]
+    point[3]  [ 0.159 -0.054 ]
+    point[4]  [ 0.168  0     ]
+    point[5]  [ 0.159  0.054 ]
+    point[6]  [ 0.134  0.101 ]
+    point[7]  [ 0.096  0.136 ]
+    point[8]  [ 0.046  0.136 ]
+    z [ 0.184 0.234 ]
+  )
+  
+  # left wheel
+  block
+  (
+    points 4
+    point[0] [  0.083  0.177 ]
+    point[1] [ -0.083  0.177 ]
+    point[2] [ -0.083  0.140 ]
+    point[3] [  0.083  0.140 ]
+    z [0 0.165 ]
+    color "gray15"
+  )
+
+  # right wheel
+  block
+  (
+    points 4
+    point[0] [  0.083  -0.14 ]
+    point[1] [ -0.083  -0.14 ]
+    point[2] [ -0.083  -0.177 ]
+    point[3] [  0.083  -0.177 ]
+    z [ 0 0.165 ]
+    color "gray15"
+  )
+
+  # castor
+  block
+  (
+    points 4
+    point[3] [ -0.2475  0.012 ]
+    point[2] [ -0.1825  0.012 ]
+    point[1] [ -0.1825 -0.012 ]
+    point[0] [ -0.2475 -0.012 ]
+    z [ 0 0.065 ]
+    color "gray15"
+  )
+ 
+  # lid
+  block
+  (
+    points 22
+    point[21] [  0.174 0 ]
+    point[20] [  0.166 -0.056 ]
+    point[19] [  0.145 -0.107 ]
+    point[18] [  0.112 -0.155 ]
+    point[17] [  0.064 -0.190 ]
+    point[16] [  -0.074 -0.190 ]
+    point[15] [  -0.096 -0.160 ]
+    point[14] [  -0.151 -0.160 ]
+    point[13] [  -0.2   -0.155 ]
+    point[12] [  -0.236 -0.107 ]
+    point[11] [  -0.256 -0.056 ]
+    point[10] [  -0.264  0     ]
+    point[9] [  -0.256  0.056 ]
+    point[8] [ -0.236  0.107 ]
+    point[7] [ -0.2    0.155 ]
+    point[6] [ -0.151  0.160 ]
+    point[5] [ -0.096  0.160 ]
+    point[4] [ -0.074  0.190 ]
+    point[3] [  0.064  0.190 ]
+    point[2] [  0.112  0.155 ]
+    point[1] [  0.145  0.107 ]
+    point[0] [  0.166  0.056 ]
+    z [ 0.234 0.24 ]
+
+    # a dark top looks more realistic, but isn't very useful 
+    # for a top-down view
+    #color "gray10"
+  )
+)
+
+
+# define 10 straight bumpers around the edge of the robot
+#
+# (these angles are correct for p2dx but the offsets are approximate - RTV)
+# format: bumper[x] [x y th length radius] (zero radius gives a straight line)
+# WARNING: bumpers are not currently supported by Stage>=1.5
+# define pioneer2dxbumper bumper
+# ( 
+#   bumpers10
+#   bumper[0] [  0.17 -0.22  -52  0.105 0.0 ]
+#   bumper[1] [  0.24 -0.12  -19  0.105 0.0 ]
+#   bumper[2] [  0.26  0.00    0  0.105 0.0 ]
+#   bumper[3] [  0.24  0.12   19  0.105 0.0 ]
+#   bumper[4] [  0.17  0.22   52  0.105 0.0 ]
+#   bumper[5] [ -0.25  0.22  128  0.105 0.0 ]
+#   bumper[6] [ -0.32  0.12  161  0.105 0.0 ]
+#   bumper[7] [ -0.34  0.00  180  0.105 0.0 ]
+#   bumper[8] [ -0.32 -0.12  199  0.105 0.0 ]
+#   bumper[9] [ -0.25 -0.22  232  0.105 0.0 ]
+# )
+
+
+# The Pioneer3DX standard configuration
+define pioneer3dx pioneer_base
+(
+  # Actual size
+  size [0.511 0.4 0.22 ]
+
+  # The pioneer's center of rotation is offset from its center of area
+  origin [-0.04465 0.0 0.0]
+
+  # Estimated mass in KG
+  mass 23.0 
+
+  # Body shape:
+  block( 
+    points 8
+    point[0] [-0.2 0.12]
+    point[1] [-0.2 -0.12]
+    point[2] [-0.12 -0.2555]
+    point[3] [0.12 -0.2555]
+    point[4] [0.2 -0.12]
+    point[5] [0.2 0.12]
+    point[6] [0.12 0.2555]
+    point[7] [-0.12 0.2555]
+    z [0 0.22]
+  )
+
+  # Use the sonar array defined above
+  p3dx_sonar( pose [ 0 0 -0.03 0 ] )  
+) 
+
+# The Pioneer3AT standard configuration
+define pioneer3at pioneer_base
+(
+  # Actual size
+  size [0.626 0.505]
+
+  # The pioneer's center of rotation is offset from its center of area
+  origin [-0.04465 0.0 0.0]
+
+  # Estimated mass in KG
+  mass 40.0 
+
+  # Body shape:
+  block( 
+    points 8
+    point[0] [-0.18 0.313]
+    point[1] [0.18 0.313]
+    point[2] [0.2525 0.18]
+    point[3] [0.2525 -0.18]
+    point[4] [0.18 -0.313]
+    point[5] [-0.18 -0.313]
+    point[6] [-0.2525 -0.18]
+    point[7] [-0.2525 0.18]
+  )
+
+  # Use the sonar array defined above
+  p3at_sonar( pose [ 0 0 -0.03 0 ] )  
+)
+
+
+### AMIGOBOT ####
+
+# The AmigoBot sonar array
+define amigo_sonar ranger
+(
+  scount 8
+  spose[0] [ 0.073 0.105 90 ]
+  spose[1] [ 0.130 0.078 41 ]
+  spose[2] [ 0.154 0.030 15 ]
+  spose[3] [ 0.154 -0.030 -15 ]
+  spose[4] [ 0.130 -0.078 -41 ]	
+  spose[5] [ 0.073 -0.105 -90 ]
+  spose[6] [ -0.146 -0.060 -145 ]
+  spose[7] [ -0.146 0.060 145 ]
+)
+
+define amigobot position
+(
+  size [0.330 0.280 0.25]
+  origin [0 0 0 0] # what should this value be? send email to vaughan@sfu.ca.
+  amigo_sonar( pose [0 0 -0.02 0 ] )
+)
diff --git a/sm/drivers/playerstage/mricp/stage_test/rescue.world b/sm/drivers/playerstage/mricp/stage_test/rescue.world
new file mode 100644
index 0000000000000000000000000000000000000000..f4805edd65ba82918179c71eab0e9558fdef893a
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/stage_test/rescue.world
@@ -0,0 +1,70 @@
+resolution     0.047
+interval_sim 100  # milliseconds per update step
+interval_real 100 # real-time milliseconds per update step
+include "pioneer.inc"
+#include "homer.inc"
+include "map.inc"
+size [47 20 2]
+#size [33 38]
+window
+( 
+  size [ 1000 500] 
+  center [0 0 0] 
+  show_data 1
+)
+
+map
+(
+  name "CAS AREA"
+  #size [15.5 11.4]
+  #bitmap "bitmaps/casarea.pnm"
+  bitmap "casarea.jpeg"
+  #bitmap "semi-cas.jpeg"
+  #bitmap "cas.png"
+  #bitmap_resolution 0.044
+  size [46.53 19.74 2]  # casarea
+  #size [13.332 10.78] # semi-cas
+  #size [32.192 37.184 ] # cas
+)
+
+define hokuyolaser1 laser
+(
+  range_min 0.0
+  range_max 4.0
+  fov 240.0
+  samples 768
+  color "LightBlue"
+  size [0.05 0.05 2]
+  pose [0 0 0 0]
+)
+
+
+define hokuyolaser2 laser
+(
+  range_min 0.0
+  range_max 4.0
+  fov 240.0
+  samples 768
+  color "LightBlue"
+  size [ 0.05 0.05 2]
+  pose [-0.2 0 0 180 ]
+)
+
+define sicklaser laser
+(
+  range_min 0.0
+  range_max 8.0
+  fov 184.0
+  samples 183
+  color "LightBlue"
+  size [0.14 0.14 2]
+  pose [0.4 -0.15 0 0 ]
+)
+
+pioneer2dx
+( 
+   name "robot"
+   pose [14.2 -4.6 0 180]
+   hokuyolaser1()
+)
+
diff --git a/sm/drivers/playerstage/mricp/stage_test/stage_mricp.cfg b/sm/drivers/playerstage/mricp/stage_test/stage_mricp.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..5d4cedf8d1181aba790683e7c0f79d4904b7518a
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/stage_test/stage_mricp.cfg
@@ -0,0 +1,41 @@
+
+driver
+(
+  name "stage"
+  plugin "stageplugin"
+  provides ["simulation:0"]
+  worldfile "rescue.world"
+)
+
+# robot 1
+driver
+(
+  name "stage"
+  provides ["position2d:0" "laser:0"]
+  model "robot"
+)
+
+driver
+(
+  name "mricp"
+  plugin "libmricp"
+  provides ["position2d:1" "map:0"]
+  requires ["position2d:0" "laser:0"]
+  number_of_lasers 1
+  laser0_ranges [-120 120]
+  playerv_debug 0
+  period 0.2
+  MAXR 3.9
+  MINR 0.05
+  map_resolution 0.05
+  map_saving_period 5
+  map_size 10
+  use_max_range 1
+  sparse_scans_rate 3
+  map_path "logs/"
+  debug 0
+  alwayson 1
+  log 1
+  sanitycheck_distance 0.1
+  sanitycheck_angle 5
+)
diff --git a/sm/drivers/playerstage/mricp/stage_test/wheelchair.inc b/sm/drivers/playerstage/mricp/stage_test/wheelchair.inc
new file mode 100644
index 0000000000000000000000000000000000000000..b0f1b667d03017749b271ab3feece77437790a5f
--- /dev/null
+++ b/sm/drivers/playerstage/mricp/stage_test/wheelchair.inc
@@ -0,0 +1,48 @@
+
+# Desc: Device definitions for Activemedia robots.
+# Author: Andrew Howard, Richard Vaughan
+# Date: 10 Jun 2002
+# CVS: $Id: wheelchair.inc 4251 2007-11-21 10:51:57Z thjc $
+
+
+# Wheelchair standard configuration
+define wheelchair position
+(
+  # actual size
+  size     [1.2 0.7 1]
+  origin  [0.3 0 0 0] # the wheelchairs's center of rotation is offset from its center of area / I considered it to be 30 cm from the mid Axes
+  velocity [0 0 0]    # Initial Robot Velocity
+  color "gray" # body colorx
+  
+  # GUI properties
+  gui_nose 1 # draw a nose on the robot so we can see which way it points
+  #gui_grid 1
+  gui_boundary 1
+  gui_movemask 1
+  
+  # estimated mass in KG
+  mass 100.0 
+  
+  # determine how the model appears in various sensors
+  obstacle_return 1
+  laser_return 1
+  ranger_return 1
+  blobfinder_return 1
+  fiducial_return 1
+  # differential steering model
+  drive "diff"
+
+  #body shape
+  block
+  (
+	    points 4
+	    point[0] [ 0.8 -0.35 ]
+	    point[1] [-0.4 -0.35 ]
+	    point[2] [-0.4  0.35 ]
+	    point[3] [ 0.8  0.35 ]
+	    #z[0 1] 
+  )
+  #bitmap ""
+  #bitmap_resolution 0	
+) 
+