From 7a7f46052b63ab051de988e3fce927349fb1338b Mon Sep 17 00:00:00 2001 From: apoorva Date: Fri, 10 Mar 2023 15:46:04 +0530 Subject: [PATCH] two object pose estimation but failing --- ros2_ws/src/find-pose/src/FindObjectROS.cpp | 228 ++++++++++---------- 1 file changed, 115 insertions(+), 113 deletions(-) diff --git a/ros2_ws/src/find-pose/src/FindObjectROS.cpp b/ros2_ws/src/find-pose/src/FindObjectROS.cpp index b548a2ce..4571235d 100644 --- a/ros2_ws/src/find-pose/src/FindObjectROS.cpp +++ b/ros2_ws/src/find-pose/src/FindObjectROS.cpp @@ -63,119 +63,121 @@ 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))); + + + } void FindObjectROS::estimate_pose() { + 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/yolov3/runs/detect/exp8/stereo_image952.jpeg"); + cv::Mat rgb_image = cv::imread("/media/psf/freelancing/greenhouse/stereo/stereo_5.jpg"); // read depth Image - cv::Mat depth = cv::imread("/media/psf/freelancing/greenhouse/depth_try1/depth_img_952.jpeg"); + cv::Mat depth_img = cv::imread("/media/psf/freelancing/greenhouse/depth/depth_4.jpg", cv::IMREAD_ANYDEPTH); // bounding box dimensions - - std::vector transforms; - // store bounding boxes. (This should come from yolo ideally) - float objectWidth = 372; // width of bounding box in pixels - float objectHeight = 554; // 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/641.587158203125; - std::vector objectPoints(6); - std::vector imagePoints(6); - cv::Vec3f point_1 = this->getDepth(depth, - 246,665, - float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f, - 1.0f/depthConstant, 1.0f/depthConstant); - cv::Vec3f point_2 = this->getDepth(depth, - 618, 665, - float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f, - 1.0f/depthConstant, 1.0f/depthConstant); - cv::Vec3f point_3 = this->getDepth(depth, - 618, 111, - float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f, - 1.0f/depthConstant, 1.0f/depthConstant); - cv::Vec3f point_4 = this->getDepth(depth, - 246, 111, - float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f, - 1.0f/depthConstant, 1.0f/depthConstant); - cv::Vec3f point_5 = this->getDepth(depth, - 432, 386, - float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f, - 1.0f/depthConstant, 1.0f/depthConstant); - cv::Vec3f point_6 = this->getDepth(depth, - 537,224, - float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f, - 1.0f/depthConstant, 1.0f/depthConstant); - objectPoints[0] = cv::Point3f(point_1.val[0], point_1.val[1], point_1.val[2]); - objectPoints[1] = cv::Point3f(point_2.val[0], point_2.val[1], point_2.val[2]); - objectPoints[2] = cv::Point3f(point_3.val[0], point_3.val[1], point_3.val[2]); - objectPoints[3] = cv::Point3f(point_4.val[0], point_4.val[1], point_4.val[2]); - objectPoints[4] = cv::Point3f(point_5.val[0], point_5.val[1], point_5.val[2]); - objectPoints[5] = cv::Point3f(point_6.val[0], point_6.val[1], point_6.val[2]); - - // QPointF pt = iter->map(QPointF(0, 0)); - imagePoints[0] = cv::Point2f(246,665); - // pt = iter->map(QPointF(objectWidth, 0)); - imagePoints[1] = cv::Point2f(618, 665); - // pt = iter->map(QPointF(objectWidth, objectHeight)); - imagePoints[2] = cv::Point2f(618,111); - // pt = iter->map(QPointF(0, objectHeight)); - imagePoints[3] = cv::Point2f(246, 111); - imagePoints[4] = cv::Point2f(432, 386); - imagePoints[5] = cv::Point2f(537, 224); - - - 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; - std::cout << "depth img cols: " << depth.cols << " rows: " << depth.rows << std::endl; - 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(objectWidth/2, 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); + // Convert the image to a single-channel 16-bit image + cv::Mat depth; + depth_img.convertTo(depth, CV_16UC1); - 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()) + std::vector> imagePoints_; + std::vector imagePoints_oj; + imagePoints_oj.resize(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 + + 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++) { - tfBroadcaster_->sendTransform(transforms); + 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); + + 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); + } + } } @@ -199,24 +201,24 @@ cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage, cv::Vec3f pt; // Use correct principal point from calibration - float center_x = cx; //cameraInfo.K.at(2) - float center_y = cy; //cameraInfo.K.at(5) + float center_x = cx; //cameraInfo.K.at(2) // 656.1123046875 + float center_y = cy; //cameraInfo.K.at(5) // 361.80828857421875 - // std::cout << "cx: " << center_x << " cy: " << center_y << std::endl; - // std::cout << "fx: " << fx << " fy: " << fy << std::endl; + std::cout << "cx: " << center_x << " cy: " << center_y << std::endl; + std::cout << "fx: " << fx << " fy: " << fy << std::endl; bool isInMM = depthImage.type() == CV_16UC1; // is in mm? // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) float unit_scaling = isInMM?0.001f:1.0f; - float constant_x = unit_scaling / fx; //cameraInfo.K.at(0) - float constant_y = unit_scaling / fy; //cameraInfo.K.at(4) + float constant_x = unit_scaling / fx; //cameraInfo.K.at(0) // 642.437744140625 + float constant_y = unit_scaling / fy; //cameraInfo.K.at(4) // 640.8887939453125 float bad_point = std::numeric_limits::quiet_NaN (); float depth; bool isValid; if(isInMM) { - // std::cout << "depth is in mm!!" << std::endl; + std::cout << "depth is in mm!!" << std::endl; depth = (float)depthImage.at(y,x); isValid = depth != 0.0f; } @@ -225,14 +227,14 @@ cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage, depth = depthImage.at(y,x); isValid = std::isfinite(depth) && depth > 0.0f; - // std::cout << "depth is NOT in mm!! " << depth << std::endl; + std::cout << "depth is NOT in mm!! " << depth << std::endl; } // Check for invalid measurements if (!isValid) { pt.val[0] = pt.val[1] = pt.val[2] = bad_point; - // std::cout << "depth pt: " << pt.val[0] << " y: " << pt.val[1] << " z: " << pt.val[2] << std::endl; + std::cout << "depth pt: " << pt.val[0] << " y: " << pt.val[1] << " z: " << pt.val[2] << std::endl; } else {