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

This package converts point cloud to depth image. Also has code to find pose based on depth

Browse Source
This commit is contained in:
Apoorva Gupta 2023-03-03 18:59:55 +05:30
parent 13c51ae2a2
commit cb6fdc66b5
6 changed files with 645 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
ros2_ws/src/find-pose/CMakeLists.txt Normal file
View File

@ -0,0 +1,93 @@
cmake_minimum_required(VERSION 3.8)
project(find-pose)
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(Boost REQUIRED COMPONENTS system) # dependencies from PCL
find_package(PCL REQUIRED COMPONENTS common io)
find_package(depth_image_proc REQUIRED)
include_directories(SYSTEM
${PCL_INCLUDE_DIRS}
${rcl_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
include_directories(${depth_image_proc_INCLUDE_DIRS})
SET(Libraries
pcl_conversions
)
####### DEPENDENCIES #######
FIND_PACKAGE(OpenCV REQUIRED) # tested on 2.3.1
# check if version status is "-dev" (SIFT compatibility issue between 4.3.0 vs 4.3.0-dev)
FIND_FILE(OpenCV_VERSION_HPP opencv2/core/version.hpp
PATHS ${OpenCV_INCLUDE_DIRS}
NO_DEFAULT_PATH)
FILE(READ ${OpenCV_VERSION_HPP} TMPTXT)
STRING(FIND "${TMPTXT}" "-dev" matchres)
IF(NOT ${matchres} EQUAL -1)
add_definitions(-DOPENCV_DEV)
ENDIF(NOT ${matchres} EQUAL -1)
SET(NONFREE 1)
IF(NOT (OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND))
SET(NONFREE 0)
ELSEIF(OpenCV_VERSION VERSION_GREATER "3.4.2")
FIND_FILE(OpenCV_MODULES_HPP opencv2/opencv_modules.hpp
PATHS ${OpenCV_INCLUDE_DIRS}
NO_DEFAULT_PATH)
FILE(READ ${OpenCV_MODULES_HPP} TMPTXT)
STRING(FIND "${TMPTXT}" "#define OPENCV_ENABLE_NONFREE" matchres)
IF(${matchres} EQUAL -1)
SET(NONFREE 0)
ENDIF(${matchres} EQUAL -1)
ENDIF()
############################################################################
find_package(ament_cmake REQUIRED)
find_package(cv_bridge REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(pcl_conversions REQUIRED)
find_package(pcl_ros REQUIRED)
find_package(rclcpp REQUIRED)
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)
target_link_libraries(convert_pointcloud_to_depth ${PCL_LBRARIES} ${Boost_LIBRARIES})
include_directories(
include
${catkin_INCLUDE_DIRS}
)
SET(SRC_FILES
${SRC_FILES}
src/FindObjectROS.cpp
)
find_package(Qt5 COMPONENTS Core Widgets REQUIRED)
include_directories(${Qt5Core_INCLUDE_DIRS} ${Qt5Widgets_INCLUDE_DIRS})
#ADD_LIBRARY(find_object ${SRC_FILES})
add_executable(find_pose_node src/FindObjectROS.cpp src/find_pose_node.cpp)
set_target_properties(find_pose_node PROPERTIES OUTPUT_NAME find_pose_node)
ament_target_dependencies(find_pose_node rclcpp rclcpp_components cv_bridge tf2_msgs tf2_geometry_msgs ${AMENT_LIBRARIES})
target_link_libraries(find_pose_node Qt5::Core Qt5::Widgets ${OpenCV_LIBRARIES} ${LIBRARIES})
install(TARGETS
convert_pointcloud_to_depth
find_pose_node
DESTINATION lib/${PROJECT_NAME})
ament_package()

75
ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h Normal file
View File

@ -0,0 +1,75 @@
/*
Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef FINDOBJECTROS_H_
#define FINDOBJECTROS_H_
#include <rclcpp/rclcpp.hpp>
#include <cv_bridge/cv_bridge.h>
#include <tf2_ros/transform_broadcaster.h>
// #include "find_object/FindObject.h"
#include <std_msgs/msg/float32_multi_array.hpp>
// #include "find_object_2d/msg/objects_stamped.hpp"
// #include "find_object_2d/msg/detection_info.hpp"
// #include <QtCore/QObject>
// #include <QtCore/QMultiMap>
// #include <QtCore/QPair>
// #include <QtCore/QRect>
// #include <QtGui/QTransform>
class FindObjectROS
{
// Q_OBJECT;
public:
FindObjectROS(rclcpp::Node * node);
virtual ~FindObjectROS() {}
// public Q_SLOTS:
// void publish(const find_object::DetectionInfo & info, const find_object::Header & header, const cv::Mat & depth, float depthConstant);
void estimate_pose();
private:
cv::Vec3f getDepth(const cv::Mat & depthImage,
int x, int y,
float cx, float cy,
float fx, float fy);
private:
rclcpp::Node * node_;
rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr pub_;
// rclcpp::Publisher<find_object_2d::msg::ObjectsStamped>::SharedPtr pubStamped_;
// rclcpp::Publisher<find_object_2d::msg::DetectionInfo>::SharedPtr pubInfo_;
std::string objFramePrefix_;
bool usePnP_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
};
#endif /* FINDOBJECTROS_H_ */

31
ros2_ws/src/find-pose/package.xml Normal file
View File

@ -0,0 +1,31 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>find-pose</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="eic.apoorva@gmail.com">parallels</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>libpcl-all-dev</build_depend>
<depend>rclcpp</depend>
<depend>pcl</depend>
<depend>sensor_msgs</depend>
<build_depend>pcl_conversions</build_depend>
<exec_depend>pcl_conversions</exec_depend>
<build_depend>depth_image_proc</build_depend>
<exec_depend>depth_image_proc</exec_depend>
<depend>pcl_ros</depend>
<build_depend>cv_bridge</build_depend>
<exec_depend>cv_bridge</exec_depend>
<build_depend>pcl_msgs</build_depend>
<exec_depend>pcl_msgs</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

218
ros2_ws/src/find-pose/src/FindObjectROS.cpp Normal file
View File

@ -0,0 +1,218 @@
/*
Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "find-pose/FindObjectROS.h"
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2/LinearMath/Matrix3x3.h>
#ifdef PRE_ROS_HUMBLE
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#endif
#include <cmath>
#include <opencv2/opencv.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <cv_bridge/cv_bridge.h>
#include <QtCore/QPointF>
// using namespace find_object;
FindObjectROS::FindObjectROS(rclcpp::Node * node) :
node_(node),
objFramePrefix_("object"),
usePnP_(true)
{
tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node);
objFramePrefix_ = node->declare_parameter("object_prefix", objFramePrefix_);
usePnP_ = node->declare_parameter("pnp", usePnP_);
RCLCPP_INFO(node->get_logger(), "object_prefix = %s", objFramePrefix_.c_str());
RCLCPP_INFO(node->get_logger(), "pnp = %s", usePnP_?"true":"false");
// pub_ = node->create_publisher<std_msgs::msg::Float32MultiArray>("objects", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1));
// pubStamped_ = node->create_publisher<find_object_2d::msg::ObjectsStamped>("objectsStamped", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)1));
// 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)));
}
void FindObjectROS::estimate_pose()
{
// Read RGB Image
cv::Mat rgb_image = cv::imread("/media/psf/freelancing/greenhouse/yolov3/runs/detect/exp8/stereo_image952.jpeg");
// read depth Image
cv::Mat depth = cv::imread("/media/psf/freelancing/greenhouse/depth_try1/depth_img_952.jpeg");
// bounding box dimensions
std::vector<geometry_msgs::msg::TransformStamped> 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;
std::vector<cv::Point3f> objectPoints(4);
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(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);
float depthConstant = 1.0f/641.587158203125;
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;
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<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(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);
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);
}
}
cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,
int x, int y,
float cx, float cy,
float fx, float fy)
{
if(!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows))
{
RCLCPP_ERROR(node_->get_logger(), "Point must be inside the image (x=%d, y=%d), image size=(%d,%d)",
x, y,
depthImage.cols, depthImage.rows);
return cv::Vec3f(
std::numeric_limits<float>::quiet_NaN (),
std::numeric_limits<float>::quiet_NaN (),
std::numeric_limits<float>::quiet_NaN ());
}
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)
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 bad_point = std::numeric_limits<float>::quiet_NaN ();
float depth;
bool isValid;
if(isInMM)
{
std::cout << "depth is in mm!!" << std::endl;
depth = (float)depthImage.at<uint16_t>(y,x);
isValid = depth != 0.0f;
}
else
{
depth = depthImage.at<float>(y,x);
isValid = std::isfinite(depth) && depth > 0.0f;
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;
}
else
{
// Fill in XYZ
pt.val[0] = (float(x) - center_x) * depth * constant_x;
pt.val[1] = (float(y) - center_y) * depth * constant_y;
pt.val[2] = depth*unit_scaling;
std::cout << "depth pt: " << pt.val[0] << " y: " << pt.val[1] << " z: " << pt.val[2] << std::endl;
}
return pt;
}

176
ros2_ws/src/find-pose/src/convert_pointcloud_to_depth.cpp Normal file
View File

@ -0,0 +1,176 @@
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/filters/voxel_grid.h>
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));
subscription_ = create_subscription<sensor_msgs::msg::PointCloud2>(
"/camera/depth/color/points", 10, std::bind(&PointCloudToDepthImageNode::pc_callback, this, std::placeholders::_1));
publisher_ = create_publisher<sensor_msgs::msg::Image>("/depth_image_topic", rclcpp::QoS(10));
}
private:
// void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
// // Create a point cloud object from the message
// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// pcl::moveFromROSMsg(*msg, *cloud);
// // Determine the size of the depth image (assume 640x480 resolution)
// const int width = 1280;
// const int height = 720;
// // Create a depth image
// cv::Mat depth_image(height, width, CV_32F, cv::Scalar::all(0));
// // Convert the point cloud to a depth image
// for (int i = 0; i < cloud->points.size(); i++) {
// pcl::PointXYZ point = cloud->points[i];
// // Calculate the pixel coordinates of the point in the depth image
// int u = (int)(point.x / point.z * fx_ + cx_);
// int v = (int)(point.y / point.z * fy_ + cy_);
// // Ignore points that are outside of the image bounds
// if (u < 0 || u >= width || v < 0 || v >= height) {
// continue;
// }
// // Calculate the depth of the point
// float depth = sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
// // Update the depth image
// depth_image.at<float>(v, u) = depth;
// }
// // Convert the depth image to a ROS message and publish it
// sensor_msgs::msg::Image::SharedPtr msg_out = cv_bridge::CvImage(std_msgs::msg::Header(), sensor_msgs::image_encodings::TYPE_32FC1, depth_image).toImageMsg();
// publisher_->publish(*msg_out);
// cv::imwrite("/media/psf/freelancing/greenhouse/depth_try1/depth_img_" + std::to_string(count) + ".jpeg" , depth_image);
// count++;
// }
// second method to use both rgb and point cloud to create depth image
void rgb_callback(const sensor_msgs::msg::Image::SharedPtr msg)
{
std::cout << "rgb cb" << std::endl;
// Store the RGB image message
rgb_msg_ = msg;
}
void pc_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
{
std::cout << "pc cb" << std::endl;
// Convert the point cloud message to PCL point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*msg, *cloud);
// if(cloud->isOrganized())
// {
// std::cout << "isUnorganized"<< std::endl;
// }
// Convert the RGB image message to OpenCV Mat
cv_bridge::CvImageConstPtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvShare(rgb_msg_, sensor_msgs::image_encodings::BGR8);
}
catch (const cv_bridge::Exception& e)
{
RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
return;
}
// Create the depth image with the same size and type as the RGB image
cv::Mat depth_img(cv_ptr->image.rows, cv_ptr->image.cols, CV_32FC1);
std::cout << "Depth img size: " << cv_ptr->image.rows << " " << cv_ptr->image.cols << std::endl;
std::cout << "total points: " << cloud->points.size() << std::endl;
for (int i = 0; i < cloud->points.size(); i++) {
pcl::PointXYZ point = cloud->points[i];
// Calculate the pixel coordinates of the point in the depth image
int u = (int)(point.x / point.z * fx_ + cx_);
int v = (int)(point.y / point.z * fy_ + cy_);
std::cout << "u: " << point.x << " v: " << point.y << std::endl;
// Ignore points that are outside of the image bounds
if (u < 0 || u >= cv_ptr->image.rows || v < 0 || v >= cv_ptr->image.cols) {
continue;
}
// Calculate the depth of the point
float depth = sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
// Update the depth image
// depth_image.at<float>(v, u) = depth;
}
// // Create a VoxelGrid filter object
// pcl::VoxelGrid<pcl::PointXYZRGB> voxel_grid;
// // Set the leaf size of the filter
// voxel_grid.setLeafSize(0.01f, 0.01f, 0.01f); // The leaf size determines the size of the grid cells
// // Set the input cloud for the filter
// voxel_grid.setInputCloud(*cloud);
// // Create an empty point cloud to store the output (organized) cloud
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_organized(new pcl::PointCloud<pcl::PointXYZRGB>);
// // Apply the filter
// voxel_grid.filter(*cloud_organized);
// int width = cloud_organized->width; // Width of the point cloud
// int height = cloud_organized->height;
// std::cout << "height: " << height <<" width: " << width << std::endl;
// Iterate over each pixel in the RGB image and get the corresponding depth value from the point cloud
for (int i = 0; i < cv_ptr->image.rows; i++)
{
for (int j = 0; j < cv_ptr->image.cols; j++)
{
// Get the 3D point from the point cloud corresponding to the current pixel in the RGB image
pcl::PointXYZ pt = cloud->at(j, i);
// Set the depth value of the current pixel in the depth image to the z-coordinate of the 3D point
depth_img.at<float>(i, j) = pt.z;
std::cout << "pt z: " << pt.z << std::endl;
}
}
// Convert the depth image to ROS message and publish
sensor_msgs::msg::Image::SharedPtr depth_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "32FC1", depth_img).toImageMsg();
publisher_->publish(*depth_msg);
}
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_rgb_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher_;
sensor_msgs::msg::Image::SharedPtr rgb_msg_;
// Camera intrinsic parameters (replace with actual values)
float fx_ = 643.008;
float fy_ = 642.201;
float cx_ = 648.812;
float cy_ = 360.262;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<PointCloudToDepthImageNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}

52
ros2_ws/src/find-pose/src/find_pose_node.cpp Normal file
View File

@ -0,0 +1,52 @@
/*
Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "find-pose/FindObjectROS.h"
class FindPoseNode : public rclcpp::Node {
public:
FindPoseNode() :
rclcpp::Node("find_object_2d"),
findObjectROS_(0)
{
findObjectROS_ = new FindObjectROS(this);
findObjectROS_->estimate_pose();
}
private:
FindObjectROS * findObjectROS_;
};
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<FindPoseNode>();
rclcpp::spin(node);
rclcpp::shutdown();
}