Closed salokin1997 closed 3 years ago
Your topic names are different than what the script expects.
The cleaner solution would be to just remap the following topics in play_rosbag.launch
(see this):
<node
pkg="rosbag"
type="play"
name="player"
output="screen"
args="--clock --loop $(find lidar_camera_calibration)/bagfiles/$(arg bagfile)">
<remap from="/os1_cloud_node/points" to="/sensors/velodyne_points"/>
<remap from="/zed2/zed_node/left/camera_info" to="/sensors/camera/camera_info"/>
<remap from="/zed2/zed_node/left/image_rect_color" to="/sensors/camera/image_color"/>
</node>
Also, all these are parameters and args that you can always set. You can also manually modify these topic names in the scripts and launch files.
camera
to /zed2/zed_node/left
here, here, and here.NOTE: You seem to be using the rectified image topic as input to the calibration script. The launch file internally rectifies the image again using the camera_info
topic. Make sure you disable this node in that case. Also update this flag accordingly. You should basically be picking points from the rectified image in the GUI, so make sure the GUI display the rectified image properly.
I actually think the remap from
and to
need to swapped above, let me know which worked for you.
I tried both and it still didn't work
can you try modifying the topic names manually as I suggested above?
I tried and i get the following result:
Press [ENTER] to pause and pick points
[INFO] [1601449532.541712]: Current PID: [9474]
[INFO] [1601449532.542513]: Projection mode: False
[INFO] [1601449532.543162]: CameraInfo topic: /zed2/zed_node/left/camera_info
[INFO] [1601449532.543808]: Image topic: /zed2/zed_node/left/image_rect_color
[INFO] [1601449532.544443]: PointCloud2 topic: /os1_cloud_node/points
[INFO] [1601449532.545097]: Output topic: None
Is there anything else I might have missed?
I am also facing the same issue mentioned by @salokin1997. @heethesh any help on this would be appreciable. Also, any guidance on how to go about debugging would also help.
What are the acquisition/frame rates for the LiDAR and camera? You may want to try increasing the slop time here in message synchronization. Looks like the callback
function is not being called due to message sync failure, @patilameya825 you can try printing something inside the function to see if it even gets called.
i have to change the slop higher than the actual unix time
. After this i get the GUI and the following message:
Press [ENTER] to pause and pick points
[INFO] [1601543597.304148]: Current PID: [12138]
[INFO] [1601543597.304941]: Projection mode: False
[INFO] [1601543597.305596]: CameraInfo topic: /zed2/zed_node/left/camera_info
[INFO] [1601543597.306235]: Image topic: /zed2/zed_node/left/image_rect_color
[INFO] [1601543597.306869]: PointCloud2 topic: /os1_cloud_node/points
[INFO] [1601543597.307496]: Output topic: None
[INFO] [1601543597.391614]: Setting up camera model
[INFO] [1601543597.392587]: Setting up static transform listener
[INFO] [1601543598.096853]: 2D Picker PID: [12199]
[INFO] [1601543598.099598]: 3D Picker PID: [12200]
WARNING: QApplication was not created in the main() thread.
(40, 512, 9)
[INFO] [1601543598.139481]: PCL points available: 40
WARNING: QApplication was not created in the main() thread.
Process Process-2:
Traceback (most recent call last):
File "/home/labor/.local/lib/python3.8/site-packages/matplotlib/axes/_axes.py", line 4291, in _parse_scatter_color_args
colors = mcolors.to_rgba_array(c)
File "/home/labor/.local/lib/python3.8/site-packages/matplotlib/colors.py", line 341, in to_rgba_array
return np.array([to_rgba(cc, alpha) for cc in c])
File "/home/labor/.local/lib/python3.8/site-packages/matplotlib/colors.py", line 341, in <listcomp>
return np.array([to_rgba(cc, alpha) for cc in c])
File "/home/labor/.local/lib/python3.8/site-packages/matplotlib/colors.py", line 189, in to_rgba
rgba = _to_rgba_no_colorcycle(c, alpha)
File "/home/labor/.local/lib/python3.8/site-packages/matplotlib/colors.py", line 265, in _to_rgba_no_colorcycle
raise ValueError("RGBA sequence should have length 3 or 4")
ValueError: RGBA sequence should have length 3 or 4
The above exception was the direct cause of the following exception:
Traceback (most recent call last):
File "/usr/lib/python3.8/multiprocessing/process.py", line 315, in _bootstrap
self.run()
File "/usr/lib/python3.8/multiprocessing/process.py", line 108, in run
self._target(*self._args, **self._kwargs)
File "/home/labor/Schreibtisch/project/Labor/Code_heethesh1/src/lidar_camera_calibration/scripts/calibrate_camera_lidar.py", line 254, in extract_points_3D
ax.scatter(points[:, 0], points[:, 1], points[:, 2], c=colors, s=2, picker=5)
File "/home/labor/.local/lib/python3.8/site-packages/mpl_toolkits/mplot3d/axes3d.py", line 2312, in scatter
patches = super().scatter(xs, ys, s=s, c=c, *args, **kwargs)
File "/home/labor/.local/lib/python3.8/site-packages/matplotlib/__init__.py", line 1438, in inner
return func(ax, *map(sanitize_sequence, args), **kwargs)
File "/home/labor/.local/lib/python3.8/site-packages/matplotlib/cbook/deprecation.py", line 411, in wrapper
return func(*inner_args, **inner_kwargs)
File "/home/labor/.local/lib/python3.8/site-packages/matplotlib/axes/_axes.py", line 4451, in scatter
self._parse_scatter_color_args(
File "/home/labor/.local/lib/python3.8/site-packages/matplotlib/axes/_axes.py", line 4297, in _parse_scatter_color_args
raise invalid_shape_exception(c.size, xsize) from err
ValueError: 'c' argument has 1440 elements, which is inconsistent with 'x' and 'y' with size 360.
^CEuler angles (RPY): (-0.9825272479088979, -0.7621367412272636, -0.5641881702214682)
Rotation Matrix: [[ 0.61125778 0.78211675 0.1210674 ]
[-0.38680351 0.16177149 0.90786179]
[ 0.69046866 -0.60176688 0.40140959]]
Translation Offsets: [[-0.21234719 0.83647285 -2.64976181]]
Press [ENTER] to pause and pick points
i have to change the slop higher than the actual unix time.
What did you have to set it to?
Looks like a shape error to me, your points size is 360 and the colors are 1440, you might need to reshape the colors
array by doing something like colors = colors.reshape(-1, 4)[:, :3]
. Just remove c=colors
when you display if you face other issues.
Also check this comment if you want to change the FOV of LiDAR displayed when picking points.
What did you have to set it to?
I have to set it to 1.7e9
colors = colors.reshape(-1, 4)[:, :3]
This works fine! But the GUI is still black
See the documentation here, you should not set the slop so high. It should be < 0.5 seconds for good calibration. If you set it so high your LiDAR and cameras will be totally out of sync.
However, if the slop is less, the callback will not be carried out.
This means something is probably wrong with the recorded bag file and/or the topic timestamps. Were the images and point cloud recorded together? Your rosbag info looks correct, not sure what's going on here..
Were the images and point cloud recorded together
Yes, they did...
What are the acquisition/frame rates for the LiDAR and camera? You may want to try increasing the slop time here in message synchronization. Looks like the
callback
function is not being called due to message sync failure, @patilameya825 you can try printing something inside the function to see if it even gets called.
The acquisition rate for lidar is ~20hz and frame rate for a camera is around ~24hz. Also, increasing the slop rate higher than 1.7e9 (as mentioned by @salokin1997) does not help. You are right. The callback function does not get called.
@patilameya825 Please don't increase the slop rate so high, in your case 0.1s should be good enough for synchronization. Are you sure that the topics printed when the applications starts are the same as in your bag file? Another way to debug would be removing all topics in the message sync and adding them one-by-one to see when the callback starts to fail.
@patilameya825 Are your timestamps of the messages correct? I fixed the problem. The Lidar which I used used his intern oscillator. Everytime I started the lidar the timestamp started with zero. I have had to change the timemode to use the pc-masterclock and the unix-time.
I fixed the time-sync-problem. But i've a second problem, too. If i press enter, two windows would open, but them are black. I got following text in the console:
Press [ENTER] to pause and pick points
[INFO] [1602055187.835358]: Current PID: [6739]
[INFO] [1602055187.836199]: Projection mode: False
[INFO] [1602055187.836834]: CameraInfo topic: /zed2/zed_node/left/camera_info
[INFO] [1602055187.837420]: Image topic: /zed2/zed_node/left/image_rect_color
[INFO] [1602055187.838006]: PointCloud2 topic: /os1_cloud_node/points
[INFO] [1602055187.838753]: Output topic: None
[INFO] [1602055187.888078]: Setting up camera model
[INFO] [1602055187.889377]: Setting up static transform listener
[INFO] [1602055193.462744]: 2D Picker PID: [6801]
[INFO] [1602055193.465564]: 3D Picker PID: [6802]
WARNING: QApplication was not created in the main() thread.
(45, 512, 9)
[INFO] [1602055193.509351]: PCL points available: 45
WARNING: QApplication was not created in the main() thread.
Have anyone an idea?
Can you debug by printing the shapes of the point cloud and image numpy arrays before visualizing in the GUI?
Can you debug by printing the shapes of the point cloud and image numpy arrays before visualizing in the GUI?
Yes, I can. And if i used de openCV tools, i can show the image.
cv2.imshow('image',disp)
cv2.waitKey(0)
cv2.destroyAllWindows()
LookupException: "world" passed to lookupTransform argument target_frame does not exist.
I made following changes:
in calibrate_camera_lidar.py
def project_point_cloud(velodyne, img_msg, image_pub):
global TF_BUFFER
print ("Project Point Cloud", "*"*50)
# Read image using CV bridge
try:
img = CV_BRIDGE.imgmsg_to_cv2(img_msg, 'bgr8')
print (img.shape, "*"*10)
except CvBridgeError as e:
print (e)
rospy.logerr(e)
return
# Transform the point cloud
#try:
transform = TF_BUFFER.lookup_transform('world', 'os1_sensor', rospy.Time())
print ("/"*100, transform)
velodyne = do_transform_cloud(velodyne, transform)
print ("#"*10, transform, velodyne)
#except tf2_ros.LookupException:
# print ("tf2_ros")
# pass
in display_camera_lidar_calibration.launch
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="camera" default="/sensors/camera" />
<!-- Play rosbag record -->
<!--include file="$(find lidar_camera_calibration)/launch/play_rosbag.launch">
<arg name="bagfile" value="2016-11-22-14-32-13_test_updated.bag" />
</include -->
<!-- Nodelet manager for this pipeline -->
<node
pkg="nodelet"
type="nodelet"
args="manager"
name="lidar_camera_manager"
output="screen" />
<node
pkg="image_proc"
type="image_proc"
name="image_proc_node1" />
<!-- Run image_proc/rectify nodelet -->
<node
pkg="nodelet"
type="nodelet"
name="rectify_color"
args="load image_proc/rectify lidar_camera_manager --no-bond" >
<!-- Remap input topics -->
<remap from="image_mono" to="$(arg camera)/image_color" />
<remap from="camera_info" to="$(arg camera)/camera_info" />
<!-- Remap output topics -->
<remap from="image_rect" to="$(arg camera)/image_rect_color" />
</node>
<!-- Wire static transform from the world to velodyne frame -->
<node
pkg="tf2_ros"
type="static_transform_publisher"
name="world_velodyne_tf"
output="screen"
args="-14.44986149 -8.93718396 -71.49091976 -2.3713286213358544 0.9228590101120796 2.200537576589965 world os1_sensor" />
<!-- Setup params for Camera-LiDAR calibration script -->
<param name="camera_info_topic" type="str" value="/usb_cam/camera_info" />
<param name="image_color_topic" type="str" value="/usb_cam/image_raw" />
<param name="velodyne_points_topic" type="str" value="/os1_cloud_node/points" />
<param name="camera_lidar_topic" type="str" value="/camera_lidar" />
<param name="project_mode" type="bool" value="true" />
<!-- Run Camera-LiDAR projection script -->
<node
pkg="lidar_camera_calibration"
type="calibrate_camera_lidar.py"
name="calibrate_camera_lidar"
output="screen" />
<!-- Run image view to display the projected points image -->
<node
name="camera_lidar_projection"
pkg="image_view"
type="image_view"
respawn="false"
output="screen">
<!-- Remap input topics -->
<remap from="image" to="/camera_lidar" />
</node>
in this, I inputted the transform values of XYZ and YPR from the output generated by
rosrun lidar_camera_calibration calibrate_camera_lidar.py --calibrate
Output
aslan_user@kl-pc:~$ rosrun lidar_camera_calibration calibrate_camera_lidar.py --calibrate This #################################################################################################### Starting keyboard handle_keyboard Started keyboard Press [ENTER] to pause and pick points [INFO] [1602153993.043990]: Current PID: [15678] [INFO] [1602153993.044721]: Projection mode: False [INFO] [1602153993.045290]: CameraInfo topic: /usb_cam/camera_info [INFO] [1602153993.045825]: Image topic: /usb_cam/image_raw [INFO] [1602153993.046377]: PointCloud2 topic: /os1_cloud_node/points [INFO] [1602153993.046900]: Output topic: None Synced Topics [INFO] [1602153993.148396]: Setting up camera model [INFO] [1602153993.148829]: Setting up static transform listener
extract_points_2D [INFO] [1602153997.116139]: 2D Picker PID: [15721] extract_points_3D [INFO] [1602153997.118500]: 3D Picker PID: [15722] (131072, 3) ** (797, 3) [INFO] [1602153997.241704]: PCL points available: 797 [] about to save save_data save_data save_data In save_data Calibrate (8, 2) (8, 3) Euler angles (RPY): (2.200537576589965, 0.9228590101120796, -2.3713286213358544) Rotation Matrix: [[-0.43317905 -0.87258392 -0.22572817] [-0.42026197 -0.0260073 0.90703004] [-0.79733041 0.48777138 -0.35544803]] Translation Offsets: [[-14.44986149 -8.93718396 -71.49091976]] Starting keyboard handle_keyboard Press [ENTER] to pause and pick points Started keyboard
Also, if use following code:
def project_point_cloud(velodyne, img_msg, image_pub):
global TF_BUFFER
print ("Project Point Cloud", "*"*50)
# Read image using CV bridge
try:
img = CV_BRIDGE.imgmsg_to_cv2(img_msg, 'bgr8')
print (img.shape, "*"*10)
except CvBridgeError as e:
print (e)
rospy.logerr(e)
return
# Transform the point cloud
try:
transform = TF_BUFFER.lookup_transform('world', 'os1_sensor', rospy.Time())
print ("/"*100, transform)
velodyne = do_transform_cloud(velodyne, transform)
print ("#"*10, transform, velodyne)
except tf2_ros.LookupException:
print ("tf2_ros")
pass
Command
roslaunch lidar_camera_calibration display_camera_lidar_calibration.launch
Output
aslan_user@kl-pc:~$ $ rlaunch lidar_camera_calibration display_camera_lidar_calibration.launch ... logging to /home/aslan_user/.ros/log/2a3959e0-093a-11eb-a364-7cd30a81cfd7/roslaunch-kl-pc-15757.log Checking log directory for disk usage. This may take awhile. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://kl-pc:43783/
PARAMETERS
NODES / calibrate_camera_lidar (lidar_camera_calibration/calibrate_camera_lidar.py) camera_lidar_projection (image_view/image_view) image_proc_node1 (image_proc/image_proc) lidar_camera_manager (nodelet/nodelet) rectify_color (nodelet/nodelet) world_velodyne_tf (tf2_ros/static_transform_publisher)
ROS_MASTER_URI=http://localhost:11311
process[lidar_camera_manager-1]: started with pid [15774] [ INFO] [1602154088.437283672]: Initializing nodelet with 12 worker threads. process[image_proc_node1-2]: started with pid [15775] process[rectify_color-3]: started with pid [15803] process[world_velodyne_tf-4]: started with pid [16025] [ INFO] [1602154089.712644229]: Spinning until killed publishing world to os1_sensor process[calibrate_camera_lidar-5]: started with pid [16091] process[camera_lidar_projection-6]: started with pid [16102] [ INFO] [1602154090.588767822]: Using transport "raw" I am in Project [INFO] [1602154090.894145]: Current PID: [16091] [INFO] [1602154090.894320]: Projection mode: True [INFO] [1602154090.894488]: CameraInfo topic: /usb_cam/camera_info [INFO] [1602154090.894585]: Image topic: /usb_cam/image_raw [INFO] [1602154090.894743]: PointCloud2 topic: /os1_cloud_node/points [INFO] [1602154090.894866]: Output topic: /camera_lidar Synced Topics [INFO] [1602154091.017092]: Setting up camera model [INFO] [1602154091.018219]: Setting up static transform listener Project Point Cloud ** (480, 640, 3) ** tf2_ros ** [] [] (0,) (480, 640, 3) [ERROR] [1602154091.176096]: bad callback: <bound method Subscriber.callback of <message_filters.Subscriber object at 0x7ff0ab3c2d50>> Traceback (most recent call last): File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 750, in _invoke_callback cb(msg) File "/opt/ros/kinetic/lib/python2.7/dist-packages/message_filters/init.py", line 75, in callback self.signalMessage(msg) File "/opt/ros/kinetic/lib/python2.7/dist-packages/message_filters/init.py", line 57, in signalMessage cb((msg + args)) File "/opt/ros/kinetic/lib/python2.7/dist-packages/message_filters/init.py", line 291, in add self.signalMessage(msgs) File "/opt/ros/kinetic/lib/python2.7/dist-packages/message_filters/init.py", line 57, in signalMessage cb(*(msg + args)) File "/home/aslan_user/catkin_ws/src/lidar_camera_calibration/scripts/calibrate_camera_lidar.py", line 472, in callback project_point_cloud(velodyne, image, image_pub) File "/home/aslan_user/catkin_ws/src/lidar_camera_calibration/scripts/calibrate_camera_lidar.py", line 426, in project_point_cloud (points2D[:, 0] < img.shape[1]) & IndexError: too many indices for array
I am not able to understand where exactly I am making the mistake.
@patilameya825 Are your timestamps of the messages correct? I fixed the problem. The Lidar which I used used his intern oscillator. Everytime I started the lidar the timestamp started with zero. I have had to change the timemode to use the pc-masterclock and the unix-time.
In the os1.launch file, which option do we need to select for using pc time?
<arg name="timestamp_mode" default="" doc="method used to timestamp measurements: TIME_FROM_INTERNAL_OSC, TIME_FROM_SYNC_PULSE_IN, TIME_FROM_PTP_1588"/>
You need the mode:TIME_FROM_PTP_1588
Read chapter 3.3 of the SW-user-Guide. You have to do the following terminal-commands to change the timemode:
$ nc "lidar-IP" 7501
set_config_param timestamp_mode TIME_FROM_PTP_1588
reinitialize
check the timemode with get_time_info
@patilameya825
in this, I inputted the transform values of XYZ and YPR from the output generated by
The static transform is not set up when you rosrun
the node, the transform broadcaster is only started in the launch file for display, that's the reason why you get the missing frame error? Try manually running the transform broadcaster node from another terminal if you want it for calibration as well and debug by looking at the ROS TF tree.
I am not able to understand where exactly I am making the mistake.
Seems to me that your points2D
array is empty. Check calibration_data/lidar_camera_calibration/img_corners.npy
file by loading in numpy. Were your picked points saved properly?
The program finally works. However, I have a problem with my calibration results. I moved the chess board exactly as shown in the video. But I got bad results.
@salokin1997 How many total pair of correspondences did you select? Try running cv2.solvePnPRefineLM
on the initial estimate with all the inlier correspondences.
How many total pair of correspondences did you select?
I selected 4 pairs (each corner)
Did you update this line with the estimated transform?
I did
The .npy files are only updated. Are there no issues with older content?
You need more than 4 pairs, pick around 20-30. If you picked the wrong pairs, you should delete the npy file to reset correspondences.
It finally works. I have picked over 40 points and got really good results. Thank you very much!
@salokin1997 That's great, you may use cv2.solvePnPRefineLM (see this comment) to further refine the results. If your current OpenCV version (>4.1.1) already has the method, then the refinement is already applied, if not, you may do it manually.
@patilameya825 Are your timestamps of the messages correct? I fixed the problem. The Lidar which I used used his intern oscillator. Everytime I started the lidar the timestamp started with zero. I have had to change the timemode to use the pc-masterclock and the unix-time.
Did you set the timestamp of lidar into the ros time, is it? I also set the lidar timestamp into ros time but I can show image but the GUI for point cloud was not occurred, do you know the problem? Thx
I fixed the time-sync-problem. But i've a second problem, too. If i press enter, two windows would open, but them are black. I got following text in the console:
Press [ENTER] to pause and pick points [INFO] [1602055187.835358]: Current PID: [6739] [INFO] [1602055187.836199]: Projection mode: False [INFO] [1602055187.836834]: CameraInfo topic: /zed2/zed_node/left/camera_info [INFO] [1602055187.837420]: Image topic: /zed2/zed_node/left/image_rect_color [INFO] [1602055187.838006]: PointCloud2 topic: /os1_cloud_node/points [INFO] [1602055187.838753]: Output topic: None [INFO] [1602055187.888078]: Setting up camera model [INFO] [1602055187.889377]: Setting up static transform listener [INFO] [1602055193.462744]: 2D Picker PID: [6801] [INFO] [1602055193.465564]: 3D Picker PID: [6802] WARNING: QApplication was not created in the main() thread. (45, 512, 9) [INFO] [1602055193.509351]: PCL points available: 45 WARNING: QApplication was not created in the main() thread.
Have anyone an idea?
How did you solve the problem of ouster lidar? I used the same lidar but I could not open GUI of lidar, thx
Thanks for the work. the reshape solve one of the issue. However i still have the problem of black screen in lidar window. I already set the slop to be 0.5 My lidar is ouster lidar running at 10hz and camera is also 10 hz. The time difference between them are marginally small.
Did you update this line with the estimated transform? You mean,after getting the extrinsic parameter and then manually input the result here.... or
You mean,after getting the extrinsic parameter and then manually input the result here.... or
Yes that's right
@patilameya825 Are your timestamps of the messages correct? I fixed the problem. The Lidar which I used used his intern oscillator. Everytime I started the lidar the timestamp started with zero. I have had to change the timemode to use the pc-masterclock and the unix-time.
Hello @patilameya825 , I seem to have the same problem. How did you change the timemode to use the pc masterclock and the unix-time ? Setting slop to 1.7e9 solves the problem of GUI starting up. Kindly help
Hello,
I use an Ouster os1 lidar and a ZED2 stereocamera from Stereolabs. I created the following bagfile:
When I used "rosrun lidar_camera_calibration calibrate_camera_lidar.py --calibrate", the results were:
I get no response and the GUI is still closed. I'm hoping that you can help me. Thanks!