diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScan.h b/include/depthimage_to_laserscan/DepthImageToLaserScan.h
index 4a64360..1b66566 100644
--- a/include/depthimage_to_laserscan/DepthImageToLaserScan.h
+++ b/include/depthimage_to_laserscan/DepthImageToLaserScan.h
@@ -177,28 +177,34 @@ namespace depthimage_to_laserscan
       const double unit_scaling = depthimage_to_laserscan::DepthTraits<T>::toMeters( T(1) );
       const float constant_x = unit_scaling / cam_model.fx();
 
-      const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
+      const T* depth_row0 = reinterpret_cast<const T*>(&depth_msg->data[0]);
       const int row_step = depth_msg->step / sizeof(T);
 
       const int offset = (int)(center_y - scan_height/2);
-      depth_row += offset*row_step; // Offset to center of image
 
-      for(int v = offset; v < offset+scan_height_; ++v, depth_row += row_step){
-        for (int u = 0; u < (int)depth_msg->width; ++u) // Loop over each pixel in row
-        {
+      const int depth_row_offset_start = offset*row_step;
+      const int depth_row_offset_end = (offset+scan_height_)*row_step;
+
+      double z_coef = depthimage_to_laserscan::DepthTraits<T>::toMeters(1.0);
+
+      for (int u = 0; u < (int)depth_msg->width; ++u) {
+        const double th = -atan2((double)(u - center_x) * constant_x, unit_scaling); // Atan2(x, z), but depth divides out
+        const int index = (th - scan_msg->angle_min) / scan_msg->angle_increment;
+
+        double x_coef = (u - center_x) * constant_x;
+        double r_coef = hypot(x_coef, z_coef);
+
+        for(const T* depth_row = depth_row0 + depth_row_offset_start;
+            depth_row < depth_row0 + depth_row_offset_end;
+            depth_row += row_step) {
+
           const T depth = depth_row[u];
 
-          double r = depth; // Assign to pass through NaNs and Infs
-          const double th = -atan2((double)(u - center_x) * constant_x, unit_scaling); // Atan2(x, z), but depth divides out
-          const int index = (th - scan_msg->angle_min) / scan_msg->angle_increment;
+          double r = depth;
 
           if (depthimage_to_laserscan::DepthTraits<T>::valid(depth)){ // Not NaN or Inf
-            // Calculate in XYZ
-            double x = (u - center_x) * depth * constant_x;
-            double z = depthimage_to_laserscan::DepthTraits<T>::toMeters(depth);
-
             // Calculate actual distance
-            r = hypot(x, z);
+            r = r_coef * depth;
           }
 
           // Determine if this point should be used.
diff --git a/src/DepthImageToLaserScan.cpp b/src/DepthImageToLaserScan.cpp
index 1f5e25b..d4863f3 100644
--- a/src/DepthImageToLaserScan.cpp
+++ b/src/DepthImageToLaserScan.cpp
@@ -64,10 +64,7 @@ bool DepthImageToLaserScan::use_point(const float new_value, const float old_val
 
   // Infs are preferable over NaNs (more information)
   if(!new_finite && !old_finite){ // Both are not NaN or Inf.
-    if(!std::isnan(new_value)){ // new is not NaN, so use it's +-Inf value.
-      return true;
-    }
-    return false; // Do not replace old_value
+    return !std::isnan(new_value); // new is not NaN, so use it's +-Inf value.
   }
 
   // If not in range, don't bother