├── scripts ├── __pycache__ │ ├── cal.cpython-36.pyc │ ├── test.cpython-36.pyc │ ├── rs_util.cpython-36.pyc │ └── test_realsense.cpython-36.pyc ├── rs_util.py ├── publish_custom_instrinsics.py ├── test_realsense.py └── cal.py ├── launch ├── calibrations │ ├── 1_30_2021_daniilidis_camA_custom_intrinsics.launch │ ├── 1_30_2021_daniilidis_camB_custom_intrinsics.launch │ └── 2_3_2021_daniilidis_camC_default_intrinsics.launch └── vision.launch ├── package.xml └── CMakeLists.txt /scripts/__pycache__/cal.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carismoses/vision_test/main/scripts/__pycache__/cal.cpython-36.pyc -------------------------------------------------------------------------------- /scripts/__pycache__/test.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carismoses/vision_test/main/scripts/__pycache__/test.cpython-36.pyc -------------------------------------------------------------------------------- /scripts/__pycache__/rs_util.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carismoses/vision_test/main/scripts/__pycache__/rs_util.cpython-36.pyc -------------------------------------------------------------------------------- /scripts/__pycache__/test_realsense.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/carismoses/vision_test/main/scripts/__pycache__/test_realsense.cpython-36.pyc -------------------------------------------------------------------------------- /launch/calibrations/1_30_2021_daniilidis_camA_custom_intrinsics.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 7 | 8 | -------------------------------------------------------------------------------- /launch/calibrations/1_30_2021_daniilidis_camB_custom_intrinsics.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 7 | 8 | -------------------------------------------------------------------------------- /scripts/rs_util.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pyrealsense2 as rs 3 | 4 | def rs_intrinsics_to_opencv_intrinsics(intr): 5 | D = np.array(intr.coeffs) 6 | K = np.array([[intr.fx, 0, intr.ppx], 7 | [0, intr.fy, intr.ppy], 8 | [0, 0, 1]]) 9 | return K, D 10 | 11 | def get_serial_number(pipeline_profile): 12 | return pipeline_profile.get_device().get_info(rs.camera_info.serial_number) 13 | 14 | def get_intrinsics(pipeline_profile, stream=rs.stream.color): 15 | stream_profile = pipeline_profile.get_stream(stream) # Fetch stream profile for depth stream 16 | intr = stream_profile.as_video_stream_profile().get_intrinsics() # Downcast to video_stream_profile and fetch intrinsics 17 | return rs_intrinsics_to_opencv_intrinsics(intr) 18 | -------------------------------------------------------------------------------- /launch/calibrations/2_3_2021_daniilidis_camC_default_intrinsics.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 7 | 8 | 10 | 11 | -------------------------------------------------------------------------------- /launch/vision.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /scripts/publish_custom_instrinsics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Publishes custom camera intrinsics to new topics 5 | Copyright 2021 Massachusetts Institute of Technology 6 | """ 7 | 8 | import rospy 9 | from sensor_msgs.msg import CameraInfo 10 | from cal import get_custom_intrinsics 11 | 12 | if __name__=="__main__": 13 | # Setup 14 | rospy.init_node("custom_intrinsics_publisher") 15 | camera_names = ["A", "B", "C"] 16 | rospy.loginfo(f"Publishing custom intrinsics for cameras: {camera_names}") 17 | 18 | # Initialize publishers for each camera 19 | pubs = {} 20 | for n in camera_names: 21 | topic_name = f"/camera_{n}/custom_intrinsics" 22 | pubs[n] = rospy.Publisher(topic_name, CameraInfo, queue_size=1) 23 | 24 | # Loop indefinitely and publish intrinsics for all cameras 25 | r = rospy.Rate(10) 26 | while not rospy.is_shutdown(): 27 | for n in camera_names: 28 | K, dist = get_custom_intrinsics(n) 29 | info = CameraInfo() 30 | info.K = K.flatten().tolist() 31 | info.D = dist.squeeze().tolist() 32 | pubs[n].publish(info) 33 | r.sleep() 34 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | vision_test 4 | 0.0.0 5 | The vision_test package 6 | 7 | 8 | 9 | 10 | caris 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /scripts/test_realsense.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import pyrealsense2 as rs 3 | import cv2 4 | import sys 5 | import numpy as np 6 | import rospy 7 | 8 | # in another ROS package 9 | from cv_bridge import CvBridge 10 | from sensor_msgs.msg import Image 11 | 12 | # in this ROS package 13 | from cal import get_custom_intrinsics 14 | from rs_util import get_intrinsics 15 | 16 | 17 | use_custom_intrinsics = True 18 | vis_opencv = True 19 | publish_ros = True 20 | camera_lookup = {'032622074588':'A', '028522072401':'B', '032622073024': 'C'} 21 | 22 | 23 | def step(pipeline, publisher): 24 | # Wait for a coherent pair of frames: depth and color 25 | frames = pipeline.wait_for_frames() 26 | color_frame = frames.get_color_frame() 27 | if not color_frame: 28 | print('didnt get color frames ') 29 | return 30 | 31 | # convert image to numpy array 32 | color_image = np.asarray(color_frame.get_data()) 33 | # convert to grayscale 34 | gray_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2GRAY) 35 | 36 | # publish image 37 | if publish_ros: 38 | bridge = CvBridge() 39 | image_message = bridge.cv2_to_imgmsg(color_image, encoding="passthrough") 40 | publisher.publish(image_message) 41 | 42 | # show image 43 | if vis_opencv: 44 | cv2.imshow('Camera View', color_image) 45 | cv2.waitKey(1) 46 | 47 | 48 | def configure_camera(serial_number): 49 | # Configure depth and color streams 50 | pipeline = rs.pipeline() 51 | config = rs.config() 52 | 53 | # Get device product line for setting a supporting resolution 54 | pipeline_wrapper = rs.pipeline_wrapper(pipeline) 55 | pipeline_profile = config.resolve(pipeline_wrapper) 56 | device = pipeline_profile.get_device() 57 | 58 | found_rgb = False 59 | for s in device.sensors: 60 | if s.get_info(rs.camera_info.name) == 'RGB Camera': 61 | found_rgb = True 62 | break 63 | if not found_rgb: 64 | print("The demo requires Depth camera with Color sensor") 65 | exit(0) 66 | 67 | config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) 68 | #config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30) 69 | 70 | # Start streaming 71 | pipeline.start(config) 72 | 73 | # And get the device info 74 | print(f'Connected to {serial_number}') 75 | 76 | # get the camera intrinsics 77 | if use_custom_intrinsics: 78 | print('Using custom intrinsics from camera.') 79 | intrinsics = get_custom_intrinsics(camera_lookup[serial_number]) 80 | else: 81 | print('Using default intrinsics from camera.') 82 | intrinsics = get_intrinsics(pipeline_profile) 83 | 84 | publisher = None 85 | if publish_ros: 86 | publisher = rospy.Publisher('camera_%s' % camera_lookup[serial_number], 87 | Image, 88 | queue_size=10) 89 | 90 | return pipeline, publisher 91 | 92 | 93 | def main(): 94 | print('Listing available realsense devices...') 95 | 96 | serial_numbers = [] 97 | for i, device in enumerate(rs.context().devices): 98 | serial_number = device.get_info(rs.camera_info.serial_number) 99 | serial_numbers.append(serial_number) 100 | print(f'{i+1}. {serial_number}') 101 | 102 | pipelines, publishers = [], [] 103 | for serial_number in serial_numbers: 104 | pipeline, publisher = configure_camera(serial_number) 105 | pipelines.append(pipeline) 106 | publishers.append(publisher) 107 | 108 | if len(pipelines) == 0: 109 | print('Could not find any Realsense Devices.') 110 | sys.exit(1) 111 | 112 | if publish_ros: 113 | rospy.init_node('publish_images') 114 | 115 | # loop and get camera data 116 | try: 117 | if publish_ros: 118 | while not rospy.is_shutdown(): 119 | for pipeline, publisher in zip(pipelines, publishers): 120 | step(pipeline, publisher) 121 | else: 122 | while True: 123 | for pipeline, publisher in zip(pipelines, publishers): 124 | step(pipeline, publisher) 125 | 126 | finally: 127 | # Stop streaming 128 | for pipeline in pipelines: 129 | pipeline.stop() 130 | 131 | 132 | if __name__ == '__main__': 133 | main() 134 | -------------------------------------------------------------------------------- /scripts/cal.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def get_custom_intrinsics(camera_name): 4 | if camera_name == 'A': 5 | # Camera A: 1920 x 1080 Mike's Cal (camera_calibration ros) 6 | mtx = np.array([[1342.451693 , 0. , 1005.799801], 7 | [ 0. ,1339.678018, 545.304949], 8 | [ 0. , 0. , 1. ]]) 9 | 10 | dist = np.array([[ 0.069644, -0.154332, -0.004702, 0.004893, 0.000000]]) 11 | elif camera_name == 'B': 12 | # Camera B: 1920x1080 Mike's Cal (camera_calibration ros) 13 | mtx = np.array([[1349.544501 , 0. , 974.411300], 14 | [ 0. ,1348.501863, 530.852211], 15 | [ 0. , 0. , 1. ]]) 16 | 17 | dist = np.array([[ 0.099147, -0.190634, 0.001793, 0.004096, 0.000000]]) 18 | elif camera_name == 'C': 19 | # Camera C: 1920x1080 Mike's Cal (camera_calibration ros) 20 | mtx = np.array([[1296.617377 , 0. , 1010.359942], 21 | [ 0. ,1295.837058, 576.386493], 22 | [ 0. , 0. , 1. ]]) 23 | 24 | dist = np.array([[0.104545, -0.148822, 0.000863, 0.003670, 0.000000]]) 25 | return None 26 | else: 27 | raise NotImplementedError() 28 | return mtx, dist 29 | 30 | ########## 680x480 ########## 31 | 32 | # Camera A: 640x480 Izzy's Cal 33 | # mtx = np.array([[628.1604173 , 0. , 286.50564325], 34 | # [ 0. , 626.92951202, 250.89533263], 35 | # [ 0. , 0. , 1. ]]) 36 | # 37 | # dist = np.array([[ 0.09078199, -0.01523302, 0.00307738, -0.01766021, -0.19786653]]) 38 | 39 | # Camera A: 640 x 480 Realsense Defaults 40 | # mtx = np.array([[606.583374023438 , 0. , 331.773406982422], 41 | # [ 0. , 606.31201171875, 250.26139831543], 42 | # [ 0. , 0. , 1. ]]) 43 | # 44 | # dist = np.array([[ 0., 0., 0., 0., 0.]]) 45 | 46 | # Camera A: 640 x 480 Mike's Cal (camera_calibration ros) 47 | # mtx = np.array([[603.306972 , 0. , 337.270625], 48 | # [ 0. , 603.468975, 247.853768], 49 | # [ 0. , 0. , 1. ]]) 50 | # 51 | # dist = np.array([[ 0.115294, -0.241355, -0.002980, 0.003112, 0.000000]]) 52 | 53 | ########## 1280x720 ########## 54 | 55 | # Camera A: 1280 x 720 Realsense Defaults 56 | # mtx = np.array([[909.875122070312 , 0. , 657.66015625], 57 | # [ 0. , 909.467956542969, 375.39208984375], 58 | # [ 0. , 0. , 1. ]]) 59 | # 60 | # dist = np.array([[ 0., 0., 0., 0., 0.]]) 61 | 62 | # Camera A: 1280 x 720 Mike's Cal (camera_calibration ros) 63 | # mtx = np.array([[902.257272 , 0. , 664.567352], 64 | # [ 0. , 901.807867, 363.254469], 65 | # [ 0. , 0. , 1. ]]) 66 | # 67 | # dist = np.array([[ 0.091499, -0.163526, -0.005734, 0.003308, 0.000000]]) 68 | 69 | ########## 1920x1080 ########## 70 | 71 | # Camera A: 1920x1080 Realsense Defaults 72 | # mtx = np.array([[1364.81262207031 , 0. , 986.490234375], 73 | # [ 0. ,1364.20202636719, 563.088134765625], 74 | # [ 0. , 0. , 1. ]]) 75 | # 76 | # dist = np.array([[ 0., 0., 0., 0., 0.]]) 77 | 78 | # Camera A: 1920 x 1080 Mike's Cal (camera_calibration ros) 79 | # mtx = np.array([[1342.451693 , 0. , 1005.799801], 80 | # [ 0. ,1339.678018, 545.304949], 81 | # [ 0. , 0. , 1. ]]) 82 | # 83 | # dist = np.array([[ 0.069644, -0.154332, -0.004702, 0.004893, 0.000000]]) 84 | 85 | # Camera B: 1920x1080 Realsense Defaults 86 | # mtx = np.array([[1377.68286132812 , 0. , 964.721923828125], 87 | # [ 0. ,1375.89331054688, 527.139770507812], 88 | # [ 0. , 0. , 1. ]]) 89 | # 90 | # dist = np.array([[ 0., 0., 0., 0., 0.]]) 91 | 92 | # Camera B: 1920x1080 Mike's Cal (camera_calibration ros) 93 | # mtx = np.array([[1349.544501 , 0. , 974.411300], 94 | # [ 0. ,1348.501863, 530.852211], 95 | # [ 0. , 0. , 1. ]]) 96 | # 97 | # dist = np.array([[ 0.099147, -0.190634, 0.001793, 0.004096, 0.000000]]) 98 | 99 | # Camera C: 1920x1080 Mike's Cal (camera_calibration ros) 100 | # mtx = np.array([[1296.617377 , 0. , 1010.359942], 101 | # [ 0. ,1295.837058, 576.386493], 102 | # [ 0. , 0. , 1. ]]) 103 | # 104 | # dist = np.array([[0.104545 -0.148822 0.000863 0.003670 0.000000]]) 105 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(vision_test) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES vision_test 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/vision_test.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/vision_test_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # catkin_install_python(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_vision_test.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | --------------------------------------------------------------------------------