diff --git a/crosbot_ogmbicp/CMakeLists.txt b/crosbot_ogmbicp/CMakeLists.txt
index 7d388f67dbd50a9301a54d4e14bd668c2b981cea..0d846249a628a0d0123716a512fbfeed0f6931a4 100644
--- a/crosbot_ogmbicp/CMakeLists.txt
+++ b/crosbot_ogmbicp/CMakeLists.txt
@@ -66,15 +66,15 @@ include_directories(include
 # )
 
 ## Declare a cpp executable
-# add_executable(crosbot_ogmbicp_node src/crosbot_ogmbicp_node.cpp)
+add_executable(crosbot_ogmbicp src/main.cpp src/OgmbicpNode.cpp src/Ogmbicp.cpp)
 
 ## Add dependencies to the executable
 # add_dependencies(crosbot_ogmbicp_node ${PROJECT_NAME})
 
 ## Specify libraries to link a library or executable target against
-# target_link_libraries(crosbot_ogmbicp_node
-#   ${catkin_LIBRARIES}
-# )
+ target_link_libraries(crosbot_ogmbicp
+   ${catkin_LIBRARIES}
+ )
 
 #############
 ## Install ##
@@ -95,11 +95,11 @@ include_directories(include
 # )
 
 ## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
+ install(DIRECTORY include/${PROJECT_NAME}/
+   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+   FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
+   PATTERN ".svn" EXCLUDE
+ )
 
 ## Mark other files for installation (e.g. launch and bag files, etc.)
 # install(FILES
@@ -107,6 +107,12 @@ include_directories(include
 #   # myfile2
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )
+install(DIRECTORY launch
+   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+   FILES_MATCHING PATTERN "*.launch"
+   PATTERN ".svn" EXCLUDE
+ )
+
 
 #############
 ## Testing ##
diff --git a/crosbot_ogmbicp/include/crosbot_ogmbicp/Ogmbicp.hpp b/crosbot_ogmbicp/include/crosbot_ogmbicp/Ogmbicp.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..453895fd6e1bd59eed58ed68a1cf04220277bd6b
--- /dev/null
+++ b/crosbot_ogmbicp/include/crosbot_ogmbicp/Ogmbicp.hpp
@@ -0,0 +1,51 @@
+/*
+ * Ogmbicp.hpp
+ *
+ * Created on: 12/9/2013
+ *     Author: adrianr
+ */
+
+#ifndef OGMBICP_HPP_
+#define OGMBICP_HPP_
+
+#include <ros/ros.h>
+#include <crosbot/utils.hpp>
+#include <crosbot/data.hpp>
+#include <sensor_msgs/LaserScan.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+
+using namespace crosbot;
+using namespace std;
+
+class Ogmbicp {
+public:
+   /*
+    * Initialise the position tracker
+    */
+   void initialise(ros::NodeHandle &nh);
+
+   /*
+    * Start the position tracker. After this is called, the position
+    * tracker is ready to take scans
+    */
+   void start();
+
+   /*
+    * Stop the position tracker
+    */
+   void stop();
+
+   /*
+    * Called for processing the first scan
+    */
+   void initialiseTrack(Pose sensorPose, PointCloudPtr cloud);
+
+   /*
+    * Update the position tracker with the lastest scan
+    */
+   void updateTrack(Pose sensorPose, PointCloudPtr cloud);
+
+};
+
+#endif
diff --git a/crosbot_ogmbicp/include/crosbot_ogmbicp/OgmbicpNode.hpp b/crosbot_ogmbicp/include/crosbot_ogmbicp/OgmbicpNode.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..5592b8584ad04c86db10e1c7652de053008d68a3
--- /dev/null
+++ b/crosbot_ogmbicp/include/crosbot_ogmbicp/OgmbicpNode.hpp
@@ -0,0 +1,71 @@
+/*
+ * OgmbicpNode.hpp
+ *
+ * Created on: 12/9/2013
+ *     Author: adrianr
+ */
+
+
+#ifndef OGMBICPNODE_HPP_
+#define OGMBICPNODE_HPP_
+
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <sensor_msgs/LaserScan.h>
+
+#include <crosbot/data.hpp>
+#include <crosbot/utils.hpp>
+
+#include <crosbot_ogmbicp/Ogmbicp.hpp>
+
+#define DEFAULT_ICPFRAME "/icp"
+#define DEFAULT_BASEFRAME "/base_link"
+#define DEFAULT_ODOMFRAME ""
+#define DEFAULT_MAXWAIT4TRANSFORM 2.0
+
+using namespace std;
+
+class OgmbicpNode {
+public:
+
+   OgmbicpNode(Ogmbicp&);
+
+   /*
+    * Initialises the ogmbicp node
+    */
+   void initialise(ros::NodeHandle& nh);
+
+   /*
+    * Shuts down the ogmbicp node
+    */
+   void shutdown();
+
+private:
+  
+   /*
+    * ROS config params for position tracking
+    */
+   string icp_frame, base_frame, odom_frame;
+   string scan_sub;
+
+
+   /*
+    * ROS connections
+    */
+   ros::Subscriber scanSubscriber;
+   tf::TransformListener tfListener;
+
+   Ogmbicp &pos_tracker;
+   //Is it the initial scan?
+   bool isInit;
+
+   /*
+    * Main callback for position tracker. Processes a new scan
+    */
+   void callbackScan(const sensor_msgs::LaserScanConstPtr& lastestScan);
+
+};
+
+
+#endif
diff --git a/crosbot_ogmbicp/include/crosbot_ogmbicp/pointgrid.hpp b/crosbot_ogmbicp/include/crosbot_ogmbicp/pointgrid.hpp
deleted file mode 100644
index 68f05823a2d631d75f31bc9d0e5c16435a1c4c71..0000000000000000000000000000000000000000
--- a/crosbot_ogmbicp/include/crosbot_ogmbicp/pointgrid.hpp
+++ /dev/null
@@ -1,55 +0,0 @@
-/*
- * pointgrid.hpp
- *
- *  Created on: 28/02/2013
- *      Author: mmcgill
- */
-
-#ifndef CROSBOT_POINTGRID_HPP_
-#define CROSBOT_POINTGRID_HPP_
-
-namespace crosbot {
-
-namespace ogmbicp {
-
-class PointGrid {
-public:
-	class Cell : public HandledObject {
-	public:
-		Cell() {}
-	};
-	typedef Handle<Cell> CellPtr;
-
-	uint32_t width, height;
-	double resolution;
-	std::vector< std::vector < CellPtr > > cells;
-	Pose origin;
-
-	PointGrid(uint32_t width, uint32_t height, double resolution) :
-		width(width), height(height), resolution(resolution),
-		origin(-width * resolution / 2.0, -height * resolution / 2.0, 0)
-	{
-		if (width > 0 && height > 0) {
-			cells.resize(height);
-			for (uint32_t r = 0; r < height; ++r) {
-				cells[r].resize(width);
-				for (uint32_t c = 0; c < width; ++c) {
-					cells[r][c] = new Cell();
-				}
-			}
-		}
-	}
-
-	void centerOn(Point p);
-protected:
-	void shift(int x, int y);
-};
-
-}
-
-} // namespace crosbot
-
-
-
-
-#endif /* CROSBOT_POINTGRID_HPP_ */
diff --git a/crosbot_ogmbicp/launch/crosbot_ogmbicp.launch b/crosbot_ogmbicp/launch/crosbot_ogmbicp.launch
new file mode 100644
index 0000000000000000000000000000000000000000..c2c757d0d105ecaf69384a3c4a2faa4d66392325
--- /dev/null
+++ b/crosbot_ogmbicp/launch/crosbot_ogmbicp.launch
@@ -0,0 +1,34 @@
+<launch>
+	<param name="use_sim_time" value="true" />
+	
+	<!--node pkg="tf" type="static_transform_publisher" name="laser_broadcast"
+		args="0 0 1 0 0 0 base_link laser 100" >
+	</node-->
+	
+	<!--node pkg="tf" type="static_transform_publisher" name="odom_broadcast"
+		args="0 0 0 0 0 0 odom base_link 100" >
+	</node-->
+
+	<!--node pkg="crosbot_map" type="localmap" name="localmap" >
+		<param name="map_frame" value="icp" />
+		<!- -param name="base_frame" value="/base_footprint" /- ->
+
+		<remap from="image" to="/mbicp/image" />
+	</node-->
+	
+	<node pkg="image_view" type="image_view" name="localmap_view">
+		<!--remap from="image" to="/mbicp/image" /-->
+	</node>
+
+	<node pkg="image_view" type="image_view" name="image_view">
+		<remap from="image" to="/camera/rgb/image_color" />
+   </node>
+	
+	<node pkg="crosbot_ogmbicp" type="crosbot_ogmbicp" name="crosbot_ogmbicp" output="screen" >
+		<param name="odom_frame" Xvalue="odom" value="" />
+		<param name="base_frame" value="base_footprint" />
+	</node>
+
+	<!--node pkg="crosbot_ui" type="gui" args="$(env PWD)/bhuman.gui" name="gui" output="screen" >
+	</node-->
+</launch>	
diff --git a/crosbot_ogmbicp/src/Ogmbicp.cpp b/crosbot_ogmbicp/src/Ogmbicp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..80f8e989a6c9274f91a58eadd5624fd77bcfa537
--- /dev/null
+++ b/crosbot_ogmbicp/src/Ogmbicp.cpp
@@ -0,0 +1,28 @@
+/*
+ * Ogmbicp.cpp
+ *
+ * Created on: 16/9/2013
+ *     Author: adrianr
+ */
+
+#include <crosbot_ogmbicp/Ogmbicp.hpp>
+
+void Ogmbicp::initialise(ros::NodeHandle &nh) {
+}
+
+void Ogmbicp::start() {
+   cout << "starting ogmbicp" << endl;
+}
+
+void Ogmbicp::stop() {
+   cout << "stopping ogmbicp" << endl;
+
+}
+
+void Ogmbicp::initialiseTrack(Pose sensorPose, PointCloudPtr cloud) {
+   cout << "ogmbicp: initialise track" << endl;
+}
+
+void Ogmbicp::updateTrack(Pose sensorPose, PointCloudPtr cloud) {
+   cout << "ogmbicp: update track" << endl;
+}
diff --git a/crosbot_ogmbicp/src/OgmbicpNode.cpp b/crosbot_ogmbicp/src/OgmbicpNode.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..dee6016da8843778e0c26ae948c97822e4052e69
--- /dev/null
+++ b/crosbot_ogmbicp/src/OgmbicpNode.cpp
@@ -0,0 +1,71 @@
+/*
+ * OgmbicpNode.cpp
+ *
+ * Created on: 12/9/2013
+ *     Author: adrianr
+ */
+
+#include <crosbot_ogmbicp/OgmbicpNode.hpp>
+
+using namespace std;
+using namespace crosbot;
+
+OgmbicpNode::OgmbicpNode(Ogmbicp &posTracker): icp_frame(DEFAULT_ICPFRAME),
+                            base_frame(DEFAULT_BASEFRAME),
+                            odom_frame(DEFAULT_ODOMFRAME),
+                            pos_tracker(posTracker)
+{
+   pos_tracker = posTracker;
+   isInit = false;
+}
+
+void OgmbicpNode::initialise(ros::NodeHandle &nh) {
+   ros::NodeHandle paramNH("~");
+   paramNH.param<std::string>("icp_frame", icp_frame, DEFAULT_ICPFRAME);
+   paramNH.param<std::string>("base_frame", base_frame, DEFAULT_BASEFRAME);
+   paramNH.param<std::string>("odom_frame", odom_frame, DEFAULT_ODOMFRAME);
+   paramNH.param<std::string>("scan_sub", scan_sub, "scan");
+
+   pos_tracker.initialise(nh);
+   pos_tracker.start();
+
+   scanSubscriber = nh.subscribe(scan_sub, 1, &OgmbicpNode::callbackScan, this);
+}
+
+void OgmbicpNode::shutdown() {
+   scanSubscriber.shutdown();
+   pos_tracker.stop();
+
+}
+
+void OgmbicpNode::callbackScan(const sensor_msgs::LaserScanConstPtr& latestScan) {
+   Pose odomPose, sensorPose;
+  	tf::StampedTransform laser2Base, base2Odom;
+  	bool haveOdometry = odom_frame != "";
+  	try {
+  		tfListener.waitForTransform(base_frame, latestScan->header.frame_id,
+             latestScan->header.stamp, ros::Duration(1, 0));
+  		tfListener.lookupTransform(base_frame,
+   				latestScan->header.frame_id, latestScan->header.stamp, laser2Base);
+  		sensorPose = laser2Base;
+
+  		if (haveOdometry) {
+     		tfListener.waitForTransform(odom_frame, base_frame, latestScan->header.stamp, ros::Duration(1, 0));
+     		tfListener.lookupTransform(odom_frame, base_frame,
+     				latestScan->header.stamp, base2Odom);
+     		odomPose = base2Odom;
+ 		}
+  	} catch (tf::TransformException& ex) {
+ 		fprintf(stderr, "ogmbicp: Error getting transform. (%s) (%d.%d)\n", ex.what(),
+   		latestScan->header.stamp.sec, latestScan->header.stamp.nsec);
+   	return;
+   }
+   PointCloudPtr cloud = new PointCloud(base_frame, PointCloud(latestScan, true), sensorPose);
+   if (!isInit) {
+      isInit = true;
+      pos_tracker.initialiseTrack(sensorPose, cloud);
+   } else {
+      pos_tracker.updateTrack(sensorPose, cloud);
+   }
+}
+
diff --git a/crosbot_ogmbicp/src/main.cpp b/crosbot_ogmbicp/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..698092ab035643f56678274e82503044d0ce0dcb
--- /dev/null
+++ b/crosbot_ogmbicp/src/main.cpp
@@ -0,0 +1,30 @@
+/*
+ * main.cpp
+ *
+ * Created on: 12/9/2013
+ *     Author: adrianr
+ */
+
+#include <ros/ros.h>
+
+#include <crosbot_ogmbicp/OgmbicpNode.hpp>
+#include <crosbot_ogmbicp/Ogmbicp.hpp>
+
+int main(int argc, char **argv) {
+   ros::init(argc, argv, "ogmbicp");
+
+   ros::NodeHandle nh;
+
+   Ogmbicp posTracker;
+   OgmbicpNode node(posTracker);
+   node.initialise(nh);
+
+   while (ros::ok()) {
+      ros::spin();
+   }
+   node.shutdown();
+
+   return 0;
+}
+
+