def meas_cb(self, msg):
with self.lock:
# check if cam_info is valid
for camera in msg.M_cam:
P = camera.cam_info.P
all_zero = True
for i in range(12):
if P[i] != 0:
all_zero = False
if all_zero:
rospy.logfatal("Camera info of %s is all zero. You should calibrate your camera intrinsics first "%camera.camera_id)
exit(-1)
# Modify all the camera info messages to make sure that ROI and binning are removed, and P is updated accordingly
# Update is done in place
#for camera in msg.M_cam:
# camera.cam_info = camera_info_converter.unbin(camera.cam_info)
if rospy.has_param('optimizer_conditional_resetter_enable'):
self.reset_flag = rospy.get_param('optimizer_conditional_resetter_enable')
else:
self.reset_flag = False
if self.reset_flag == True :
urdf_cam_ns = rospy.get_param("urdf_cam_ns")
new_cam_ns = rospy.get_param("new_cam_ns")
u_info = rospy.wait_for_message(urdf_cam_ns+'/camera_info', CameraInfo)
n_info = rospy.wait_for_message(new_cam_ns+'/camera_info', CameraInfo)
urdf_cam_frame = u_info.header.frame_id
new_cam_frame = n_info.header.frame_id
mounting_frame = rospy.get_param("mounting_frame")
# if any of the frames has changed...
if self.prev_3_frames != () :
if self.prev_3_frames != (urdf_cam_frame, new_cam_frame, mounting_frame):
self.frames_changed = True
if self.frames_changed :
self.state = None
self.meas = [] # clear cache
self.timestamps = []
self.measurement_count = 0
self.prev_tf_transforms = {}
rospy.loginfo('Reset calibration state')
print "\033[43;1mTarget frames have changed. Clean up everything and start over! \033[0m\n\n"
self.frames_changed = False
if len(self.tf_check_pairs) > 0 :
self.prev_tf_pair = self.tf_check_pairs[0]
self.tf_check_pairs = []
self.tf_check_pairs.append((mounting_frame, urdf_cam_frame)) # only keep one pair in the list at a time
self.prev_3_frames = (urdf_cam_frame, new_cam_frame, mounting_frame)
self.prev_tf_pair = (mounting_frame, urdf_cam_frame)
TheMoment = msg.header.stamp
# check for tf transform changes
for (frame1, frame2) in self.tf_check_pairs:
transform = None
print "\nLooking up transformation \033[34;1mfrom\033[0m %s \033[34;1mto\033[0m %s ..." % (frame1, frame2)
while not rospy.is_shutdown():
try:
self.tf_listener.waitForTransform(frame1, frame2, TheMoment, rospy.Duration(10))
transform = self.tf_listener.lookupTransform(frame1, frame2, TheMoment) #((),())
print "found\n"
break
except (tf.LookupException, tf.ConnectivityException):
print "transform lookup failed, retrying..."
if self.measurement_count > 0:
prev_transform = self.prev_tf_transforms[(frame1, frame2)]
dx = transform[0][0] - prev_transform[0][0]
dy = transform[0][1] - prev_transform[0][1]
dz = transform[0][2] - prev_transform[0][2]
dAngle = self.getAngle(transform[1]) - self.getAngle(prev_transform[1])
a=self.getAxis(transform[1])
b=self.getAxis(prev_transform[1])
dAxis = (a[0]*b[0] + a[1]*b[1] + a[2]*b[2])/1. # a dot b
#print "\n"
#print "\033[34;1m-- Debug Info --\033[0m"
print "checking for pose change..."
print "measurement_count = %d" % self.measurement_count
print " dx = %10f | < 0.0002 m" % dx
print " dy = %10f | < 0.0002 m" % dy
print " dz = %10f | < 0.0002 m" % dz
print "dAngle = %10f | < 0.00174 rad" % dAngle
print " dAxis = %10f | > 0.99999\n" % dAxis
if ( (math.fabs(dx) > 0.0002) or (math.fabs(dy) > 0.0002) or (math.fabs(dz) > 0.0002) or
(math.fabs(dAngle)>0.00174) or (math.fabs(dAxis) < 0.99999) ) : # if transform != previous_transform:
print "\033[44;1mDetect pose change: %s has changed pose relative to %s since last time!\033[0m\n\n" % (frame1, frame2)
###self.reset() # no, you cannot call reset() here.
self.state = None
count = len(self.meas)
self.meas = [] # clear cache
self.timestamps = []
rospy.loginfo('Reset calibration state')
print "\033[33;1m%ld\033[0m previous measurements in the cache are deleted.\n\n" % count
self.measurement_count = 0
self.prev_tf_transforms[(frame1, frame2)] = transform
# add timestamp to list
self.timestamps.append(msg.header.stamp)
# add measurements to list
self.meas.append(msg)
print "MEAS", len(self.meas)
for m in self.meas:
print " - stamp: %f"%m.header.stamp.to_sec()
print "\n"
self.measurement_count += 1
print "\nNo. of measurements fed to optimizer: %d\n\n" % self.measurement_count
## self.last_stamp_pub.publish(self.meas[-1].header.stamp)
# initialize state if needed
if True: #not self.state:
self.state = CalibrationEstimate()
camera_poses, checkerboard_poses = init_optimization_prior.find_initial_poses(self.meas)
self.state.targets = [ posemath.toMsg(checkerboard_poses[i]) for i in range(len(checkerboard_poses)) ]
self.state.cameras = [ CameraPose(camera_id, posemath.toMsg(camera_pose)) for camera_id, camera_pose in camera_poses.iteritems()]
print "Proceed to optimization...\n"
# run optimization
self.state = estimate.enhance(self.meas, self.state)
print "\nOptimized!\n"
# publish calibration state
res = CameraCalibration() ## initialize a new Message instance
res.camera_pose = [camera.pose for camera in self.state.cameras]
res.camera_id = [camera.camera_id for camera in self.state.cameras]
res.time_stamp = [timestamp for timestamp in self.timestamps] #copy
res.m_count = len(res.time_stamp); # self.measurement_count
self.pub.publish(res)
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)
#print "Jacobian:\n", J
#print "Residual:\n", reshape(residual, [-1,2])
#step = linalg.pinv(J)*residual
#print "Unscaled Step:\n", reshape(step, [-1, 6]).T
#print "RMS: ", estimate.rms(residual)
#next_estimate = estimate.oplus(cal_estimate, step)
#print "Next Estimate:\n", next_estimate
BAG = '/u/vpradeep/kinect_bags/kinect_extrinsics_2011-04-05-16-01-28.bag'
bag = rosbag.Bag(BAG)
for topic, msg, t in bag:
assert topic == 'robot_measurement'
cal_samples = [msg for topic, msg, t in bag]
# create prior
camera_poses, checkerboard_poses = init_optimization_prior.find_initial_poses(cal_samples)
cal_estimate = CalibrationEstimate()
cal_estimate.targets = [ posemath.toMsg(checkerboard_poses[i]) for i in range(len(checkerboard_poses)) ]
cal_estimate.cameras = [ CameraPose(camera_id, posemath.toMsg(camera_pose)) for camera_id, camera_pose in camera_poses.iteritems()]
# Run optimization
new_cal_estimate = estimate.enhance(cal_samples, cal_estimate)
cam_dict_list = dump_estimate.to_dict_list(new_cal_estimate.cameras)
# For now, hardcode what transforms we care about
tf_config = dict();
tf_config['camera_a'] = {'calibrated_frame':'camera_a/openni_rgb_optical_frame',
'parent_frame': 'world_frame',
'child_frame': 'camera_a/openni_camera'}
tf_config['camera_b'] = {'calibrated_frame':'camera_b/openni_rgb_optical_frame',
'parent_frame': 'world_frame',
'child_frame': 'camera_b/openni_camera'}
# Insert TF Data into output file
for cam_dict in cam_dict_list:
cam_id = cam_dict['camera_id']
if cam_id in tf_config: