diff --git a/depth_image_proc/src/nodelets/register.cpp b/depth_image_proc/src/nodelets/register.cpp
index 091a6a7..5967ef5 100644
--- a/depth_image_proc/src/nodelets/register.cpp
+++ b/depth_image_proc/src/nodelets/register.cpp
@@ -11,6 +11,10 @@
 #include <Eigen/Core>
 #include "depth_traits.h"
 
+#define SHIFT_SCALE 0.125
+#define FOCAL_LENGTH 570.342224121
+#define BASELINE 0.075
+
 namespace depth_image_proc {
 
 using namespace message_filters::sync_policies;
@@ -181,7 +185,25 @@ void RegisterNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
   double rgb_fx = rgb_model_.fx(), rgb_fy = rgb_model_.fy();
   double rgb_cx = rgb_model_.cx(), rgb_cy = rgb_model_.cy();
   double rgb_Tx = rgb_model_.Tx(), rgb_Ty = rgb_model_.Ty();
-  
+
+  double u_coeff, v_coeff, d_coeff, offset;
+  int dist;
+  if(!ros::param::get ("/projector_coefficients/u_coeff", u_coeff)){
+    u_coeff = 0.0;
+  }
+  if(!ros::param::get ("/projector_coefficients/v_coeff", v_coeff)){
+    v_coeff = 0.0;
+  }
+  if(!ros::param::get ("/projector_coefficients/d_coeff", d_coeff)){
+    d_coeff = 1.0;
+  }
+  if(!ros::param::get ("/projector_coefficients/offset", offset)){
+    offset = 0.0;
+  }
+  if(!ros::param::get ("/projector_coefficients/distance", dist)){
+    dist = 150;
+  }
+
   // Transform the depth values into the RGB frame
   /// @todo When RGB is higher res, interpolate by rasterizing depth triangles onto the registered image  
   const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
@@ -197,6 +219,18 @@ void RegisterNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
         continue;
       
       double depth = DepthTraits<T>::toMeters(raw_depth);
+      float uu = depth_cx - u;
+      float vv = depth_cy - v;
+      float d = FOCAL_LENGTH * BASELINE / depth;
+      float d_fitted = d_coeff * d + offset + u_coeff*uu*uu + v_coeff*vv*vv;
+      float depth_fitted = FOCAL_LENGTH * BASELINE / d_fitted;
+
+      if ( (0 < depth_fitted) && (uu*uu + vv*vv < dist*dist)){
+        depth = depth_fitted;
+      }else{
+        depth = 0;
+      }
+
 
       /// @todo Combine all operations into one matrix multiply on (u,v,d)
       // Reproject (u,v,Z) to (X,Y,Z,1) in depth camera frame
