diff --git a/depth_image_proc/src/nodelets/register.cpp b/depth_image_proc/src/nodelets/register.cpp index a8d544d38..7c1501439 100644 --- a/depth_image_proc/src/nodelets/register.cpp +++ b/depth_image_proc/src/nodelets/register.cpp @@ -195,6 +195,53 @@ void RegisterNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg, pub_registered_.publish(registered_msg, registered_info_msg); } +template +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 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(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 @@ -212,12 +259,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(registered_msg->width); + const int dst_height = static_cast(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(&depth_msg->data[0]); @@ -236,75 +281,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::fromMeters(xyz_rgb.z()); // Validity and Z-buffer checks if (!DepthTraits::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::fromMeters(0.5*(xyz_rgb_1.z()+xyz_rgb_2.z())); + const T new_depth = DepthTraits::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)