def calc_mean_reproj_error(self,msg): ros_cam = CameraModel._from_parts(intrinsics=msg) all_ims = [] for imd in self.db: ros_pix = ros_cam.project_3d_to_pixel(imd['cc'], distorted=True) d = (ros_pix-imd['pix'])**2 drows = np.sqrt(np.sum(d, axis=1)) mean_d = np.mean(drows) all_ims.append(mean_d) mean_err = np.mean(all_ims) return mean_err
def _build_test_camera(**kwargs): o = Bunch(**kwargs) if not kwargs.get('at_origin',False): translation = geometry_msgs.msg.Point() translation.x = 0.273485679077 translation.y = 0.0707310128808 translation.z = 0.0877802104531 rotation = geometry_msgs.msg.Quaternion() rotation.x = 0.309377331102 rotation.y = 0.600893485738 rotation.z = 0.644637681813 rotation.w = 0.357288321925 else: translation = geometry_msgs.msg.Point() translation.x = 0.0 translation.y = 0.0 translation.z = 0.0 rotation = geometry_msgs.msg.Quaternion() rotation.x = 0.0 rotation.y = 0.0 rotation.z = 0.0 rotation.w = 1.0 if 'from_file' in kwargs: if kwargs['type']=='ros': cam = CameraModel.load_camera_from_file( fname=kwargs['filename'], extrinsics_required=False ) i = cam.get_intrinsics_as_bunch() cam = CameraModel._from_parts( translation=point_msg_to_tuple(translation), rotation=parse_rotation_msg(rotation), intrinsics=i, name='cam', ) elif kwargs['type']=='pymvg': del translation del rotation system = MultiCameraSystem.from_pymvg_file(fname=kwargs['filename']) names = system.get_names() names.sort() name = names[0] # get first camera from system cam = system._cameras[name] rotation = cam.get_extrinsics_as_bunch().rotation translation = cam.get_extrinsics_as_bunch().translation else: raise ValueError('unknown file type') else: if o.ROS_test_data: i = sensor_msgs.msg.CameraInfo() # these are from image_geometry ROS package in the utest.cpp file i.height = 480 i.width = 640 i.distortion_model = 'plumb_bob' i.D = [-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0] i.K = [430.15433020105519, 0.0, 311.71339830549732, 0.0, 430.60920415473657, 221.06824942698509, 0.0, 0.0, 1.0] i.R = [0.99806560714807102, 0.0068562422224214027, 0.061790256276695904, -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664, -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516] i.P = [295.53402059708782, 0.0, 285.55760765075684, 0.0, 0.0, 295.53402059708782, 223.29617881774902, 0.0, 0.0, 0.0, 1.0, 0.0] else: i = sensor_msgs.msg.CameraInfo() i.height = 494 i.width = 659 i.distortion_model = 'plumb_bob' i.D = [-0.34146457767225, 0.196070795764995, 0.000548988393912233, 0.000657058395082583, -0.0828776806503243] i.K = [516.881868241566, 0.0, 333.090936517613, 0.0, 517.201263180996, 231.526036849886, 0.0, 0.0, 1.0] i.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] i.P = [442.17529296875, 0.0, 334.589001099812, 0.0, 0.0, 474.757141113281, 228.646131377705, 0.0, 0.0, 0.0, 1.0, 0.0] cam = CameraModel._from_parts( translation=point_msg_to_tuple(translation), rotation=parse_rotation_msg(rotation), intrinsics=i, name='cam', ) if kwargs.get('flipped',False): cam = cam.get_flipped_camera() if kwargs.get('get_input_data',False): return dict(cam=cam, translation=translation, rotation=rotation, ) return cam