├── 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 |
--------------------------------------------------------------------------------