diff --git a/ros2_ws/src/convert_2_img/convert_2_img/convert_to_img.py b/ros2_ws/src/convert_2_img/convert_2_img/convert_to_img.py index 36b8272c..4896569c 100644 --- a/ros2_ws/src/convert_2_img/convert_2_img/convert_to_img.py +++ b/ros2_ws/src/convert_2_img/convert_2_img/convert_to_img.py @@ -10,8 +10,8 @@ import threading class ImageSaver(Node): def __init__(self): super().__init__('image_saver') - self.depth_sub = self.create_subscription(Image, '/zed2i/zed_node/depth/depth_registered', self.depth_callback, 10) - self.image_sub = self.create_subscription(Image, '/zed2i/zed_node/left_raw/image_raw_color', self.image_callback, 10) + self.depth_sub = self.create_subscription(Image, '/camera/aligned_depth_to_color/image_raw', self.depth_callback, 1) + self.image_sub = self.create_subscription(Image, '/camera/color/image_raw', self.image_callback, 1) self.cv_bridge = CvBridge() self.depth_image = None self.color_image = None @@ -20,12 +20,13 @@ class ImageSaver(Node): def depth_callback(self, data): print("depth cb") - self.depth_image = self.cv_bridge.imgmsg_to_cv2(data) + self.depth_image = self.cv_bridge.imgmsg_to_cv2(data, desired_encoding="passthrough") self.save_images('d') def image_callback(self, data): print("image cb") + self.color_image = self.cv_bridge.imgmsg_to_cv2(data) self.save_images('s') @@ -51,7 +52,7 @@ class ImageSaver(Node): if __name__ == '__main__': rclpy.init() image_saver = ImageSaver() - rate = image_saver.create_rate(1) # 10 Hz + # rate = image_saver.create_rate(1) # 10 Hz rclpy.spin(image_saver) # while rclpy.ok(): # image_saver.save_images() diff --git a/ros2_ws/src/find-pose/CMakeLists.txt b/ros2_ws/src/find-pose/CMakeLists.txt index d859e755..881c55b4 100644 --- a/ros2_ws/src/find-pose/CMakeLists.txt +++ b/ros2_ws/src/find-pose/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.8) project(find-pose) - +add_compile_options(-g) # find dependencies find_package(ament_cmake REQUIRED) @@ -59,7 +59,7 @@ find_package(sensor_msgs REQUIRED) find_package(pcl_msgs REQUIRED) add_executable(convert_pointcloud_to_depth src/convert_pointcloud_to_depth.cpp) -ament_target_dependencies(convert_pointcloud_to_depth ${Libraries} rclcpp_components rclcpp sensor_msgs cv_bridge) +ament_target_dependencies(convert_pointcloud_to_depth ${Libraries} rclcpp_components rclcpp sensor_msgs cv_bridge pcl_conversions) target_link_libraries(convert_pointcloud_to_depth ${PCL_LBRARIES} ${Boost_LIBRARIES}) include_directories( diff --git a/ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h b/ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h index 46e08f27..63fc442c 100644 --- a/ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h +++ b/ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h @@ -53,7 +53,7 @@ public: // public Q_SLOTS: // void publish(const find_object::DetectionInfo & info, const find_object::Header & header, const cv::Mat & depth, float depthConstant); - void estimate_pose(); + void estimate_pose(float objecWidth, float objectHeight, cv::Point2f point_1, cv::Point2f point_2, cv::Point2f point_3, cv::Point2f point_4); private: cv::Vec3f getDepth(const cv::Mat & depthImage, int x, int y, @@ -69,6 +69,10 @@ private: std::string objFramePrefix_; bool usePnP_; std::shared_ptr tfBroadcaster_; + unsigned int num_objects_; + std::vector objectHeight_; + std::vector objectWidth_; + }; diff --git a/ros2_ws/src/find-pose/src/FindObjectROS.cpp b/ros2_ws/src/find-pose/src/FindObjectROS.cpp index 4571235d..7126952b 100644 --- a/ros2_ws/src/find-pose/src/FindObjectROS.cpp +++ b/ros2_ws/src/find-pose/src/FindObjectROS.cpp @@ -63,21 +63,12 @@ FindObjectROS::FindObjectROS(rclcpp::Node * node) : // pubInfo_ = node->create_publisher("info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1)); // this->connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(publish(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float))); - - - + num_objects_ = 2; + objectHeight_.resize(num_objects_); } -void FindObjectROS::estimate_pose() +void FindObjectROS::estimate_pose(float objectWidth, float objectHeight, cv::Point2f point_1, cv::Point2f point_2, cv::Point2f point_3, cv::Point2f point_4) { - std::cout << "OpenCV version string " << cv::getVersionString() << std::endl; - num_objects_ = 1; - objectHeight_.resize(num_objects_); - objectHeight_.push_back(168); - objectHeight_.push_back(116); - objectWidth_.resize(num_objects_); - objectWidth_.push_back(160); - objectWidth_.push_back(155); // Read RGB Image cv::Mat rgb_image = cv::imread("/media/psf/freelancing/greenhouse/stereo/stereo_5.jpg"); // read depth Image @@ -86,99 +77,97 @@ void FindObjectROS::estimate_pose() // Convert the image to a single-channel 16-bit image cv::Mat depth; depth_img.convertTo(depth, CV_16UC1); - - std::vector> imagePoints_; - std::vector imagePoints_oj; - imagePoints_oj.resize(4); + + std::vector transforms; + // store bounding boxes. (This should come from yolo ideally) +// float objectWidth = 160; // width of bounding box in pixels +// float objectHeight = 168; // height of bounding box in pixels +// float objectWidth = 155; // width of bounding box in pixels +// float objectHeight = 116; // height of bounding box in pixels + + + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation.x=0; + transform.transform.rotation.y=0; + transform.transform.rotation.z=0; + transform.transform.rotation.w=1; + transform.transform.translation.x=0; + transform.transform.translation.y=0; + transform.transform.translation.z=0; + transform.child_frame_id = "l_trail"; + transform.header.frame_id = "camera_link"; + // transform.header.stamp.sec = ros::Time::now(); + tf2::Quaternion q; + float depthConstant = 1.0f/640.8887939453125; // 0.00156033 // 1.0f/cameraInfoMsg->K[4]; + std::cout << "depthconstant: " << depthConstant << std::endl; + std::vector objectPoints(4); std::vector imagePoints(4); - imagePoints_oj[0] = cv::Point2f(350,548); - // pt = iter->map(QPointF(objectWidth, 0)); - imagePoints_oj[1] = cv::Point2f(510,548); - // pt = iter->map(QPointF(objectWidth, objectHeight)); - imagePoints_oj[2] = cv::Point2f(510,716); - // pt = iter->map(QPointF(0, objectHeight)); - imagePoints_oj[3] = cv::Point2f(350,716); - imagePoints_.push_back(imagePoints_oj); //pUSHED + objectPoints[0] = cv::Point3f(-0.5, -(objectHeight/objectWidth)/2.0f,0); + objectPoints[1] = cv::Point3f(0.5,-(objectHeight/objectWidth)/2.0f,0); + objectPoints[2] = cv::Point3f(0.5,(objectHeight/objectWidth)/2.0f,0); + objectPoints[3] = cv::Point3f(-0.5,(objectHeight/objectWidth)/2.0f,0); + // QPointF pt = iter->map(QPointF(0, 0)); +// imagePoints[0] = cv::Point2f(350,548); +// // pt = iter->map(QPointF(objectWidth, 0)); +// imagePoints[1] = cv::Point2f(510,548); +// // pt = iter->map(QPointF(objectWidth, objectHeight)); +// imagePoints[2] = cv::Point2f(510,716); +// // pt = iter->map(QPointF(0, objectHeight)); +// imagePoints[3] = cv::Point2f(350,716); + // QPointF pt = iter->map(QPointF(0, 0)); + imagePoints[0] = point_1; + // pt = iter->map(QPointF(objectWidth, 0)); + imagePoints[1] = point_2; + // pt = iter->map(QPointF(objectWidth, objectHeight)); + imagePoints[2] = point_3; + // pt = iter->map(QPointF(0, objectHeight)); + imagePoints[3] = point_4; - imagePoints[0] = cv::Point2f(819,600); - // pt = iter->map(QPointF(objectWidth, 0)); - imagePoints[1] = cv::Point2f(974,600); - // pt = iter->map(QPointF(objectWidth, objectHeight)); - imagePoints[2] = cv::Point2f(974,716); - // pt = iter->map(QPointF(0, objectHeight)); - imagePoints[3] = cv::Point2f(819,716); - imagePoints_.push_back(imagePoints); //pUSHED - for(unsigned int i = 0; i < num_objects_; i++) - { - std::vector transforms; - // store bounding boxes. (This should come from yolo ideally) - float objectWidth = objectWidth_[i]; // width of bounding box in pixels - float objectHeight = objectHeight_[i]; // height of bounding box in pixels - geometry_msgs::msg::TransformStamped transform; - transform.transform.rotation.x=0; - transform.transform.rotation.y=0; - transform.transform.rotation.z=0; - transform.transform.rotation.w=1; - transform.transform.translation.x=0; - transform.transform.translation.y=0; - transform.transform.translation.z=0; - transform.child_frame_id = "trail_" + std::to_string(i); - transform.header.frame_id = "camera_link"; - // transform.header.stamp.sec = ros::Time::now(); - tf2::Quaternion q; - float depthConstant = 1.0f/640.8887939453125; // 0.00156033 // 1.0f/cameraInfoMsg->K[4]; - std::cout << "depthconstant: " << depthConstant << std::endl; - std::vector objectPoints(4); - std::cout << "image_points: " << imagePoints_[i] << std::endl; - std::vector imagePoints = imagePoints_[i]; - std::cout << "Image points: " << imagePoints[0].x <<" y: " << imagePoints[0].y << std::endl; - objectPoints[0] = cv::Point3f(-0.5, -(objectHeight/objectWidth)/2.0f,0); - objectPoints[1] = cv::Point3f(0.5,-(objectHeight/objectWidth)/2.0f,0); - objectPoints[2] = cv::Point3f(0.5,(objectHeight/objectWidth)/2.0f,0); - objectPoints[3] = cv::Point3f(-0.5,(objectHeight/objectWidth)/2.0f,0); - cv::Mat cameraMatrix = cv::Mat::eye(3,3,CV_64FC1); - cameraMatrix.at(0,0) = 1.0f/depthConstant; - cameraMatrix.at(1,1) = 1.0f/depthConstant; - cameraMatrix.at(0,2) = float(depth.cols/2)-0.5f; - cameraMatrix.at(1,2) = float(depth.rows/2)-0.5f; - cv::Mat distCoeffs; - cv::Mat rvec(1,3, CV_64FC1); - cv::Mat tvec(1,3, CV_64FC1); - cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec); + cv::Mat cameraMatrix = cv::Mat::eye(3,3,CV_64FC1); + cameraMatrix.at(0,0) = 1.0f/depthConstant; + cameraMatrix.at(1,1) = 1.0f/depthConstant; + cameraMatrix.at(0,2) = float(depth.cols/2)-0.5f; + cameraMatrix.at(1,2) = float(depth.rows/2)-0.5f; + cv::Mat distCoeffs; + cv::Mat rvec(1,3, CV_64FC1); + cv::Mat tvec(1,3, CV_64FC1); + cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec); - std::cout << "Pose tvec x: " << tvec.at(0) << " y: " << tvec.at(1) << " z: " << tvec.at(2) << std::endl; - cv::Mat R; - cv::Rodrigues(rvec, R); - tf2::Matrix3x3 rotationMatrix( - R.at(0,0), R.at(0,1), R.at(0,2), - R.at(1,0), R.at(1,1), R.at(1,2), - R.at(2,0), R.at(2,1), R.at(2,2)); - rotationMatrix.getRotation(q); - QPointF center = QPointF(imagePoints[0].x + objectWidth/2, imagePoints[0].y + objectHeight/2); - cv::Vec3f center3D = this->getDepth(depth, - center.x()+0.5f, center.y()+0.5f, - float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f, - 1.0f/depthConstant, 1.0f/depthConstant); - std::cout << "center x: " << center.x() << " y: " << center.y() << std::endl; - std::cout << "center 3D x: " << center3D[0] << " y: " << center3D[1] <<" z: " << center3D[1] << std::endl; - - transform.transform.translation.x = tvec.at(0)*(center3D.val[2]/tvec.at(2)); - transform.transform.translation.y = tvec.at(1)*(center3D.val[2]/tvec.at(2)); - transform.transform.translation.z = tvec.at(2)*(center3D.val[2]/tvec.at(2)); - std::cout << "Pose x: " << transform.transform.translation.x << " y: " << transform.transform.translation.y << " z: " << transform.transform.translation.z << std::endl; - // set x axis going front of the object, with z up and y left - tf2::Quaternion q2; - q2.setRPY(CV_PI/2.0, CV_PI/2.0, 0); - q *= q2; - transform.transform.rotation = tf2::toMsg(q.normalized()); - transforms.push_back(transform); - if(transforms.size()) - { - tfBroadcaster_->sendTransform(transforms); - } + std::cout << "Pose tvec x: " << tvec.at(0) << " y: " << tvec.at(1) << " z: " << tvec.at(2) << std::endl; - } + cv::Mat R; + cv::Rodrigues(rvec, R); + tf2::Matrix3x3 rotationMatrix( + R.at(0,0), R.at(0,1), R.at(0,2), + R.at(1,0), R.at(1,1), R.at(1,2), + R.at(2,0), R.at(2,1), R.at(2,2)); + rotationMatrix.getRotation(q); + +// QPointF center = QPointF(imagePoints[0].x + objectWidth/2, imagePoints[0].y + objectHeight/2); + QPointF center = QPointF(442.252, 609.895); + cv::Vec3f center3D = this->getDepth(depth, + center.x()+0.5f, center.y()+0.5f, + float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f, + 1.0f/depthConstant, 1.0f/depthConstant); + std::cout << "center x: " << center.x() << " y: " << center.y() << std::endl; + std::cout << "center 3D x: " << center3D[0] << " y: " << center3D[1] <<" z: " << center3D[1] << std::endl; + + transform.transform.translation.x = tvec.at(0)*(center3D.val[2]/tvec.at(2)); + transform.transform.translation.y = tvec.at(1)*(center3D.val[2]/tvec.at(2)); + transform.transform.translation.z = tvec.at(2)*(center3D.val[2]/tvec.at(2)); + std::cout << "Pose x: " << transform.transform.translation.x << " y: " << transform.transform.translation.y << " z: " << transform.transform.translation.z << std::endl; + // set x axis going front of the object, with z up and y left + tf2::Quaternion q2; + q2.setRPY(CV_PI/2.0, CV_PI/2.0, 0); + q *= q2; + transform.transform.rotation = tf2::toMsg(q.normalized()); + transforms.push_back(transform); +// if(transforms.size()) +// { +// tfBroadcaster_->sendTransform(transforms); +// } + std::cout << "Done!!!!!!!!!!!!" << std::endl; } cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage, diff --git a/ros2_ws/src/find-pose/src/convert_pointcloud_to_depth.cpp b/ros2_ws/src/find-pose/src/convert_pointcloud_to_depth.cpp index 47d7c360..3ec95079 100644 --- a/ros2_ws/src/find-pose/src/convert_pointcloud_to_depth.cpp +++ b/ros2_ws/src/find-pose/src/convert_pointcloud_to_depth.cpp @@ -11,19 +11,116 @@ #include #include #include +#include +#include +#include +#include int count = 0; class PointCloudToDepthImageNode : public rclcpp::Node { public: PointCloudToDepthImageNode() : Node("pointcloud_to_depth_image") { - sub_rgb_ = create_subscription( - "/camera/color/image_raw", 10, std::bind(&PointCloudToDepthImageNode::rgb_callback, this, std::placeholders::_1)); + // sub_rgb_ = create_subscription( + // "/camera/color/image_raw", 10, std::bind(&PointCloudToDepthImageNode::rgb_callback, this, std::placeholders::_1)); subscription_ = create_subscription( - "/camera/depth/color/points", 10, std::bind(&PointCloudToDepthImageNode::pc_callback, this, std::placeholders::_1)); + "/camera/depth/color/points", 10, std::bind(&PointCloudToDepthImageNode::convert_unorganized_to_organized_pc, this, std::placeholders::_1)); publisher_ = create_publisher("/depth_image_topic", rclcpp::QoS(10)); } private: + void convert_unorganized_to_organized_pc(const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { + // Define lidar field of view parameters + double h_fov_max = 87.0; // Maximum horizontal angle in radians + double h_fov_min = 0; // Minimum horizontal angle in radians + double v_fov_max = 58.0; // Maximum vertical angle in radians + double v_fov_min = 0; // Minimum vertical angle in radians + + double width = 1280; + double height = 720; + float hfov_deg = 87.0; + float vfov_deg = 58.0; + double pi = 3.14159265359; + float hfov_rad = hfov_deg * (pi / 180); + float vfov_rad = vfov_deg * (pi / 180); + float h_angle_per_pixel = hfov_rad / 1280; // calculate the angle per pixel + // h_fov_max = (1280 / 2) / tan(h_angle_per_pixel / 2); // calculate the maximum range + float v_angle_per_pixel = vfov_rad / 720; // calculate the angle per pixel + // v_fov_max = (720 / 2) / tan(v_angle_per_pixel / 2); // calculate the maximum range + // Calculate angular resolutions + const double h_resolution = (h_fov_max - h_fov_min) / width; + const double v_resolution = (v_fov_max - v_fov_min) / height; + std::cout << "convert_unorganized_to_organized_pc : " << h_fov_max << " v: " << v_fov_max << std::endl; + // Load the unorganized point cloud from a PCD file + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + + pcl::moveFromROSMsg(*msg, *cloud); + + // Remove NaN points + // std::vector indices; + // pcl::removeNaNFromPointCloud(*cloud, *cloud, indices); + + // Initialize the organized point cloud + pcl::PointCloud::Ptr organized_cloud(new pcl::PointCloud); + organized_cloud->width = 1280; + organized_cloud->height = 720; + organized_cloud->is_dense = true; + organized_cloud->points.resize(organized_cloud->width * organized_cloud->height); + std::cout << "cloud width: " << cloud->width << " height: " << cloud->height << std::endl; + // pcl::PassThrough pass; + // pass.setInputCloud(cloud); + // pass.setFilterFieldName("x"); + // pass.setFilterLimitsNegative(false); + // pass.setFilterLimits(-tan(hfov_rad / 2.0), tan(hfov_rad / 2.0)); + // pass.filter(*cloud); + // pass.setInputCloud(cloud); + // pass.setFilterFieldName("y"); + // pass.setFilterLimitsNegative(false); + // pass.setFilterLimits(-tan(vfov_rad / 2.0), tan(vfov_rad / 2.0)); + // pass.filter(*cloud); + // Copy the points from the unorganized point cloud to the organized point cloud + // for (int i = 0; i < cloud->points.size(); ++i) + // { + // int row = i / cloud->width; + // int col = i % cloud->width; + // organized_cloud->at(col, row) = cloud->points[i]; + // } + // for (int i = 0; i < cloud->points.size(); ++i) + // { + // pcl::PointXYZRGB point = cloud->points[i]; + + // // std::cout << point.x << std::endl; + // int x = (point.x + tan(hfov_rad / 2.0)) / (tan(hfov_rad) / organized_cloud->width); + // int y = (point.y + tan(vfov_rad / 2.0)) / (tan(vfov_rad) / organized_cloud->height); + // int index = y * organized_cloud->width + x; + // std::cout << "point x: " << x << std::endl; + // std::cout << "point y: " << y << std::endl; + // std::cout << "point index: " << index << std::endl; + // if (organized_cloud->points[index].z == 0) + // { + // organized_cloud->points[index] = point; + // } + // else if (point.z < organized_cloud->points[index].z) + // { + // organized_cloud->points[index] = point; + // } + // } + for (int i = 0; i < cloud->size(); i++) { + pcl::PointXYZRGB point = cloud->points[i]; + double h_angle = atan2(point.y, point.x); + double v_angle = atan2(point.z, sqrt(point.x * point.x + point.y * point.y)); + int col = static_cast((h_angle - h_fov_min) / h_resolution); + int row = static_cast((v_angle - v_fov_min) / v_resolution); + // std::cout << "point col: " << col << " row: " << row << std::endl; + if (col >= 0 && col < organized_cloud->width && row >= 0 && row < organized_cloud->height) { + organized_cloud->at(col, row) = point; + } +} + + // Save the organized point cloud to a PCD file + // pcl::io::savePCDFileASCII("organized_cloud.pcd", *organized_cloud); + + } // void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { // // Create a point cloud object from the message diff --git a/ros2_ws/src/find-pose/src/find_pose_node.cpp b/ros2_ws/src/find-pose/src/find_pose_node.cpp index 42d1f1f5..b4bbc94a 100644 --- a/ros2_ws/src/find-pose/src/find_pose_node.cpp +++ b/ros2_ws/src/find-pose/src/find_pose_node.cpp @@ -35,7 +35,8 @@ class FindPoseNode : public rclcpp::Node { findObjectROS_(0) { findObjectROS_ = new FindObjectROS(this); - findObjectROS_->estimate_pose(); + findObjectROS_->estimate_pose(115, 116, cv::Point2f(819,600), cv::Point2f(974,600), cv::Point2f(974,716), cv::Point2f(819,716)); + findObjectROS_->estimate_pose(160, 168, cv::Point2f(350,548), cv::Point2f(510,548), cv::Point2f(510,716), cv::Point2f(350,716)); } private: