Stereo

.gitignore

@@ -3,3 +3,4 @@
 *.o
 *.so
 *.sublime*
+/.vscode/

CMakeLists.txt

@@ -4,6 +4,12 @@ project(raspicam_node)
 
 set(CMAKE_CXX_STANDARD 14) # use C++14
 
+# Enable compile optimizations
+# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops")
+set(CMAKE_BUILD_TYPE Release)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=armv8-a+crc -mtune=cortex-a53 -mfpu=crypto-neon-fp-armv8 -mfloat-abi=hard -ftree-vectorize -funsafe-math-optimizations -O3 -pipe")
+
+# add_definitions(-D_DEBUG)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -15,8 +21,9 @@ find_package(catkin REQUIRED COMPONENTS
   dynamic_reconfigure
   diagnostic_updater
   message_generation
+  cv_bridge
 )
-
+find_package(OpenCV 4.2 REQUIRED COMPONENTS imgproc highgui)
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 
@@ -43,7 +50,7 @@ if(${CMAKE_SYSTEM_PROCESSOR} MATCHES arm*)
   IF( (NOT vchostif_LIBS ))
       MESSAGE(FATAL_ERROR "Could not find vchostif library")
   ENDIF()
-
+  
   set(RPI_LIBRARIES ${RPI_LIBRARIES} 
   ${bcmhost_LIBS}
   ${vcos_LIBS} 
@@ -117,22 +124,45 @@ include_directories(include
   /opt/vc/include
   /opt/vc/include/interface/vcos/pthreads
   /opt/vc/include/interface/vmcs_host/linux
+  ${OpenCV_INCLUDE_DIRS}
 )
 
-add_compile_options(-Wall -Wuseless-cast -Wformat-nonliteral)
+add_compile_options(-Wall -Wuseless-cast -Wformat-nonliteral -Wparentheses)
+
+## Declare a cpp library
+add_library(raspicli STATIC
+  src/RaspiCLI.c 
+)
+
+add_library(raspicamcontrol STATIC
+  src/RaspiCamControl.c
+)
+
+## Declare a cpp executables
+add_executable(raspicam_node src/raspicam_node.cpp)
+add_executable(stereo_node src/stereo_node.cpp)
 
-## Declare a cpp executable
-add_executable(raspicam_node src/raspicam_node.cpp src/RaspiCamControl.cpp)
 
 ## Add cmake target dependencies of the executable/library
 add_dependencies(raspicam_node raspicam_node_generate_messages_cpp)
 add_dependencies(raspicam_node raspicam_node_gencfg)
+add_dependencies(stereo_node raspicam_node_generate_messages_cpp)
+add_dependencies(stereo_node raspicam_node_gencfg)
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(raspicam_node
+  raspicamcontrol
+  raspicli
   ${catkin_LIBRARIES}
   ${RPI_LIBRARIES} 
 )
+ target_link_libraries(stereo_node
+   raspicamcontrol
+   raspicli
+   ${catkin_LIBRARIES}
+   ${RPI_LIBRARIES} 
+   ${OpenCV_LIBS}
+)
 
 #############
 ## Install ##
@@ -149,7 +179,7 @@ target_link_libraries(raspicam_node
 # )
 
 ## Mark executables and/or libraries for installation
-install(TARGETS raspicam_node
+install(TARGETS raspicam_node 
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

README.md

@@ -35,7 +35,7 @@ rosdep install --from-paths src --ignore-src --rosdistro=kinetic -y
 
 Compile the code with `catkin_make`.
 
-## Running the Node
+## Running the monocamera Node
 Once you have the node built, you can run it using a launch file.
 
 For a V2.x camera, run `roslaunch raspicam_node camerav2_1280x960.launch`
@@ -44,6 +44,13 @@ For a V1.x camera, run `roslaunch raspicam_node camerav1_1280x720.launch`
 
 Use `rqt_image_view` on a connected computer to view the published image.
 
+## Running the stereocamera Node
+Once you have the node built, you can run it using a launch file.
+
+For a V2.x camera, run `roslaunch raspicam_node stereo_1280x720.launch`
+
+Use `rqt_image_view` on a connected computer to view the published image.
+
 ## Configuring the node with dynamic reconfigure
 The `raspicam_node` supports dynamically reconfiguring the camera parameters.
 
@@ -68,6 +75,7 @@ make sure that the camera cable is properly seated on both ends, and that the ca
 
 ## Node Information
 
+# Raspicam_node
 Topics:
 
 * `~/image/compressed`:
@@ -110,6 +118,50 @@ This parameter is mainly present in order to keep backward compatibility.
 
 * `~enable_imv` (bool): Publish inline motion vectors computed by the GPU
 
+* `~camera_id` (int): The camera id (only supported on Compute Module)
+# Stereo_node
+
+Topics:
+
+* `~/image_left`:
+  Publishes `sensor_msgs/Image` from the camera module. 
+
+* `~/image_right`:
+  Publishes `sensor_msgs/Image` from the camera module. 
+
+* `~/left_camera_info`:
+  Publishes `sensor_msgs/CameraInfo` left camera info for each frame.
+
+* `~/left_camera_info`:
+  Publishes `sensor_msgs/CameraInfo` right camera info for each frame.
+
+Services:
+
+* `~/set_camera_info`: Used to update calibration info for the camera.
+
+Parameters:
+
+* `~private_topics` (bool): By default the topics are private, meaning the node name will be added in front of every topic name.
+If you don't want the topics to be private, you can set this parameter to "true".
+This parameter is mainly present in order to keep backward compatibility.
+
+* `~camera_frame_id` (tf frame): The frame identifier to associate the camera.
+
+* `~left_camera_info_url`: The URL of the camera calibration `.yaml` file for left camera.
+* `~left_camera_info_url`: The URL of the camera calibration `.yaml` file for right camera.
+
+* `~left_camera_name` (string): The name of the left camera, should match with name in camera_info file.
+
+* `~right_camera_name` (string): The name of the right camera, should match with name in camera_info file.
+
+* `~framerate` (fps): Framerate to capture at. Maximum 90fps
+
+* `~height` (pixels): Height to capture images at.
+
+* `~width` (pixels): Width to capture images at.
+
+* `~quality` (0-100): Quality of the captured images.
+
 * `~camera_id` (int): The camera id (only supported on Compute Module)
 
 ## Calibration

camera_info/left/left.yaml

@@ -0,0 +1,26 @@
+image_width: 640
+image_height: 480
+camera_name: left_camera_optical_frame
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [ 383.37648,    0.     ,  318.72515,
+            0.     ,  386.12691,  251.37268,
+            0.     ,    0.     ,    1.     ]
+camera_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.294786, 0.071835, -0.004425, 0.000128, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [ 0.99380801, -0.0319161 ,  0.10642835,
+          0.03129902,  0.9994822 ,  0.00746371,
+         -0.10661146, -0.00408639,  0.99429236]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [ 357.83988,    0.     ,  263.50254,    0.     ,
+            0.     ,  357.83988,  249.2244 ,    0.     ,
+            0.     ,    0.     ,    1.     ,    0.     ]

camera_info/left/ost.yaml

@@ -0,0 +1,26 @@
+image_width: 640
+image_height: 480
+camera_name: left_camera_optical_frame
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [ 377.34204,    0.     ,  317.17444,
+            0.     ,  377.98404,  242.82206,
+            0.     ,    0.     ,    1.     ]
+camera_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.317285, 0.096353, -0.000721, 0.000562, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [ 1.,  0.,  0.,
+          0.,  1.,  0.,
+          0.,  0.,  1.]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [ 284.43878,    0.     ,  317.7113 ,    0.     ,
+            0.     ,  322.88959,  243.25744,    0.     ,
+            0.     ,    0.     ,    1.     ,    0.     ]

camera_info/right/ost.yaml

@@ -0,0 +1,26 @@
+image_width: 640
+image_height: 480
+camera_name: right_camera_optical_frame
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [ 370.55914,    0.     ,  329.09807,
+            0.     ,  371.35797,  257.59864,
+            0.     ,    0.     ,    1.     ]
+camera_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.324288, 0.107501, -0.001059, 0.000672, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [ 1.,  0.,  0.,
+          0.,  1.,  0.,
+          0.,  0.,  1.]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [ 289.42285,    0.     ,  328.57791,    0.     ,
+            0.     ,  314.12253,  263.15155,    0.     ,
+            0.     ,    0.     ,    1.     ,    0.     ]

camera_info/right/right.yaml

@@ -0,0 +1,26 @@
+image_width: 640
+image_height: 480
+camera_name: right_camera_optical_frame
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [ 378.37387,    0.     ,  329.03434,
+            0.     ,  380.28319,  260.9376 ,
+            0.     ,    0.     ,    1.     ]
+camera_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.305682, 0.078253, -0.001969, 0.000841, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [ 0.99584801, -0.02643552,  0.08710852,
+          0.02693943,  0.99962642, -0.00461425,
+         -0.086954  ,  0.00694175,  0.99618814]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [ 357.83988,    0.     ,  263.50254,  -23.39691,
+            0.     ,  357.83988,  249.2244 ,    0.     ,
+            0.     ,    0.     ,    1.     ,    0.     ]

include/RaspiCLI.h

@@ -0,0 +1,56 @@
+/*
+Copyright (c) 2013, Broadcom Europe Ltd
+Copyright (c) 2013, James Hughes
+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 copyright holder 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 RASPICLI_H_
+#define RASPICLI_H_
+
+typedef struct
+{
+   int id;
+   char *command;
+   char *abbrev;
+   char *help;
+   int num_parameters;
+} COMMAND_LIST;
+
+/// Cross reference structure, mode string against mode id
+typedef struct xref_t
+{
+   char *mode;
+   int mmal_mode;
+} XREF_T;
+
+
+void raspicli_display_help(const COMMAND_LIST *commands, const int num_commands);
+int raspicli_get_command_id(const COMMAND_LIST *commands, const int num_commands, const char *arg, int *num_parameters);
+
+int raspicli_map_xref(const char *str, const XREF_T *map, int num_refs);
+const char *raspicli_unmap_xref(const int en, XREF_T *map, int num_refs);
+
+
+#endif