diff --git a/include/slam_toolbox/slam_toolbox.hpp b/include/slam_toolbox/slam_toolbox.hpp
index 6e90f2904..5b54e485d 100644
--- a/include/slam_toolbox/slam_toolbox.hpp
+++ b/include/slam_toolbox/slam_toolbox.hpp
@@ -143,7 +143,7 @@ class SlamToolbox
   int throttle_scans_;
   ros::Duration map_update_interval_;
   double resolution_, minimum_time_interval_, minimum_travel_distance_;
-  bool publish_occupancy_map_;
+  bool publish_occupancy_map_, first_measurement_;
 
   // Karto bookkeeping
   karto::Mapper* mapper_;
diff --git a/src/slam_toolbox.cpp b/src/slam_toolbox.cpp
index d5b123203..ff6659447 100644
--- a/src/slam_toolbox.cpp
+++ b/src/slam_toolbox.cpp
@@ -36,7 +36,8 @@ SlamToolbox::SlamToolbox() :
                          pause_new_measurements_(false),
                          interactive_mode_(false),
                          laser_count_(0),
-                         tf_(ros::Duration(14400.)) // 4 hours
+                         tf_(ros::Duration(14400.)),
+                         first_measurement_(true) // 4 hours
 /*****************************************************************************/
 {
   interactive_server_ = \
@@ -629,6 +630,21 @@ void SlamToolbox::LaserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
     return;
   }
 
+  // let the first measurement pass to start the ball rolling
+  if (first_measurement_)
+  {
+    karto::Pose2 pose;
+    if(!GetOdomPose(pose, scan->header.stamp))
+    {
+      return;
+    }
+    q_.push(posed_scan(scan, pose));
+    last_scan_time = scan->header.stamp.toSec(); 
+    last_pose = pose;
+    first_measurement_ = false;
+    return;
+  }
+
   // throttled out
   laser_count_++;
   if ((laser_count_ % throttle_scans_) != 0)