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)
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
