{"id":45266,"date":"2019-01-10T16:10:00","date_gmt":"2019-01-10T23:10:00","guid":{"rendered":"https:\/\/www.realsenseai.com\/uncategorized-cn\/how-to-multiple-camera-setup-with-ros-2\/"},"modified":"2019-01-10T16:10:00","modified_gmt":"2019-01-10T23:10:00","slug":"how-to-multiple-camera-setup-with-ros-2","status":"publish","type":"post","link":"https:\/\/www.realsenseai.com\/cn\/news-insights-cn\/whitepapers-cn\/how-to-multiple-camera-setup-with-ros-2\/","title":{"rendered":"How-to: Multiple camera setup with ROS"},"content":{"rendered":"\n
\n

RealSense\u2122 D400 series depth cameras use stereo-based algorithms to calculate depth. One key advantage of stereo depth systems is the ability to use as many cameras as you want to within a specific scene. In this post, we are going to cover creating a unified point cloud with multiple cameras using ROS.<\/p>\n\n\n\n

For the initial demonstration, we set up two RealSense\u2122 D435 cameras looking at the same area from two different points of view.<\/p>\n<\/div>\n\n\n\n

\n
\"Agility
Agility Robotics is the world\u2019s leading manufacturer of mobile manipulation robots.<\/figcaption><\/figure>\n<\/div>\n\n\n\n
\n

The cameras themselves have no data regarding their relative position. In Step 3, we\u2019ll use a 3rd party program to set this up.<\/p>\n\n\n\n

Step 0: Ensure you have your environment set up.<\/h3>\n\n\n\n

Install the RealSense SDK 2.0 (librealsense<\/em>) for Linux* following the instructions here<\/a>.
Install the ROS (not ROS 2) wrapper for librealsense<\/em> from here<\/a>. It is recommended to follow this set of instructions<\/a> for the installation.<\/p>\n\n\n\n

Step 1: Obtaining the camera serial numbers.<\/h3>\n\n\n\n

If you already know each camera\u2019s serial number you can skip this step.<\/p>\n\n\n\n

Otherwise,<\/p>\n\n\n\n

1. Open terminal and change directory to catkin_ws<\/em><\/p>\n\n\n\n

2. Make sure only cam_1<\/em> is connected and start the realsense2_camera<\/em> wrapper:<\/p>\n\n\n\n

roslaunch realsense2_camera rs_camera.launch<\/pre>\n\n\n\n
Note the serial number it finds in the following log line:<\/p>\n\n\n\n
[ INFO] [1538983051.813336379]: setupDevice...\n \n[ INFO] [1538983051.813399792]: JSON file is not provided\n \n[ INFO] [1538983051.813427820]: ROS Node Namespace: camera\n \n[ INFO] [1538983051.813442869]: Device Name: RealSense D435\n \n[ INFO] [1538983051.813605893]: Device Serial No: 728312070349\n \n[ INFO] [1538983051.813622583]: Device FW version: 05.10.03.00<\/pre>\n\n\n\n
The serial number in this case is 728312070349<\/strong>.
Record this somewhere and terminate the node using CTRL+C.
Repeat the step with now only cam_2<\/em> connected.<\/p>\n\n\n\n
Step 2: Start both cameras.<\/h3>\n\n\n\n
Open 2 terminal windows (and change directory to catkin_ws<\/em> directory).<\/p>\n\n\n\n
In terminal 1 enter the following (don\u2019t forget to replace 728312070349<\/strong> with the cam_1<\/em> serial number you recorded in Step 1):<\/p>\n\n\n\n
roslaunch realsense2_camera rs_camera.launch camera:=cam_1 serial_no:=728312070349 filters:=spatial,temporal,pointcloud_<\/pre>\n\n\n\n
In terminal 2 enter the following (again, fill in the correct serial number for cam_2<\/em>):<\/p>\n\n\n\n
roslaunch realsense2_camera rs_camera.launch camera:=cam_2 serial_no:=<serial_number_2> filters:=spatial,temporal,pointcloud_<\/pre>\n\n\n\n
We now have two cameras running with their own point clouds.<\/p>\n\n\n\n
Step 3: Publish spatial connection between cameras.<\/h3>\n\n\n\n
We estimate the translation of cam_2<\/em> from cam_1<\/em> at 70(cm) on X-axis and 60(cm) on Y-axis. We also estimate the yaw angle of cam_2<\/em> relative to cam_1<\/em> as 90(degrees) clockwise \u2013 this based on our knowledge of the setup in the image above. To simplify things, the coordinate system of cam_1<\/em> will serves as the coordinate system for the whole scene. These are the initial parameters we set for the transformation between the 2 cameras.<\/p>\n\n\n\n
The following script calculates the transformation between the 2 cameras from the given parameters and publish the frame transformation.<\/p>\n\n\n\n
Open a third terminal window \u2013 terminal 3 \u2013 and enter the following:<\/p>\n\n\n\n
cd catkin_ws\npython src\/realsense\/realsense2_camera\/scripts\/set_cams_transforms.py cam_1_link cam_2_link 0.7 0.6 0 -90 0 0<\/pre>\n\n\n\n
Step 4: Visualizing the point clouds and fine-tune the camera calibration.<\/h3>\n\n\n\n
Open a fourth terminal \u2013 terminal 4 \u2013 and run RViz. RViz is a 3D visualizer for displaying sensor data and state information from ROS:<\/p>\n\n\n\n
rviz<\/pre>\n\n\n\n
In the RViz window, do the following:<\/p>\n\n\n\n
1. Set \u201cFixed Frame\u201d to \u201ccam_1_link\u201d\n2. Add -> By topic -> \/cam_1\/depth\/color\/points\/PointCloud2\n3. Add -> By topic -> \/cam_2\/depth\/color\/points\/PointCloud2<\/pre>\n\n\n\n
Now both point clouds are shown together. If you looked closely it\u2019s easy to see that the clouds are not exactly in position. Some features are duplicated. Now it\u2019s time for fine calibration.<\/p>\n\n\n\n
Switch to terminal 3, where set_cams_transforms.py<\/em> is still running. Use it to fine-calibrate cam_2<\/em> relative to cam_1<\/em>. The instructions are printed by the program:<\/p>\n\n\n\n
Using default file \/home\/doronhi\/catkin_ws\/src\/realsense\/realsense2_camera\/scripts\/_set_cams_info_file.txt\n \nUse given initial values.\n \nPress the following keys to change mode: x, y, z, (a)zimuth, (p)itch, (r)oll\n \nFor each mode, press 6 to increase by step and 4 to decrease\n \nPress + to multiply step by 2 or - to divide\n \nPress Q to quit<\/pre>\n\n\n\n
Below that a constantly updating line is printed, showing the current mode, value and step size after every key stroke.<\/p>\n\n\n\n
Notice, that the program prints the path of the current configuration file. It saves its last configuration automatically, all the time, to be used on the next run. If you have been running the program for a while, and want to back up the configuration file, just copy it aside. Read the rviz<\/em> manual, by running without any parameters to learn more about changing the configuration filename.<\/p>\n\n\n\n
There is no question that it would be much better to find the precise matrix between the cameras automatically. In the meanwhile, after a lot of fiddling around, I was relatively satisfied with the following numbers: Parameter Value x 0.75 y 0.575 z 0 azimuth -91.25 pitch 0.75 roll 0<\/p>\n\n\n\n
The result is a more complete cover of the scene:<\/p>\n<\/div>\n\n\n\n
\n
\"Agility
Agility Robotics is the world\u2019s leading manufacturer of mobile manipulation robots.<\/figcaption><\/figure>\n<\/div>\n\n\n\n
\n
\"Agility
Agility Robotics is the world\u2019s leading manufacturer of mobile manipulation robots.<\/figcaption><\/figure>\n<\/div>\n\n\n\n
\n
Adding more cameras<\/h2>\n\n\n\n
There are a couple of reasons why the number of cameras connected to a single computer is limited: CPU usage, USB bandwidth, power supply and also, the length of the USB cable. These are all covered in length in the Multiple camera configuration white paper<\/a>.
In order to demonstrate the scenario we created the following array:<\/p>\n<\/div>\n\n\n\n
\n
\"Agility<\/figure>\n<\/div>\n\n\n\n
\n
\"Agility
Agility Robotics is the world\u2019s leading manufacturer of mobile manipulation robots.<\/figcaption><\/figure>\n<\/div>\n\n\n\n
\n
Both cam_2<\/em> and cam_3<\/em> are visible in cam_1<\/em>\u2019s point cloud. That way, it\u2019s easier, though not trivial, to use set_cams_transforms.py<\/em> to fix their location in cam_1<\/em>\u2019s coordinate system.<\/p>\n\n\n\n
While using multiple computers we have to specifically set the roscore host. In this demo, the first command in each terminal is:<\/p>\n\n\n\n
export ROS_MASTER_URI=http:\/\/<ros host ip address>:<port><\/pre>\n\n\n\n
In this demo, the roscore<\/em>, cam_1<\/em> and cam_2<\/em> run on perclnx319<\/em> and cam_3 <\/em>is on perclnx217<\/em>.
Also, in this demo all the terminals are being opened on roscore\u2019s computer, perclnx319<\/em>.
The commands to run are as follows \u2013 but remember to replace the serial numbers with your own.<\/p>\n\n\n\n
Terminal 1<\/h4>\n\n\n\n
roscore<\/pre>\n\n\n\n
Terminal 2<\/h4>\n\n\n\n
export ROS_MASTER_URI=http:\/\/perclnx319:11311 roslaunch realsense2_camera rs_camera.launch camera:=cam_1 serial_no:=728312070349 filters:=spatial,temporal,pointcloud<\/pre>\n\n\n\n
Terminal 3<\/h4>\n\n\n\n
export ROS_MASTER_URI=http:\/\/perclnx319:11311 roslaunch realsense2_camera rs_camera.launch camera:=cam_2 serial_no:=713612070696 filters:=spatial,temporal,pointcloud<\/pre>\n\n\n\n
Terminal 4 \u2013 Used to operate the second machine, perclnx217 in our case<\/h4>\n\n\n\n
ssh perclnx217\n \n[Enter Pasword]\n \nexport ROS_MASTER_URI=http:\/\/perclnx319:11311 roslaunch realsense2_camera rs_camera.launch camera:=cam_3 serial_no:=725112060487 filters:=spatial,temporal,pointcloud<\/pre>\n\n\n\n
Publish the spatial connection between cameras.<\/h4>\n\n\n\n
cam_1<\/em> is rotated about 30 degrees compared to the left side of the imaginary square created by the 3 cameras. This camera is our coordinate system.
We estimate the translation of cam_2<\/em> from cam_1<\/em> at 90(cm) on X-axis and 30(cm) on Y-axis. We also estimate the yaw angle of cam_2<\/em> at 120(degrees) clockwise, compared to cam_1<\/em>. These are the initial parameters we set for the transformation between the 2 cameras.
As for cam_3<\/em>, we estimate the translation on X-axis at 160(cm) and on Y-axis at -20(cm). We estimate the yaw at 170(degrees) counter-clockwise.
The following script calculates the transformation from the given parameters and publish the frame transform.<\/p>\n\n\n\n
Terminal 5<\/h4>\n\n\n\n
cd catkin_ws export ROS_MASTER_URI=http:\/\/perclnx319:11311 python src\/realsense\/realsense2_camera\/scripts\/set_cams_transforms.py cam_1_link cam_2_link 0.9 0.3 0 -120 0 0 --file \u2018_cams_set_info_1_to_2\u2019<\/pre>\n\n\n\n
Press Q to Quit.<\/p>\n\n\n\n
Now set transformation for cam_3<\/em>:<\/p>\n\n\n\n
python src\/realsense\/realsense2_camera\/scripts\/set_cams_transforms.py cam_1_link cam_3_link 1.6 -0.2 0 170 0 0 --file \u2018_cams_set_info_1_to_3\u2019<\/pre>\n\n\n\n
Since set_cams_transforms.py<\/em> is not a ROS-package and you can\u2019t run 2 nodes with the same name, we run one copy at a time.<\/p>\n\n\n\n
From now on, the last values for the relative transformations are saved in the specified files. If you want to modify cam_2<\/em> extrinsics, type:<\/p>\n\n\n\n
python src\/realsense\/realsense2_camera\/scripts\/set_cams_transforms.py cam_1_link cam_2_link --file \u2018_cams_set_info_1_to_2\u2019 To modify cam_3: python src\/realsense\/realsense2_camera\/scripts\/set_cams_transforms.py cam_1_link cam_3_link --file \u2018_cams_set_info_1_to_3\u2019<\/pre>\n\n\n\n
Notice<\/strong>: Use 2 different file names for saving the results for two different sets of cameras.<\/p>\n\n\n\n
Visualizing the point clouds and fine-tuning the camera calibration<\/h3>\n\n\n\n
Terminal 6<\/h4>\n\n\n\n
Enter the following command: \u2018 rviz \u2018 In RViz, do the following:<\/p>\n\n\n\n
1. Set \u201cFixed Frame\u201d to \u201ccam_1_link\u201d\n2. Add -> By topic -> \/cam_1\/depth\/color\/points\/PointCloud2\n3. Add -> By topic -> \/cam_2\/depth\/color\/points\/PointCloud2\n4. Add -> By topic -> \/cam_3\/depth\/color\/points\/PointCloud2\n5. Add -> By display type -> TF\n6. In the \u201cDisplays\u201d window under TF > Frames enable only cam_1_link, cam_2_link and cam_3_link. Just for clarity reasons.<\/pre>\n\n\n\n
Now three point clouds are shown together. It\u2019s easy to see that the clouds are not exactly in position. Some features are duplicated. Now it\u2019s time for fine calibration:
Switching to terminal 5, where set_cams_transforms.py<\/em> is still running, we use it to fine calibrate cam_2<\/em> and cam_3<\/em> relative to cam_1<\/em>.
The instruction are printed by the program:<\/p>\n\n\n\n
Using file \/home\/doronhi\/catkin_ws\/_cams_set_info_1_to_3.txt read initial values from it.\n \nPress the following keys to change mode: x, y, z, (a)zimuth, (p)itch, (r)oll.\n \nFor each mode, press 6 to increase by step and 4 to decrease.\n \nPress + to multiply step by 2 or - to divide.\n \nPress Q to quit<\/pre>\n\n\n\n
Without an automatic calibration tool it can be quite challenging to tune the cameras. Here are a couple of tips for calibration:<\/p>\n\n\n\n
    \n
  1. Calibrate one camera at a time.<\/li>\n\n\n\n
  2. Set cam_2 in the field view of cam_1. Keep in RViz display only the point cloud of cam_1 and the axis notation (TF) of cam_2. Now it\u2019s possible to watch the axis notation of cam_2 and change its x,y,z parameters until it\u2019s located right on top of the camera as seen in cam_1\u2019s point cloud. You can even try and put some distinct object close by and directly in line of sight of cam_2 and try to modify its orientation parameters to align the axis image so it will point to the object as seen in cam_1\u2019s point cloud.<\/li>\n\n\n\n
  3. Now, display also the point cloud of cam_2. Put a board to be seen by both cameras. Modify cam_2\u2019s parameters to rotate the board until it is aligned parallel to the board as seen from cam_1, then translate it, rotate again and so on, iteratively, until they are exactly aligned. Use the board to understand what parameter to modify and your face for fine tuning.<\/li>\n\n\n\n
  4. Repeat step 2 and 3 for cam_3 as well.<\/li>\n<\/ol>\n\n\n\n
    Eventually, I was relatively satisfied with the following numbers.<\/p>\n\n\n\n
    For cam_2<\/em><\/strong>: Parameter Value x 0.95 y 0.38125 z -0.015625 azimuth -104.25 pitch 0 roll -0.5<\/p>\n\n\n\n
    for cam_3<\/em><\/strong>: Parameter Value x 1.5875 y -0.703125 z -0.05 azimuth 163 pitch 1 roll 1.25<\/p>\n\n\n\n
    No doubt, a SLAM based solution is better then the manual labor I\u2019ve been through to get this calibration, both in time and results. Nevertheless, the final 3D-model I got looked like that:<\/p>\n\n\n\n
    \n