def verbose_callback(self, cam_info, features): with self._lock: if self._mode is "verbose": # Populate measurement message msg = CameraMeasurement() msg.header.stamp = cam_info.header.stamp msg.camera_id = self._cam_id msg.cam_info = cam_info msg.features = features self._callback(self._cam_id, msg)
P = [525, 0, 319.5, 0, 0, 525, 239.5, 0, 0, 0, 1, 0] P_mat = reshape( matrix(P, float), (3,4) ) cal_pattern = CalibrationPattern() for c in check_points: pnt = Point32() pnt.x = c[0] pnt.y = c[1] pnt.z = c[2] cal_pattern.object_points.append(pnt) cal_samples = [] for target in cal_estimate.targets: cal_sample = RobotMeasurement() for camera in cal_estimate.cameras: meas = CameraMeasurement() meas.camera_id = camera.camera_id for pnt_c in check_points: pnt_msg = posemath.fromMsg(camera.pose).Inverse() * posemath.fromMsg(target) * pnt_c pnt_mat = P_mat * matrix([pnt_msg[0], pnt_msg[1], pnt_msg[2], 1]).T pnt = ImagePoint() pnt.x = (pnt_mat[0]/pnt_mat[2]) + random.random()*noise pnt.y = (pnt_mat[1]/pnt_mat[2]) + random.random()*noise pnt.d = 1 meas.image_points.append(pnt) meas.cam_info.P = P meas.features = cal_pattern cal_sample.M_cam.append(meas) cal_samples.append(cal_sample) print cal_samples