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