00001
00002 """
00003 @file check_camera_info_matrix.py
00004 """
00005 import os
00006 import sys
00007 import subprocess
00008 import signal
00009 import unittest
00010 import re
00011 import time
00012 import rospy
00013 import rostest
00014 from rs_general.rs_general import parse_camera_type
00015
00016 PKG = 'realsense_camera'
00017 NAME = 'matrix_check'
00018 PROJECTION = 'projection_matrix'
00019 PROJECTION_LENGHT = 12
00020 DISTORTION = 'distortion_coefficients'
00021 DISTORTION_LENGHT = 5
00022 ROTATION = 'rotation_identity'
00023 ROTATION_LENGHT = 9
00024 DEPTH = 'depth'
00025 COLOR = 'color'
00026 IR = 'ir'
00027 IR2 = 'ir2'
00028 FISHEYE = 'fisheye'
00029 CAMERAS = ['R200', 'F200', 'SR300', 'ZR300']
00030
00031
00032 class CheckCameraInfoMatrix(unittest.TestCase):
00033 """
00034 @class CheckCameraInfoMatrix
00035 """
00036
00037 def setUp(self):
00038 """
00039 @fn setUp
00040 @param self
00041 @return
00042 """
00043 self.assertIn(camera_type, CAMERAS)
00044
00045 def get_matrix(self, string, matrix_type):
00046 """search values of matrix type from string,
00047 and organize into matrix array
00048 @fn get_matrix
00049 @param self
00050 @param string
00051 @param matrix_type
00052 @return matrix_array: matrix values
00053 """
00054 if (matrix_type == PROJECTION and not string.find('P: ') == -1) \
00055 or (matrix_type == DISTORTION and
00056 not string.find('D: ') == -1) \
00057 or (matrix_type == ROTATION and
00058 not string.find('R: ') == -1):
00059 values = re.findall(r'([0-9]+\.[0-9]+)', string)
00060 matrix_array = map(float, values)
00061 return matrix_array
00062 else:
00063 return ''
00064
00065 def is_zero(self, value):
00066 """determine whether the value equal to 0
00067 @fn is_zero
00068 @param self
00069 @param value
00070 @return True: equal to 0; False: not equal to 0
00071 """
00072 if (value > -0.00000001) and (value < 0.00000001):
00073 return True
00074 else:
00075 return False
00076
00077 def get_camera_info(self, stream_type, matrix_type):
00078 """run rostopic to print camera info, get matrix values from stdout
00079 @fn get_camera_info
00080 @param self
00081 @param stream_type
00082 @param matrix_type
00083 @return matrix_array
00084 """
00085 cmd = 'rostopic echo /camera/' + stream_type + '/camera_info'
00086 print "cmd = ", cmd
00087 process = subprocess.Popen(cmd, stdout=subprocess.PIPE,
00088 stderr=subprocess.PIPE, shell=True)
00089 while True:
00090 buff = process.stdout.readline()
00091 if buff == '' and process.poll is not None:
00092 break
00093 array = self.get_matrix(buff, matrix_type)
00094 if array != '':
00095 os.kill(process.pid, signal.SIGKILL)
00096 return array
00097 return ''
00098
00099 def verify_projection_matrix(self, matrix, stream_type):
00100 """verify projection matrix values, adapt each stream type
00101 @fn verify_projection_matrix
00102 @param self
00103 @param matrix
00104 @param stream_type
00105 return
00106 """
00107 if matrix == '' or len(matrix) != PROJECTION_LENGHT \
00108 or stream_type == '':
00109 self.assertTrue(False,
00110 'Not get correct projection matrix or stream type.')
00111
00112 if stream_type == DEPTH:
00113 if (self.is_zero(matrix[3]) or
00114 self.is_zero(matrix[7]) or
00115 self.is_zero(matrix[11])):
00116 self.assertTrue(False, 'Projection matrix value is incorrect.')
00117 elif (stream_type == COLOR or
00118 stream_type == IR or
00119 stream_type == IR2 or
00120 stream_type == FISHEYE):
00121 if (not self.is_zero(matrix[3]) or
00122 not self.is_zero(matrix[7]) or
00123 not self.is_zero(matrix[11])):
00124 self.assertTrue(False, 'Projection matrix value is incorrect.')
00125 else:
00126 self.assertTrue(False,
00127 'Stream type "' + stream_type + '" is invalid.')
00128
00129 def verify_rotation_matrix(self, matrix):
00130 """verify rotation matrix values, adapt each stream type
00131 @fn verify_rotation_matrix
00132 @param self
00133 @param matrix
00134 return
00135 """
00136 if matrix == '' or len(matrix) != ROTATION_LENGHT:
00137 self.assertTrue(False, 'Not get correct rotation matrix.')
00138
00139 for i in range(0, len(matrix)):
00140 if i == 0 or i == 4 or i == 8:
00141 if matrix[i] < 0.99999999 or matrix[i] > 1.00000001:
00142 self.assertTrue(False, 'Rotation matrix value is incorrect.')
00143 else:
00144 if not self.is_zero(matrix[i]):
00145 self.assertTrue(False, 'Rotation matrix value is incorrect.')
00146
00147 def verify_distortion_matrix(self, matrix, stream_type):
00148 """verify distortion matrix values, adapt each stream type
00149 @fn verify_distortion_matrix
00150 @param self
00151 @param matrix
00152 @param stream_type
00153 return
00154 """
00155 if camera_type == '' or stream_type == '' or matrix == '' \
00156 or len(matrix) != DISTORTION_LENGHT:
00157 self.assertTrue(False,
00158 'Not get correct distortion matrix or stream type.')
00159
00160 if camera_type == 'R200' or camera_type == 'ZR300':
00161 if stream_type == COLOR:
00162 for i in range(0, len(matrix)):
00163 if i == 4:
00164 self.assertTrue(self.is_zero(matrix[i]), 'Distortion \
00165 matrix value of color is incorrect.')
00166 else:
00167 self.assertFalse(self.is_zero(matrix[i]), 'Distortion \
00168 matrix value of color is incorrect.')
00169 elif (stream_type == DEPTH or
00170 stream_type == IR or
00171 stream_type == IR2):
00172 for i in range(0, len(matrix)):
00173 self.assertTrue(self.is_zero(matrix[i]), 'Distortion \
00174 matrix value of (depth/IR/IR2) is incorrect.')
00175 elif stream_type == FISHEYE:
00176 for i in range(0, len(matrix)):
00177 if i == 0:
00178 self.assertFalse(self.is_zero(matrix[i]), 'Distortion \
00179 matrix value of fisheye is incorrect.')
00180 else:
00181 self.assertTrue(self.is_zero(matrix[i]), 'Distortion \
00182 matrix value of fisheye is incorrect.')
00183 else:
00184 self.assertTrue(False,
00185 'Stream type "' + stream_type + '" is invalid.')
00186 elif camera_type == 'F200' or camera_type == 'SR300':
00187 if stream_type == COLOR:
00188 for i in range(0, len(matrix)):
00189 self.assertTrue(self.is_zero(matrix[i]), 'Distortion \
00190 matrix value of color is incorrect.')
00191 elif stream_type == DEPTH or stream_type == IR:
00192 for i in range(0, len(matrix)):
00193 self.assertFalse(self.is_zero(matrix[i]), 'Distortion \
00194 matrix value of (depth/IR) is incorrect.')
00195 else:
00196 self.assertTrue(False,
00197 'Stream type "' + stream_type + '" is invalid.')
00198
00199 def test_camera_info_check_projection_matrix(self):
00200 """check projection matrix values from camera info
00201 @fn test_check_projection_matrix
00202 @param self
00203 @return
00204 """
00205 rospy.init_node(NAME, anonymous=True, log_level=rospy.INFO)
00206
00207
00208 time.sleep(10)
00209
00210 self.verify_projection_matrix(
00211 self.get_camera_info(DEPTH, PROJECTION), DEPTH)
00212 time.sleep(0.5)
00213 self.verify_projection_matrix(
00214 self.get_camera_info(COLOR, PROJECTION), COLOR)
00215 time.sleep(0.5)
00216 self.verify_projection_matrix(
00217 self.get_camera_info(IR, PROJECTION), IR)
00218 time.sleep(0.5)
00219 if camera_type == 'R200' or camera_type == 'ZR300':
00220 self.verify_projection_matrix(
00221 self.get_camera_info(IR2, PROJECTION), IR2)
00222 time.sleep(0.5)
00223 if camera_type == 'ZR300':
00224 self.verify_projection_matrix(
00225 self.get_camera_info(FISHEYE, PROJECTION), FISHEYE)
00226 time.sleep(0.5)
00227
00228 def test_camera_info_check_rotation_identity_matrix(self):
00229 """check rotation identity matrix values from camera info
00230 @fn test_check_rotation_identity_matrix
00231 @param self
00232 @return
00233 """
00234 rospy.init_node(NAME, anonymous=True, log_level=rospy.INFO)
00235
00236
00237 time.sleep(10)
00238
00239 self.verify_rotation_matrix(self.get_camera_info(DEPTH, ROTATION))
00240 time.sleep(0.5)
00241 self.verify_rotation_matrix(self.get_camera_info(COLOR, ROTATION))
00242 time.sleep(0.5)
00243 self.verify_rotation_matrix(self.get_camera_info(IR, ROTATION))
00244 time.sleep(0.5)
00245 if camera_type == 'R200' or camera_type == 'ZR300':
00246 self.verify_rotation_matrix(self.get_camera_info(IR2, ROTATION))
00247 time.sleep(0.5)
00248 if camera_type == 'ZR300':
00249 self.verify_rotation_matrix(self.get_camera_info(FISHEYE, ROTATION))
00250 time.sleep(0.5)
00251
00252 def test_camera_info_check_distortion_coefficients(self):
00253 """check distortion coefficients matrix values from camera info
00254 @fn test_check_distortion_coefficients
00255 @param self
00256 @return
00257 """
00258 rospy.init_node(NAME, anonymous=True, log_level=rospy.INFO)
00259
00260
00261 time.sleep(10)
00262
00263 self.verify_distortion_matrix(
00264 self.get_camera_info(DEPTH, DISTORTION), DEPTH)
00265 time.sleep(0.5)
00266 self.verify_distortion_matrix(
00267 self.get_camera_info(COLOR, DISTORTION), COLOR)
00268 time.sleep(0.5)
00269 self.verify_distortion_matrix(
00270 self.get_camera_info(IR, DISTORTION), IR)
00271 time.sleep(0.5)
00272 if camera_type == 'R200' or camera_type == 'ZR300':
00273 self.verify_distortion_matrix(
00274 self.get_camera_info(IR2, DISTORTION), IR2)
00275 time.sleep(0.5)
00276 if camera_type == 'ZR300':
00277 self.verify_distortion_matrix(
00278 self.get_camera_info(FISHEYE, DISTORTION), FISHEYE)
00279 time.sleep(0.5)
00280
00281 if __name__ == '__main__':
00282 camera_type = parse_camera_type(sys.argv)
00283 rostest.rosrun(PKG, NAME, CheckCameraInfoMatrix, sys.argv)