code updated to use 6 points

ros2_ws/src/find-pose/CMakeLists.txt

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

ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h

@@ -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_ */

ros2_ws/src/find-pose/package.xml

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

ros2_ws/src/find-pose/src/FindObjectROS.cpp

@@ -0,0 +1,246 @@
+/*
+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;
+    float depthConstant = 1.0f/641.587158203125;
+    std::vector<cv::Point3f> objectPoints(6);
+    std::vector<cv::Point2f> 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<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;
+                // std::cout << "depth pt: " << pt.val[0] << " y: " << pt.val[1] << " z: " << pt.val[2] << std::endl;
+	}
+	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;
+}

ros2_ws/src/find-pose/src/convert_pointcloud_to_depth.cpp

@@ -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;
+}

ros2_ws/src/find-pose/src/find_pose_node.cpp

@@ -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();
+}