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

conversion code works as well as pose estimation for two object works(not using topics)

Browse Source
This commit is contained in:
Apoorva Gupta 2023-03-10 16:22:58 +05:30
parent 7a7f46052b
commit f33742e6e8
6 changed files with 203 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ros2_ws/src/convert_2_img/convert_2_img/convert_to_img.py
View File

@ -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()

4
ros2_ws/src/find-pose/CMakeLists.txt
View File

@ -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(

6
ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h
View File

@ -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<tf2_ros::TransformBroadcaster> tfBroadcaster_;
unsigned int num_objects_;
std::vector<double> objectHeight_;
std::vector<double> objectWidth_;
};

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

@ -63,21 +63,12 @@ FindObjectROS::FindObjectROS(rclcpp::Node * node) :
// pubInfo_ = node->create_publisher<find_object_2d::msg::DetectionInfo>("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
@ -87,33 +78,14 @@ void FindObjectROS::estimate_pose()
cv::Mat depth;
depth_img.convertTo(depth, CV_16UC1);
std::vector<std::vector<cv::Point2f>> imagePoints_;
std::vector<cv::Point2f> imagePoints_oj;
imagePoints_oj.resize(4);
std::vector<cv::Point2f> 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++)
{
std::vector<geometry_msgs::msg::TransformStamped> 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
// 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;
@ -122,21 +94,36 @@ void FindObjectROS::estimate_pose()
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.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<cv::Point3f> objectPoints(4);
std::cout << "image_points: " << imagePoints_[i] << std::endl;
std::vector<cv::Point2f> imagePoints = imagePoints_[i];
std::cout << "Image points: " << imagePoints[0].x <<" y: " << imagePoints[0].y << std::endl;
std::vector<cv::Point2f> imagePoints(4);
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;
cv::Mat cameraMatrix = cv::Mat::eye(3,3,CV_64FC1);
cameraMatrix.at<double>(0,0) = 1.0f/depthConstant;
cameraMatrix.at<double>(1,1) = 1.0f/depthConstant;
@ -148,6 +135,7 @@ void FindObjectROS::estimate_pose()
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;
cv::Mat R;
cv::Rodrigues(rvec, R);
tf2::Matrix3x3 rotationMatrix(
@ -155,7 +143,9 @@ void FindObjectROS::estimate_pose()
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2));
rotationMatrix.getRotation(q);
QPointF center = QPointF(imagePoints[0].x + objectWidth/2, imagePoints[0].y + objectHeight/2);
// 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,
@ -173,12 +163,11 @@ void FindObjectROS::estimate_pose()
q *= q2;
transform.transform.rotation = tf2::toMsg(q.normalized());
transforms.push_back(transform);
if(transforms.size())
{
tfBroadcaster_->sendTransform(transforms);
}
}
// if(transforms.size())
// {
// tfBroadcaster_->sendTransform(transforms);
// }
std::cout << "Done!!!!!!!!!!!!" << std::endl;
}
cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,

103
ros2_ws/src/find-pose/src/convert_pointcloud_to_depth.cpp
View File

@ -11,19 +11,116 @@
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/passthrough.h>
#include <pcl/common/transforms.h>
#include <iostream>
int count = 0;
class PointCloudToDepthImageNode : public rclcpp::Node {
public:
PointCloudToDepthImageNode() : Node("pointcloud_to_depth_image") {
sub_rgb_ = create_subscription<sensor_msgs::msg::Image>(
"/camera/color/image_raw", 10, std::bind(&PointCloudToDepthImageNode::rgb_callback, this, std::placeholders::_1));
// sub_rgb_ = create_subscription<sensor_msgs::msg::Image>(
// "/camera/color/image_raw", 10, std::bind(&PointCloudToDepthImageNode::rgb_callback, this, std::placeholders::_1));
subscription_ = create_subscription<sensor_msgs::msg::PointCloud2>(
"/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<sensor_msgs::msg::Image>("/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<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::moveFromROSMsg(*msg, *cloud);
// Remove NaN points
// std::vector<int> indices;
// pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
// Initialize the organized point cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr organized_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
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<pcl::PointXYZRGB> 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<int>((h_angle - h_fov_min) / h_resolution);
int row = static_cast<int>((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

3
ros2_ws/src/find-pose/src/find_pose_node.cpp
View File

@ -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: