apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

pose estination updated

Browse Source
This commit is contained in:
Apoorva Gupta 2023-03-22 11:22:45 +05:30
parent 61e7117511
commit 0e1db823a8
1 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ros2_ws/src/find-pose/src/FindObjectROS.cpp
View File

@ -59,9 +59,9 @@ FindObjectROS::FindObjectROS(rclcpp::Node * node) :
RCLCPP_INFO(node->get_logger(), "object_prefix = %s", objFramePrefix_.c_str()); RCLCPP_INFO(node->get_logger(), "object_prefix = %s", objFramePrefix_.c_str());
RCLCPP_INFO(node->get_logger(), "pnp = %s", usePnP_?"true":"false"); RCLCPP_INFO(node->get_logger(), "pnp = %s", usePnP_?"true":"false");
image_transport::TransportHints hints(node); image_transport::TransportHints hints(node);
rgbSub_.subscribe(node, "/camera/color/image_raw", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile()); rgbSub_.subscribe(node, "rgb_img", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
depthSub_.subscribe(node, "/camera/aligned_depth_to_color/image_raw", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile()); depthSub_.subscribe(node, "/depth_img", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
cameraInfoSub_.subscribe(node, "/camera/color/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile()); cameraInfoSub_.subscribe(node, "/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
bboxSub_.subscribe(node, "/bboxes", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile()); bboxSub_.subscribe(node, "/bboxes", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1).get_rmw_qos_profile());
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_, bboxSub_); approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), rgbSub_, depthSub_, cameraInfoSub_, bboxSub_);
@ -78,6 +78,7 @@ void FindObjectROS::estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSha
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
const yolov3_msg::msg::BoundingBoxes::ConstSharedPtr bboxes) const yolov3_msg::msg::BoundingBoxes::ConstSharedPtr bboxes)
{ {
std::cout << "callback!!!!!!!!" << std::endl;
if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 && if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0) depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0)
{ {
@ -106,7 +107,8 @@ void FindObjectROS::estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSha
size_t num_detections = bboxes->bounding_boxes.size(); size_t num_detections = bboxes->bounding_boxes.size();
std::cout << "num_detections: " << num_detections << std::endl; std::cout << "num_detections: " << num_detections << std::endl;
std::vector<geometry_msgs::msg::TransformStamped> pose_vec; geometry_msgs::msg::TransformStamped left_trail;
geometry_msgs::msg::TransformStamped right_trail;
for(size_t i = 0; i < num_detections ; i++) for(size_t i = 0; i < num_detections ; i++)
{ {
yolov3_msg::msg::BoundingBox object_bbox = bboxes->bounding_boxes[i]; yolov3_msg::msg::BoundingBox object_bbox = bboxes->bounding_boxes[i];
@ -120,12 +122,15 @@ void FindObjectROS::estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSha
std::string object_class = object_bbox.class_id; std::string object_class = object_bbox.class_id;
geometry_msgs::msg::TransformStamped pose = estimate_pose(image, ptrDepth->image, object_width, object_height, top_left, top_right, bottom_right, bottom_left); geometry_msgs::msg::TransformStamped pose = estimate_pose(image, ptrDepth->image, object_width, object_height, top_left, top_right, bottom_right, bottom_left);
pose.child_frame_id = object_class; pose.child_frame_id = object_class;
pose_vec.push_back(pose); if (object_class == std::string("l_trail"))
left_trail = pose;
if (object_class == std::string("r_trail"))
right_trail = pose;
std::cout << "Pose for "<< object_class << " x: " << pose.transform.translation.x << " y: " << pose.transform.translation.y << " z: " << pose.transform.translation.z << std::endl; std::cout << "Pose for "<< object_class << " x: " << pose.transform.translation.x << " y: " << pose.transform.translation.y << " z: " << pose.transform.translation.z << std::endl;
} }
// geometry_msgs::msg::TransformStamped right_trail = estimate_pose(image, ptrDepth->image, 115, 116, cv::Point2f(819,600), cv::Point2f(974,600), cv::Point2f(974,716), cv::Point2f(819,716)); // geometry_msgs::msg::TransformStamped right_trail = estimate_pose(image, ptrDepth->image, 115, 116, cv::Point2f(819,600), cv::Point2f(974,600), cv::Point2f(974,716), cv::Point2f(819,716));
// geometry_msgs::msg::TransformStamped left_trail = estimate_pose(image, ptrDepth->image, 160, 168, cv::Point2f(350,548), cv::Point2f(510,548), cv::Point2f(510,716), cv::Point2f(350,716)); // geometry_msgs::msg::TransformStamped left_trail = estimate_pose(image, ptrDepth->image, 160, 168, cv::Point2f(350,548), cv::Point2f(510,548), cv::Point2f(510,716), cv::Point2f(350,716));
// std::cout << "Distance in meters !!!!!!" << distance(left_trail, right_trail) << std::endl; std::cout << "Distance in meters !!!!!!" << distance(left_trail, right_trail) << std::endl;
std::cout << std::endl; std::cout << std::endl;
} }
catch(const cv_bridge::Exception & e) catch(const cv_bridge::Exception & e)
@ -195,7 +200,7 @@ geometry_msgs::msg::TransformStamped FindObjectROS::estimate_pose(cv::Mat rgb_im
cv::Mat tvec(1,3, CV_64FC1); cv::Mat tvec(1,3, CV_64FC1);
cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec); cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
std::cout << "Pose tvec x: " << tvec.at<double>(0) << " y: " << tvec.at<double>(1) << " z: " << tvec.at<double>(2) << std::endl; // std::cout << "Pose tvec x: " << tvec.at<double>(0) << " y: " << tvec.at<double>(1) << " z: " << tvec.at<double>(2) << std::endl;
cv::Mat R; cv::Mat R;
cv::Rodrigues(rvec, R); cv::Rodrigues(rvec, R);