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)
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 # add offset cal_estimate.cameras[0].pose = posemath.toMsg(posemath.fromMsg(cal_estimate.cameras[0].pose) * offset) #cal_estimate.cameras[1].pose = posemath.toMsg(posemath.fromMsg(cal_estimate.cameras[1].pose) * PyKDL.Frame(offset)) # Run optimization estimate.enhance(cal_samples, cal_estimate) #residual, J = estimate.calculate_residual_and_jacobian(cal_samples, cal_estimate) #set_printoptions(linewidth=300, precision=5, suppress=True)