Ask Your Question
0

How to get xyz coordinates of pointcloud using Intel Real Sense D415?

asked 2021-05-26 15:55:49 -0500

arifzaman gravatar image

updated 2021-06-10 18:00:58 -0500

jayess gravatar image

I am working on Ubunto 20.4 with ROS noetic. I am having intel Real sense D415 camera by which I can visualize the point cloud on Rviz.

  1. I want to get xyz coordinates of each pointcloud and from that I want to separate min and max point xyz coordinates.
  2. After getting the min and max values of pointcloud i want to input my target xyz coordinates and want to compute the error difference.

I am very much new to ROS and doesn't have good programming experience as well. I will be very much thankful if anyone can share me any sample code or guide me step by step as i have searched almost all ROS forums but didn't get my answer.

I have gone through this code (https://www.programmersought.com/arti...) but it gives out several errors.

[ERROR] [1622062315.967796]: bad callback: <function callback_pointcloud at 0x7fe9857b61f0>
Traceback (most recent call last):
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/topics.py", line 750, in _invoke_callback
    cb(msg)
  File "/home/arif/catkin_ws/src/perception_pcl/pcl_ros/src/pcl_ros/xyz.py", line 14, in callback_pointcloud
    print (" x : %.3f  y: %.3f  z: %.3f") %(p[0],p[1],p[2])
TypeError: unsupported operand type(s) for %: 'NoneType' and 'tuple'

Kindly help me out, I will be very much thankful to you.

edit retag flag offensive close merge delete

Comments

1

Concerning your error message: The example code you referenced was written for Python2 but noetic uses python3 which requires brackets around the print statement. The correct syntax here , however, would be print (" x : %.3f y: %.3f z: %.3f" %(p[0], p[1], p[2])) (mind the position of the brackets). I just want to point out that that kind of sting formatting in not very common anymore in python3. See e.g. here for str.format or f-strings, which can be used more intuitively.

If you are new to ROS/ coding my general advice would be to go through some of the beginner tutorials first (if you have not done so). ROS has good tutorials for different levels of experience here and there are tons of python tutorials out there.

You might also want to have a look at the realsense ros wrapper examples

Phgo gravatar image Phgo  ( 2021-05-27 03:24:34 -0500 )edit

Thank you so much for your comment, I have corrected my code, and now its working and giving the xyz values.

arifzaman gravatar image arifzaman  ( 2021-05-29 16:17:07 -0500 )edit

2 Answers

Sort by ┬╗ oldest newest most voted
0

answered 2021-05-27 06:52:53 -0500

Ranjit gravatar image

updated 2021-05-27 06:55:52 -0500

Hello arifzaman,

I want to get xyz coordinates of each pointcloud

This answer represents how you can obtain x,y,z from UV (Image pixels).

https://answers.ros.org/question/3726...

from that I want to separate min and max point xyz coordinates.

Now based on these points you can check based on z which is near and which point is far. Also, you can get right and left by minimum x coordinate and maximum coordinate.

After getting the min and max values of pointcloud i want to input my target xyz coordinates

You have to then convert this coordinate with respect to the world. For example, I am using a robotic arm at that time I have to calculate this position with tf package so I can give the command to my robot arm to go at this desired location.

want to compute the error difference.

I don't get this point. You want to calculate error based on what?

I am very much new to ROS and doesn't have good programming experience as well.

If you don't have a good programming experience that it will be a bit tough for you, but I think you will achieve this.

If you are facing any issue with this. Just drop a comment will try my best to answer. I hope you got your answer if you are having more douts feel free to comment.

edit flag offensive delete link more

Comments

Respected Sir, Tons of thanks for your response. As soon as I had got your answer I started working to extract xyz coordinates and and I finally get it through this code(https://www.programmersought.com... but still struggling to get the max and min of xyz. If you don't mind can you plz elaborate how get this whether i used array or list function and then try to get the min and max or anything you might suggest.

My robot is to displace small grain of wheat/corn from one place to another so I have a plan to first get the max and min of point cloud xyz coordinates of grains so that my robot can understand how initial is the volume and after that I command the robot to displace the grains up to this desired xyz and got stop. Through this at each cycle of ...(more)

arifzaman gravatar image arifzaman  ( 2021-05-29 17:29:50 -0500 )edit

As soon as I had got your answer I started working to extract xyz coordinates and and I finally get it through this code(https://www.programmersought.com...

Broken link, can you please! fix this

but still struggling to get the max and min of xyz. If you don't mind can you plz elaborate how get this whether i used array or list function and then try to get the min and max or anything you might suggest.

It is an easy to trick you just have to check the distance from the camera to the detected object and calculate z means a distance of that object so you can get which object is near and which object is far, but before that obtain x,y,z from UV first.

Ranjit gravatar image Ranjit  ( 2021-05-31 10:55:39 -0500 )edit

Hello Ranjit, once again thank you so much for helping me. According to the link you have provided I have got code up to this, kindly help me out what mistakes I am doing 1. (First you have to subscribe to camera_info, pixel_coordinate, and point cloud topic.) I have subscribe the point cloud and camera_info topic but didn't get the pixel coordinate

def subscribe(self):
            self.pc_sub = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.pc_callback)
            self.info_sub = rospy.Subscriber("/camera/rgb/camera_info", CameraInfo, self.info_callback)
            self.camera_image = rospy.Subscriber("") #How to subscribe pixel coordinate(what is the topic name)

   def info_callback(self, data):
            self.camera_model = image_geometry.PinholeCameraModel()
            self.camera_model.fromCameraInfo(data)
            self.info_sub.unregister()
arifzaman gravatar image arifzaman  ( 2021-06-02 17:03:00 -0500 )edit

2.(Call get_depth function for obtaining the distance of that pixel into the point cloud.)

def get_depth(self, x, y):
    gen = pc2.read_points(self.pc, field_names='z', skip_nans=False, uvs=[(x, y)])
    return next(gen)

3.Call camera_model.projectPixelTo3dRay((U,V)) 4.normalize Z and then after from that vector multiply get_depth function result(distance) with the vector X and Y value.

I didn't completely understand 3 and 4 point.Is I am doing right?

def calculate_3d_point(self, pixel):
        lookup = self.depth.load()
        depth = lookup[pixel[0], pixel[1]]  # lookup pixel in depth image
        ray = self.model.projectPixelTo3dRay(tuple(pixel))
        ray_z = [el / ray[2] for el in ray] 
        pt = [el * depth for el in ray_z] 
        point = PointStamped()
        point.header.frame_id = self.frame
        point.point.x = pt[0]
        point.point.y = pt[1]
        point.point.z = pt[2]
        return point
arifzaman gravatar image arifzaman  ( 2021-06-02 17:17:22 -0500 )edit

Hello @arifzaman,

(First you have to subscribe to camera_info, pixel_coordinate, and point cloud topic.) I have subscribe the point cloud and camera_info topic but didn't get the pixel coordinate

Over here pixel coordinate means your U, V location of your detected object. Are you using any algorithm from OpenCV, ML, or Dl to identify objects? If yes, then you have to select the appropriate point(U, V) for a particular image/Video.

self.camera_image = rospy.Subscriber("") #How to subscribe pixel coordinate(what is the topic name)

In my case, my object detection algorithm was running in different nodes, and through ROS message, I was publishing that message and I was substrating over here to process it into 3D cooridnate.

Again to note, your flow may be different so you have to set this according to the flow of your code. If you are using a detection algorithm node on ...(more)

Ranjit gravatar image Ranjit  ( 2021-06-03 10:56:41 -0500 )edit

I didn't completely understand 3 and 4 point.Is I am doing right?

I think you are going right over here, but importing thing to note first you have to achieve point(U, V) value from your video stream of the particular detected object. then you can convert it to a 3D coordinate. In simple words, You need to have a 2D coordinate (cartesian coordinates) to obtain a 3D coordinate and processed it to point cloud form with PinholeCameraModel.

If you think that this answer is right, then press on tick button so in the future this answer can help others.

Thanks!

Ranjit gravatar image Ranjit  ( 2021-06-03 11:05:15 -0500 )edit
1

Thanks you so much Ranjit, I have answered the ques. In my application I have to calculate the volume of soil from pointcloud. kindly check it if anything still wrong? Thanks again.

arifzaman gravatar image arifzaman  ( 2021-06-10 14:50:07 -0500 )edit
0

answered 2021-06-10 14:48:36 -0500

arifzaman gravatar image

Thanks Ranjit once again, I have worked on my code and got the x,y,z coordinates and then finally got the volume of my pointcloud. But I am not sure that what is the unit of output of my x,y,z coordinates?is it in millimeters or meters? Here is my code:

#!/usr/bin/env python
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2
from scipy.integrate import quad
import rospy
import time
import numpy as np


des_vol = input('What is the desired volume:    ')


xpoint = []
ypoint = []
zpoint = []

def callback_pointcloud(data):
    global volume
    global x
    global y
    global z
    assert isinstance(data, PointCloud2)
    gen = point_cloud2.read_points(data, field_names=("x", "y", "z"), skip_nans=True) 

    for p in gen:
      xpoint.append(p[0])
      ypoint.append(p[1])
      zpoint.append(p[2]) 
    x = max(xpoint) - min(xpoint)
    y = max(ypoint) - min(ypoint)
    cross_section = x*y

    def f(z):
        return cross_section

    i = quad(f,min(zpoint),max(zpoint))
    volume = i

def main():
    rospy.init_node('pcl_listener', anonymous=True)
    rospy.Subscriber('/camera/depth/color/points', PointCloud2, callback_pointcloud)
    rospy.sleep(10)
    #rospy.spin()
    print ("value of x : ", x)
    print ("value of y : ", y)
    #print ("value of z : ", z)
    print ("Total initial volume is : ", volume)
    remaining_volume = volume - float(des_vol)
    quotient = remaining_volume/volume
    percentage = quotient * 100
    print("Remaining Volume needs to be completed : ",remaining_volume)
    print("Remaining Percentage needs to be completed : ",percentage)

if __name__ == "__main__":
    main()
edit flag offensive delete link more

Comments

But I am not sure that what is the unit of output of my x,y,z coordinates?is it in millimeters or meters? Here is my code

https://www.ros.org/reps/rep-0103.html

It will me meters.

For x,y,z you will get something like this value:

x,y,z

-0.043247390386682384, 0.06332617474953332, 0.01003939218142547

Ranjit gravatar image Ranjit  ( 2021-06-11 08:15:23 -0500 )edit

No, the values which i am getting are like:

value of x :  85.66250610351562
value of y :  48.047407150268555
value of z :  65.14400362968445

That's why I am wondering that if it is in meters then these values i don't think right. If i am doing anything wrong?

arifzaman gravatar image arifzaman  ( 2021-06-11 13:12:48 -0500 )edit

I think you forgot to do image_geometry.PinholeCameraModel() and vector of that coordinate.

Ranjit gravatar image Ranjit  ( 2021-06-14 12:00:04 -0500 )edit
1

Thanks Ranjit, Got your Point.

arifzaman gravatar image arifzaman  ( 2021-06-17 12:41:15 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-05-26 15:55:49 -0500

Seen: 56 times

Last updated: Jun 10