factored out common code from depth_image_proc register convert

Author: lucasw

depth_image_proc/src/nodelets/register.cpp

@@ -197,6 +197,54 @@ void RegisterNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg,
   pub_registered_.publish(registered_msg, registered_info_msg);
 }
 
+template<typename T>
+bool transform_depth(
+    const Eigen::Affine3d& depth_to_rgb,
+    const double u, const double v,
+    const image_geometry::PinholeCameraModel& depth_model_,
+    const image_geometry::PinholeCameraModel& rgb_model_,
+    const double inv_depth_fx, const double inv_depth_fy,
+    const int width, const int height,
+    const double depth,
+    int& u_rgb, int& v_rgb,
+    T& new_depth)
+{
+  // TODO(lucasw) pulling these out shouldn't cost anything
+  const auto depth_cx = depth_model_.cx(), depth_cy = depth_model_.cy();
+  const auto depth_Tx = depth_model_.Tx(), depth_Ty = depth_model_.Ty();
+  const auto rgb_fx = rgb_model_.fx(), rgb_fy = rgb_model_.fy();
+  const auto rgb_cx = rgb_model_.cx(), rgb_cy = rgb_model_.cy();
+  const auto rgb_Tx = rgb_model_.Tx(), rgb_Ty = rgb_model_.Ty();
+
+  // TODO(lucasw) factor out common code below
+  /// @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
+  Eigen::Vector4d xyz_depth;
+  xyz_depth << ((u - depth_cx)*depth - depth_Tx) * inv_depth_fx,
+               ((v - depth_cy)*depth - depth_Ty) * inv_depth_fy,
+               depth,
+               1;
+
+  // Transform to RGB camera frame
+  Eigen::Vector4d xyz_rgb = depth_to_rgb * xyz_depth;
+  // TODO(lucasw) return false if xyz_rgb.z() < 0.0 - or is that okay for some < 0 fx/fy?
+
+  // Project to (u,v) in RGB image
+  const double inv_Z = 1.0 / xyz_rgb.z();
+
+  u_rgb = (rgb_fx*xyz_rgb.x() + rgb_Tx)*inv_Z + rgb_cx + 0.5;
+  if (u_rgb < 0 || u_rgb >= width)
+    return false;
+
+  v_rgb = (rgb_fy*xyz_rgb.y() + rgb_Ty)*inv_Z + rgb_cy + 0.5;
+  if (v_rgb < 0 || v_rgb >= height)
+    return false;
+
+  new_depth = static_cast<T>(xyz_rgb.z());
+
+  return true;
+}
+
 // TODO(lucasw) need a unit test for this, simple low res image (e.g. 4x4 pixels) with
 // depth ranges of around 1.0
 // shift it to the right 1.0 units, view from 90 degrees
@@ -214,12 +262,10 @@ void RegisterNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
   // Extract all the parameters we need
   const double inv_depth_fx = 1.0 / depth_model_.fx();
   const double inv_depth_fy = 1.0 / depth_model_.fy();
-  const double depth_cx = depth_model_.cx(), depth_cy = depth_model_.cy();
-  const double depth_Tx = depth_model_.Tx(), depth_Ty = depth_model_.Ty();
-  const double rgb_fx = rgb_model_.fx(), rgb_fy = rgb_model_.fy();
-  const double rgb_cx = rgb_model_.cx(), rgb_cy = rgb_model_.cy();
-  const double rgb_Tx = rgb_model_.Tx(), rgb_Ty = rgb_model_.Ty();
-  
+
+  const int dst_width = static_cast<int>(registered_msg->width);
+  const int dst_height = static_cast<int>(registered_msg->height);
+
   // 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]);
@@ -238,75 +284,57 @@ void RegisterNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
 
       if (fill_upsampling_holes_ == false)
       {
-        // TODO(lucasw) factor out common code below
-        /// @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
-        Eigen::Vector4d xyz_depth;
-        xyz_depth << ((u - depth_cx)*depth - depth_Tx) * inv_depth_fx,
-                     ((v - depth_cy)*depth - depth_Ty) * inv_depth_fy,
-                     depth,
-                     1;
-
-        // Transform to RGB camera frame
-        Eigen::Vector4d xyz_rgb = depth_to_rgb * xyz_depth;
-
-        // Project to (u,v) in RGB image
-        double inv_Z = 1.0 / xyz_rgb.z();
-        int u_rgb = (rgb_fx*xyz_rgb.x() + rgb_Tx)*inv_Z + rgb_cx + 0.5;
-        if (u_rgb < 0 || u_rgb >= (int)registered_msg->width)
-          continue;
-
-        int v_rgb = (rgb_fy*xyz_rgb.y() + rgb_Ty)*inv_Z + rgb_cy + 0.5;
-        if (v_rgb < 0 || v_rgb >= (int)registered_msg->height)
+        int u_rgb;
+        int v_rgb;
+        T new_depth;
+        if (!transform_depth(depth_to_rgb, u, v,
+            depth_model_, rgb_model_,
+            inv_depth_fx, inv_depth_fy,
+            dst_width, dst_height,
+            depth,
+            u_rgb, v_rgb, new_depth)) {
           continue;
+        }
 
         T& reg_depth = registered_data[v_rgb*registered_msg->width + u_rgb];
-        T  new_depth = DepthTraits<T>::fromMeters(xyz_rgb.z());
         // Validity and Z-buffer checks
         if (!DepthTraits<T>::valid(reg_depth) || reg_depth > new_depth)
           reg_depth = new_depth;
+
       }
       else
       {
-        // TODO(lucasw) replace same code with for loop
-        // Reproject (u,v,Z) to (X,Y,Z,1) in depth camera frame
-        Eigen::Vector4d xyz_depth_1, xyz_depth_2;
-        xyz_depth_1 << ((u-0.5f - depth_cx)*depth - depth_Tx) * inv_depth_fx,
-                       ((v-0.5f - depth_cy)*depth - depth_Ty) * inv_depth_fy,
-                       depth,
-                       1;
-        // Transform to RGB camera frame
-        Eigen::Vector4d xyz_rgb_1 = depth_to_rgb * xyz_depth_1;
-        // Project to (u,v) in RGB image
-        const double inv_Z_1 = 1.0 / xyz_rgb_1.z();
-        const int u_rgb_1 = (rgb_fx*xyz_rgb_1.x() + rgb_Tx)*inv_Z_1 + rgb_cx + 0.5;
-        if (u_rgb_1 < 0 || u_rgb_1 >= (int)registered_msg->width)
-          continue;
-        const int v_rgb_1 = (rgb_fy*xyz_rgb_1.y() + rgb_Ty)*inv_Z_1 + rgb_cy + 0.5;
-        if (v_rgb_1 < 0 || v_rgb_1 >= (int)registered_msg->height)
+        // TODO(lucasw) loop on two -0.5, 0.5 vectors, keep track of u_min/max and v_min/max
+        // as it goes
+        int u_rgb_1;
+        int v_rgb_1;
+        T new_depth_1;
+        if (!transform_depth(depth_to_rgb,
+            u - 0.5, v - 0.5,
+            depth_model_, rgb_model_,
+            inv_depth_fx, inv_depth_fy,
+            dst_width, dst_height,
+            depth,
+            u_rgb_1, v_rgb_1, new_depth_1)) {
+          // TODO(lucasw) need to be able to handle partiall out of bounds squares here
           continue;
+        }
 
-        // TODO(lucasw) this is identical to above except adding 0.5 instead of subtracting
-        // TODO(lucasw) need a third and fourth position with u+0.5 and v-0.5 and u-0.5 and v+0.5
-        // (and then could do the triangle rastering mentioned above, or just take the min
-        // and max of all of them)
-        xyz_depth_2 << ((u+0.5f - depth_cx)*depth - depth_Tx) * inv_depth_fx,
-                       ((v+0.5f - depth_cy)*depth - depth_Ty) * inv_depth_fy,
-                       depth,
-                       1;
-        // Transform to RGB camera frame
-        Eigen::Vector4d xyz_rgb_2 = depth_to_rgb * xyz_depth_2;
-        // Project to (u,v) in RGB image
-        const double inv_Z_2 = 1.0 / xyz_rgb_2.z();
-        const int u_rgb_2 = (rgb_fx*xyz_rgb_2.x() + rgb_Tx)*inv_Z_2 + rgb_cx + 0.5;
-        if (u_rgb_2 < 0 || u_rgb_2 >= (int)registered_msg->width)
-          continue;
-        const int v_rgb_2 = (rgb_fy*xyz_rgb_2.y() + rgb_Ty)*inv_Z_2 + rgb_cy + 0.5;
-        if (v_rgb_2 < 0 || v_rgb_2 >= (int)registered_msg->height)
+        int u_rgb_2;
+        int v_rgb_2;
+        T new_depth_2;
+        if (!transform_depth(depth_to_rgb,
+            u + 0.5, v + 0.5,
+            depth_model_, rgb_model_,
+            inv_depth_fx, inv_depth_fy,
+            dst_width, dst_height,
+            depth,
+            u_rgb_2, v_rgb_2, new_depth_2)) {
           continue;
+        }
 
         // fill in the square defined by uv range
-        const T new_depth = DepthTraits<T>::fromMeters(0.5*(xyz_rgb_1.z()+xyz_rgb_2.z()));
+        const T new_depth = DepthTraits<T>::fromMeters(0.5 * (new_depth_1 + new_depth_2));
         for (int nv=std::min(v_rgb_1, v_rgb_2); nv<=std::max(v_rgb_1, v_rgb_2); ++nv)
         {
           for (int nu=std::min(u_rgb_1, u_rgb_2); nu<=std::max(u_rgb_2, u_rgb_2); ++nu)