conversion code works as well as pose estimation for two object works(not using topics)
This commit is contained in:
parent
7a7f46052b
commit
f33742e6e8
@ -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()
|
||||
|
||||
@ -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(
|
||||
|
||||
@ -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_;
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
@ -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
|
||||
@ -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<std::vector<cv::Point2f>> imagePoints_;
|
||||
std::vector<cv::Point2f> imagePoints_oj;
|
||||
imagePoints_oj.resize(4);
|
||||
|
||||
std::vector<geometry_msgs::msg::TransformStamped> 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<cv::Point3f> objectPoints(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
|
||||
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<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
|
||||
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<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;
|
||||
|
||||
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<double>(0,0) = 1.0f/depthConstant;
|
||||
cameraMatrix.at<double>(1,1) = 1.0f/depthConstant;
|
||||
cameraMatrix.at<double>(0,2) = float(depth.cols/2)-0.5f;
|
||||
cameraMatrix.at<double>(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<double>(0,0) = 1.0f/depthConstant;
|
||||
cameraMatrix.at<double>(1,1) = 1.0f/depthConstant;
|
||||
cameraMatrix.at<double>(0,2) = float(depth.cols/2)-0.5f;
|
||||
cameraMatrix.at<double>(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<double>(0) << " y: " << tvec.at<double>(1) << " z: " << tvec.at<double>(2) << std::endl;
|
||||
cv::Mat R;
|
||||
cv::Rodrigues(rvec, R);
|
||||
tf2::Matrix3x3 rotationMatrix(
|
||||
R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2),
|
||||
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);
|
||||
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<double>(0)*(center3D.val[2]/tvec.at<double>(2));
|
||||
transform.transform.translation.y = tvec.at<double>(1)*(center3D.val[2]/tvec.at<double>(2));
|
||||
transform.transform.translation.z = tvec.at<double>(2)*(center3D.val[2]/tvec.at<double>(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<double>(0) << " y: " << tvec.at<double>(1) << " z: " << tvec.at<double>(2) << std::endl;
|
||||
|
||||
}
|
||||
cv::Mat R;
|
||||
cv::Rodrigues(rvec, R);
|
||||
tf2::Matrix3x3 rotationMatrix(
|
||||
R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2),
|
||||
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(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<double>(0)*(center3D.val[2]/tvec.at<double>(2));
|
||||
transform.transform.translation.y = tvec.at<double>(1)*(center3D.val[2]/tvec.at<double>(2));
|
||||
transform.transform.translation.z = tvec.at<double>(2)*(center3D.val[2]/tvec.at<double>(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,
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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:
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user