depthsub_ = it_.subscribeCamera(depth_image_topic, 1, &FrameDrawer::depthImageCb, this);
void depthImageCb(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info_msg)
{
cv::Mat image;
cv_bridge::CvImageConstPtr input_bridge;
try {
input_bridge = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::TYPE_16UC1);// For depth image
// input_bridge = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::TYPE_32FC1);// For depth image
image = input_bridge->image;
}
catch (cv_bridge::Exception& ex){
ROS_ERROR("[draw_frames] Failed to convert image");
return;
}
cv::Point3d xyz = cam_model_.projectPixelTo3dRay(pixel_point); // cv::Point3d ray;
// ray.x = (uv_rect.x - cx() - Tx()) / fx();
// ray.y = (uv_rect.y - cy() - Ty()) / fy();
// ray.z = 1.0;
// return ray;
I have the same question wiht you, even not every pixel has a depth with it , it maybe because the direction of camera or some obstacle before the target. Above is just get the 2d point and the 3d ray, but I don't understand what is the next to get the specific point in 3D