def estimate_callback(req):
global image_data
global state_data
global imu_data
global objpoints
global imgpoints
global db
global obs_n
global cov_n
global good_corners
global last_frame_corners
global goodenough
global num_step
global entropy
rospy.loginfo(rospy.get_caller_id() + 'I heard image %s', len(image_data))
rospy.loginfo(rospy.get_caller_id() + 'I heard imu %s', len(imu_data))
rospy.loginfo(rospy.get_caller_id() + 'I heard state %s', len(state_data))
local_img_data = image_data
t = 0
# indicate camera model
# PINHOLE = 0
# FISHEYE = 1
#cam_model = 0
if req.reset == 1:
#clear all the record data
del image_data[:]
del imu_data[:]
del state_data[:]
del objpoints[:]
del imgpoints[:]
del good_corners[:]
del db[:]
obs_n = 0
num_step = 0
last_frame_corners = None
goodenough = False
#feed back the update
res = estimateSrvResponse()
res.par_upd = [0, 0, 0, 0, 0, 0, 0, 0]
res.obs = 0
res.coverage = 0
return res
if not req.reset == 1:
num_step += 1
if len(image_data) == 0:
res = estimateSrvResponse()
res.par_upd = [0, 0, 0, 0, 0, 0, 0, 0]
res.obs = 0
res.coverage = 0
return res
# estimation
# camera intrinsic calibration
# imgpoints, best_mtx = camera_intrinsic_calibration(req, image_data)
reproj_err = 1
try:
best_mtx, ipts, opts, progress, reproj_err = camera_intrinsic_calibration2(
req, image_data)
rospy.loginfo(rospy.get_caller_id() + 'I get parameters %s',
best_mtx[0, 0])
except:
pass
if num_step >= 4:
spawn_model(0) # remove old
time.sleep(2)
spawn_model(1) # generate new
time.sleep(5)
# compute the coverage
rospy.loginfo(rospy.get_caller_id() + 'I get good corners %s',
len(good_corners))
if len(good_corners) > 0:
res = estimateSrvResponse()
for c, b in good_corners:
imgpoints.append(c) # just for coverage calculation.
rospy.loginfo(rospy.get_caller_id() + 'I get corners %s',
len(imgpoints))
####progress measures camera coverage
res.coverage = np.sum(progress) - cov_n
cov_n = np.sum(progress)
# get parameter update
# compute the observation
res = compute_obsevation(res, imu_data)
res.par_upd = [0, 0, 0, 0]
res.obs = 1.0 * len(db) / 2.0 - obs_n
if num_step == 4:
res.par_upd = [
best_mtx[0, 0], best_mtx[1, 1], best_mtx[0, 2], best_mtx[1,
2]
]
#add reproj
#res.obs+=5*(1-reproj_err)
res.par_upd += list(progress)
obs_n = 1.0 * len(db) / 2.0
rospy.loginfo(rospy.get_caller_id() + 'I get db %s', len(db))
rospy.loginfo(rospy.get_caller_id() + 'I get par_upd shape %s',
len(res.par_upd))
rospy.loginfo(rospy.get_caller_id() + 'I get total cov %s',
np.sum(progress))
rospy.loginfo(rospy.get_caller_id() + 'I get reproj err %s',
reproj_err)
return res
else:
res = estimateSrvResponse()
res.obs = 0
res.coverage = 0
res.par_upd = [0, 0, 0, 0, 0, 0, 0, 0]
return res
def estimate_callback(req):
global image_data
global state_data
global imu_data
global objpoints
global imgpoints
global db
global obs_n
global cov_n
global good_corners
global last_frame_corners
global goodenough
global num_step
global last_frame_corners
global opts
global ipts
global record
global entropy
global img_cov
rospy.loginfo(rospy.get_caller_id() + 'I heard image %s', len(image_data))
rospy.loginfo(rospy.get_caller_id() + 'I heard imu %s', len(imu_data))
rospy.loginfo(rospy.get_caller_id() + 'I heard state %s', len(state_data))
record = 0
#stop record
if req.reset == 1:
#stop record
#stop_recording()
#remove history data
os.chdir(kalibr_path[:-1])
os.system("rm data.bag")
os.system("rm data_copy.bag")
os.system("rm data_tmp.bag")
# bag = rosbag.Bag('data_tmp.bag', 'w')
# bag.close()
#clear all the record data
image_data[:] = []
del imu_data[:]
del state_data[:]
del objpoints[:]
del imgpoints[:]
del good_corners[:]
del db[:]
del opts[:]
del ipts[:]
obs_n = 0
last_frame_corners = None
goodenough = False
num_step = 0
entropy = 0
img_cov = 0
#feed back the update
res = estimateSrvResponse()
res.par_upd = [0, 0, 0, 0, 0, 0]
res.obs = 0
res.coverage = 0
#start recording
#start_recording()
return res
if not req.reset == 1:
num_step += 1
rospy.loginfo(rospy.get_caller_id() + 'start estimate')
res = estimateSrvResponse()
res.par_upd = [0, 0, 0, 0, 0, 0]
res.obs = 0
res.coverage = 0
# estimation
# camera intrinsic calibration
#imgpoints, best_mtx = camera_intrinsic_calibration(req, image_data)
best_mtx, ipts, opts, progress = camera_intrinsic_calibration2(
req, image_data)
rospy.loginfo(rospy.get_caller_id() + 'I get parameters %s',
best_mtx[0, 0])
#merge bags
#stop_recording()
rospy.loginfo(rospy.get_caller_id() + 'before waiting')
time.sleep(2)
merge_bag(num_step)
#cam imu calibration
reproj_err = 1
extrinsic = np.identity(4)
H_Aopt = 0
H_eig = np.zeros((6, ))
if num_step == 3 and len(ipts) > 0:
#call kalibr
os.chdir(kalibr_path[:-1])
try_kalibr_total()
reproj_err, extrinsic = get_kalibr_results()
extrinsic = np.asarray(extrinsic)
H_Aopt, H_Dopt, H_Eopt, H_eig = read_H()
#
#
# start = time.time()
# if num_step<3:
# #get reprojection error and extrinsics
# os.chdir(kalibr_path[:-1])
# try_kalibr()
# reproj_err, extrinsic = get_kalibr_results()
# extrinsic = np.asarray(extrinsic)
# # compute observability
# H_Aopt, H_Dopt, H_Eopt, H_eig = read_H()
# rospy.loginfo(rospy.get_caller_id() + 'time taken: '+str(time.time()-start))
########not calculate observability
# else:
# os.system(k_command)
##check for leakage
all_objects = muppy.get_objects()
sum1 = summary.summarize(all_objects)
# Prints out a summary of the large objects
summary.print_(sum1)
gc.collect()
#os.system("sudo sysctl -w vm.drop_caches=3")
rospy.loginfo(rospy.get_caller_id() + ' ' + 'get kalibr reproj err %s',
reproj_err)
#process extrinsic
e_flat = extrinsic[0:3, 0:3].flatten()
rotation = PyKDL.Rotation(e_flat[0], e_flat[1], e_flat[2], e_flat[3],
e_flat[4], e_flat[5], e_flat[6], e_flat[7],
e_flat[8])
rpy = np.asarray(rotation.GetRPY())
position = extrinsic[0:3, 3]
state = np.concatenate([position.reshape(3, ), rpy.reshape(3, )])
rospy.loginfo(rospy.get_caller_id() + ' ' + 'get kalibr extrinsic %s',
state)
# compute entropy
orientation_x_en, orientation_y_en, orientation_z_en, orientation_w_en, angular_velocity_x_en, angular_velocity_y_en, angular_velocity_z_en, linear_acceleration_x_en, linear_acceleration_y_en, linear_acceleration_z_en = compute_imu_entropy(
req, imu_data)
# # compute the coverage
# if len(good_corners) > 0:
res = estimateSrvResponse()
for c, b in good_corners:
imgpoints.append(c) # just for coverage calculation.
rospy.loginfo(rospy.get_caller_id() + 'I get corners %s', len(ipts))
# ####progress measures camera coverage
# res.coverage = np.sum(progress) - cov_n
# cov_n = np.sum(progress)
res.obs = (len(ipts) - obs_n) / float(max([len(image_data), 1.0]))
obs_n = len(ipts)
img_cov += res.obs
# res.obs = 1.0 * len(db) / 10.0 - obs_n
# obs_n = 1.0 * len(db) / 10.0
# get parameter update
# compute the observation
rospy.loginfo(rospy.get_caller_id() + 'I get image coverage %s',
img_cov)
res.obs += -H_Aopt * 10000000
#add reproj
res.obs += (1 - reproj_err) * 2
res.par_upd = np.concatenate([state, np.asarray(H_eig) * 10000000])
res.coverage = (orientation_x_en + orientation_w_en +
orientation_y_en + orientation_z_en) * 0.1
rospy.loginfo(rospy.get_caller_id() + 'I get par_upd %s', res.par_upd)
rospy.loginfo(rospy.get_caller_id() + 'I get obs %s', res.obs)
rospy.loginfo(rospy.get_caller_id() + 'I get coverage %s',
res.coverage)
entropy += res.coverage
rospy.loginfo(rospy.get_caller_id() + 'I get total entropy %s',
entropy)
rospy.loginfo(rospy.get_caller_id() + 'I get reproj err %s',
reproj_err)
# res.obs = 1.0 * len(db) / 20.0 - obs_n
# obs_n = 1.0 * len(db) / 20.0
# rospy.loginfo(rospy.get_caller_id() + 'I get db %s', len(db))
# rospy.loginfo(rospy.get_caller_id() + 'I get good corners %s',
# len(good_corners))
#start_recording()
# if num_step >=3:
# spawn_model(0) # remove old
# spawn_model(1) # generate new
return res
def estimate_callback(req):
global image_data
global state_data
global imu_data
global objpoints
global imgpoints
global db
global obs_n
global cov_n
global good_corners
global last_frame_corners
global goodenough
global num_step
global last_frame_corners
global opts
global ipts
global record
rospy.loginfo(rospy.get_caller_id() + 'I heard image %s', len(image_data))
rospy.loginfo(rospy.get_caller_id() + 'I heard imu %s', len(imu_data))
rospy.loginfo(rospy.get_caller_id() + 'I heard state %s', len(state_data))
record = 0
#stop record
if req.reset == 1:
#stop record
#stop_recording()
#remove history data
os.chdir(kalibr_path[:-1])
os.system("rm data.bag")
os.system("rm data_copy.bag")
os.system("rm data_tmp.bag")
# bag = rosbag.Bag('data_tmp.bag', 'w')
# bag.close()
#clear all the record data
image_data[:] = []
del imu_data[:]
del state_data[:]
del objpoints[:]
del imgpoints[:]
del good_corners[:]
del db[:]
del opts[:]
del ipts[:]
obs_n = 0
last_frame_corners = None
goodenough = False
num_step = 0
#feed back the update
res = estimateSrvResponse()
res.par_upd = np.zeros(20)
res.obs = 0
res.coverage = 0
#start recording
#start_recording()
return res
if not req.reset == 1:
num_step += 1
rospy.loginfo(rospy.get_caller_id() + 'start estimate')
res = estimateSrvResponse()
res.par_upd = np.zeros((20, ))
res.obs = 0
res.coverage = 0
# estimation
# camera intrinsic calibration
# imgpoints, best_mtx = camera_intrinsic_calibration(req, image_data)
best_mtx, ipts, opts, cam_progress = camera_intrinsic_calibration2(
req, image_data)
# rospy.loginfo(rospy.get_caller_id() + 'I get parameters %s',
# best_mtx[0, 0])
#distance from ground truth
cam_int_truth = rospy.get_param("/rl_client/cam_ground_truth")
cal_int = [
best_mtx[0, 0], best_mtx[1, 1], best_mtx[0, 2], best_mtx[1, 2]
]
cam_err = np.linalg.norm(
np.asarray(cam_int_truth) - np.asarray(cal_int)) / 100
#camera update response:
cam_par_upd = np.zeros((4, ))
cam_obs = 0
cam_coverage = 0
if len(good_corners) > 0:
for c, b in good_corners:
imgpoints.append(c) # just for coverage calculation.
rospy.loginfo(rospy.get_caller_id() + 'I get corners %s',
len(imgpoints))
cam_par_upd = np.asarray(cal_int)
cam_obs = 1.0 * len(db) / 5.0 - obs_n
obs_n = 1.0 * len(db) / 5.0
cam_coverage = (np.sum(cam_progress) - cov_n) * 2
cov_n = np.sum(cam_progress)
cam_progress = cam_progress[-4:]
#merge bags
time.sleep(2)
merge_bag(num_step)
#if camera calibration good, do imu_calibration
imu_par_upd = np.zeros((6, ))
imu_obs = 0
imu_coverage = 0
imu_progress = np.zeros((6, ))
if cam_err < 0.05:
imu_par_upd, imu_progress, imu_obs, imu_coverage = imu_calibration(
num_step)
# # compute the coverage
# if len(good_corners) > 0:
res = estimateSrvResponse()
# get parameter update
# compute the observation
res.obs = cam_obs + imu_obs
res.par_upd = np.concatenate(
[imu_par_upd, cam_par_upd, imu_progress, cam_progress])
#add camera err here
res.coverage = imu_coverage + cam_coverage
rospy.loginfo(rospy.get_caller_id() + 'I get par_upd %s', res.par_upd)
rospy.loginfo(rospy.get_caller_id() + 'I get cam obs %s', cam_obs)
rospy.loginfo(rospy.get_caller_id() + 'I get cam coverage %s',
cam_coverage)
rospy.loginfo(rospy.get_caller_id() + 'I get imu obs %s', imu_obs)
rospy.loginfo(rospy.get_caller_id() + 'I get imu coverage %s',
imu_coverage)
# res.obs = 1.0 * len(db) / 20.0 - obs_n
# obs_n = 1.0 * len(db) / 20.0
rospy.loginfo(rospy.get_caller_id() + 'I get db %s', len(db))
rospy.loginfo(rospy.get_caller_id() + 'I get good corners %s',
len(good_corners))
#start_recording()
return res
def estimate_callback(req):
global image_data
global state_data
global imu_data
global objpoints
global imgpoints
global db
global obs_n
global cov_n
global good_corners
global last_frame_corners
global goodenough
rospy.loginfo(rospy.get_caller_id() + 'I heard image %s', len(image_data))
rospy.loginfo(rospy.get_caller_id() + 'I heard imu %s', len(imu_data))
rospy.loginfo(rospy.get_caller_id() + 'I heard state %s', len(state_data))
local_img_data = image_data
t = 0
if req.reset == 1:
#clear all the record data
del image_data[:]
del imu_data[:]
del state_data[:]
del objpoints[:]
del imgpoints[:]
del good_corners[:]
del db[:]
obs_n = 0
last_frame_corners = None
goodenough = False
#feed back the update
res = estimateSrvResponse()
res.par_upd = [0, 0, 0, 0]
res.obs = 0
res.coverage = 0
return res
if not req.reset == 1:
if len(image_data) == 0:
res = estimateSrvResponse()
res.par_upd = [0, 0, 0, 0]
res.obs = 0
res.coverage = 0
return res
# estimation
# camera intrinsic calibration
# imgpoints, best_mtx = camera_intrinsic_calibration(req, image_data)
best_mtx, ipts, opts, progress = camera_intrinsic_calibration2(
req, image_data)
rospy.loginfo(rospy.get_caller_id() + 'I get parameters %s',
best_mtx[0, 0])
# compute the coverage
if len(good_corners) > 0:
res = estimateSrvResponse()
for c, b in good_corners:
imgpoints.append(c) # just for coverage calculation.
rospy.loginfo(rospy.get_caller_id() + 'I get corners %s',
len(imgpoints))
####progress measures camera coverage
res.coverage = np.sum(progress) - cov_n
cov_n = np.sum(progress)
# get parameter update
# compute the observation
res = compute_obsevation(res, imu_data)
res.par_upd = [
best_mtx[0, 0], best_mtx[1, 1], best_mtx[0, 2], best_mtx[1, 2]
]
res.obs = 1.0 * len(db) / 2.0 - obs_n
obs_n = 1.0 * len(db) / 2.0
rospy.loginfo(rospy.get_caller_id() + 'I get db %s', len(db))
rospy.loginfo(rospy.get_caller_id() + 'I get good corners %s',
len(good_corners))
return res
else:
res = estimateSrvResponse()
res.obs = 0
res.coverage = 0
res.par_upd = [0, 0, 0, 0]
return res
def estimate_callback(req):
global image_data
global state_data
global imu_data
global objpoints
global imgpoints
global db
global obs_n
global cov_n
global good_corners
global last_frame_corners
global goodenough
global num_step
rospy.loginfo(rospy.get_caller_id() + 'I heard image %s', len(image_data))
rospy.loginfo(rospy.get_caller_id() + 'I heard imu %s', len(imu_data))
rospy.loginfo(rospy.get_caller_id() + 'I heard state %s', len(state_data))
local_img_data = image_data
t = 0
#stop record
if req.reset == 1:
#remove history data
os.chdir(kalibr_path[:-1])
os.system("rm data.bag")
os.system("rm data_copy.bag")
#os.system("rm data_tmp.bag")
#start record
#stop record
rospy.wait_for_service('record_topics')
stop_bag_record = rospy.ServiceProxy('stop_recording', StopRecording)
resp1 = stop_bag_record("data.bag")
rospy.wait_for_service('record_topics')
bag_record = rospy.ServiceProxy('record_topics', RecordTopics)
resp1 = bag_record("data.bag",
["/simple_camera/image_raw", "/imu_real"])
#clear all the record data
del image_data[:]
del imu_data[:]
del state_data[:]
del objpoints[:]
del imgpoints[:]
del good_corners[:]
del db[:]
obs_n = 0
last_frame_corners = None
goodenough = False
num_step = 0
#feed back the update
res = estimateSrvResponse()
res.par_upd = [0, 0, 0, 0, 0, 0]
res.obs = 0
res.coverage = 0
return res
if not req.reset == 1:
num_step += 1
rospy.loginfo(rospy.get_caller_id() + 'start estimate')
res = estimateSrvResponse()
res.par_upd = [0, 0, 0, 0, 0, 0]
res.obs = 0
res.coverage = 0
# estimation
# camera intrinsic calibration
# imgpoints, best_mtx = camera_intrinsic_calibration(req, image_data)
# best_mtx, ipts, opts, progress = camera_intrinsic_calibration2(
# req, image_data)
# rospy.loginfo(rospy.get_caller_id() + 'I get parameters %s',
# best_mtx[0, 0])
#cam_imu calibration
# rospy.wait_for_service('stop_recording')
# stop_bag_record = rospy.ServiceProxy('stop_recording', StopRecording)
# #resp1 = stop_bag_record("data_tmp.bag")
# #merge bags
# for root, dirs, files in os.walk(kalibr_path):
# if "data.bag" in files:
# break
# bag = rosbag.Bag('data.bag', 'w')
# bag.close()
#
# for root, dirs, files in os.walk(kalibr_path):
# if "data_tmp.bag" in files:
# break
# bag = rosbag.Bag('data_tmp.bag', 'w')
# bag.close()
#
# #copy current data
# os.chdir(kalibr_path[:-1])
# print(kalibr_path[:-1])
# os.system("cp data.bag data_copy.bag")
# #merge bag
# with rosbag.Bag(kalibr_path + 'data.bag', 'w') as outbag:
# for topic, msg, t in rosbag.Bag(kalibr_path +
# 'data_copy.bag').read_messages():
# outbag.write(topic, msg, t)
# for topic, msg, t in rosbag.Bag(kalibr_path +
# 'data_tmp.bag').read_messages():
# outbag.write(topic, msg, t)
reproj_err = 1
extrinsic = np.identity(4)
if num_step == 5:
#stop record
rospy.wait_for_service('stop_recording')
stop_bag_record = rospy.ServiceProxy('stop_recording', StopRecording)
resp1 = stop_bag_record("data.bag")
#call kalibr
os.system(k_command_total)
#start again
rospy.wait_for_service('record_topics')
bag_record = rospy.ServiceProxy('record_topics', RecordTopics)
resp1 = bag_record("data.bag",
["/simple_camera/image_raw", "/imu_real"])
#get reprojection error and extrinsics
reproj_err, extrinsic = get_kalibr_results()
extrinsic = np.asarray(extrinsic)
########not calculate observability
# else:
# os.system(k_command)
rospy.loginfo(
rospy.get_caller_id() + ' ' + 'get kalibr reproj err %s',
reproj_err)
rospy.loginfo(
rospy.get_caller_id() + ' ' + 'get kalibr extrinsic %s',
extrinsic)
e_flat = extrinsic[0:3,0:3].flatten()
rotation = PyKDL.Rotation(e_flat[0],e_flat[1],e_flat[2],e_flat[3],e_flat[4],e_flat[5],e_flat[6],e_flat[7],e_flat[8])
rpy = np.asarray(rotation.GetRPY())
position = extrinsic[0:3,3]
state = np.concatenate([position.reshape(3,),rpy.reshape(3,)])
# compute observability
H_txt_path = kalibr_path+'H.txt'
H_Aopt, H_Dopt, H_Eopt, H_eig = compute_optimality(H_txt_path)
# compute entropy
orientation_x_en, orientation_y_en, orientation_z_en, orientation_w_en, angular_velocity_x_en, angular_velocity_y_en, angular_velocity_z_en, linear_acceleration_x_en, linear_acceleration_y_en, linear_acceleration_z_en = compute_imu_entropy(
req, imu_data)
# # compute the coverage
# if len(good_corners) > 0:
res = estimateSrvResponse()
# for c, b in good_corners:
# imgpoints.append(c) # just for coverage calculation.
# rospy.loginfo(rospy.get_caller_id() + 'I get corners %s',
# len(imgpoints))
# ####progress measures camera coverage
# res.coverage = np.sum(progress) - cov_n
# cov_n = np.sum(progress)
# get parameter update
# compute the observation
res.obs = H_Aopt*100000000
res.par_upd = np.concatenate([state,np.asarray(H_eig)*100000000])
res.coverage = (orientation_x_en+orientation_w_en+orientation_y_en+orientation_z_en)*0.1
rospy.loginfo(rospy.get_caller_id() + 'I get par_upd %s', res.par_upd)
rospy.loginfo(rospy.get_caller_id() + 'I get obs %s',
res.obs)
rospy.loginfo(rospy.get_caller_id() + 'I get coverage %s',
res.coverage)
# res.obs = 1.0 * len(db) / 20.0 - obs_n
# obs_n = 1.0 * len(db) / 20.0
# rospy.loginfo(rospy.get_caller_id() + 'I get db %s', len(db))
# rospy.loginfo(rospy.get_caller_id() + 'I get good corners %s',
# len(good_corners))
return res
def estimate_callback(req):
global image_data
global state_data
global imu_data
global objpoints
global imgpoints
global db
global obs_n
global cov_n
global good_corners
global last_frame_corners
global goodenough
global num_step
rospy.loginfo(rospy.get_caller_id() + 'I heard image %s', len(image_data))
rospy.loginfo(rospy.get_caller_id() + 'I heard imu %s', len(imu_data))
rospy.loginfo(rospy.get_caller_id() + 'I heard state %s', len(state_data))
local_img_data = image_data
t = 0
#stop record
if req.reset == 1:
#remove history data
os.chdir(kalibr_path[:-1])
os.system("rm data.bag")
os.system("rm data_copy.bag")
rospy.wait_for_service('record_topics')
stop_bag_record = rospy.ServiceProxy('stop_recording', StopRecording)
resp1 = stop_bag_record("data_tmp.bag")
#start rosbag record
rospy.wait_for_service('record_topics')
bag_record = rospy.ServiceProxy('record_topics', RecordTopics)
resp1 = bag_record("data_tmp.bag",
["/simple_camera/image_raw", "/imu_real"])
#clear all the record data
del image_data[:]
del imu_data[:]
del state_data[:]
del objpoints[:]
del imgpoints[:]
del good_corners[:]
del db[:]
obs_n = 0
last_frame_corners = None
goodenough = False
num_step = 0
#feed back the updateros
res = estimateSrvResponse()
res.par_upd = np.zeros((18, ))
res.obs = 0
res.coverage = 0
return res
if not req.reset == 1:
num_step += 1
rospy.loginfo(rospy.get_caller_id() + 'start estimate')
res = estimateSrvResponse()
res.par_upd = np.zeros((18, ))
res.obs = 0
res.coverage = 0
# estimation
# camera intrinsic calibration
# imgpoints, best_mtx = camera_intrinsic_calibration(req, image_data)
# best_mtx, ipts, opts, progress = camera_intrinsic_calibration2(
# req, image_data)
# rospy.loginfo(rospy.get_caller_id() + 'I get parameters %s',
# best_mtx[0, 0])
#stop record tmp
rospy.wait_for_service('record_topics')
stop_bag_record = rospy.ServiceProxy('stop_recording', StopRecording)
resp1 = stop_bag_record("data_tmp.bag")
rospy.loginfo(rospy.get_caller_id() + 'finish recording')
for root, dirs, files in os.walk(kalibr_path):
if "data.bag" in files:
break
bag = rosbag.Bag('data.bag', 'w')
bag.close()
#copy current data
os.chdir(kalibr_path[:-1])
print(kalibr_path[:-1])
os.system("cp data.bag data_copy.bag")
#merge bag
with rosbag.Bag(kalibr_path + 'data.bag', 'w') as outbag:
for topic, msg, t in rosbag.Bag(kalibr_path +
'data_copy.bag').read_messages():
outbag.write(topic, msg, t)
for topic, msg, t in rosbag.Bag(kalibr_path +
'data_tmp.bag').read_messages():
outbag.write(topic, msg, t)
#start tmp again
rospy.wait_for_service('record_topics')
bag_record = rospy.ServiceProxy('record_topics', RecordTopics)
resp1 = bag_record("data_tmp.bag",
["/simple_camera/image_raw", "/imu_real"])
#cam_imu calibration
extrinsic = np.zeros((6, ))
reproj_err = 1
if num_step == 3:
#call kalibr
os.system(k_command_total)
#get reprojection error and extrinsics
reproj_err, extrinsic = get_kalibr_results()
else:
os.system(k_command)
rospy.loginfo(rospy.get_caller_id() + ' ' + 'get kalibr reproj err %s',
reproj_err)
rospy.loginfo(rospy.get_caller_id() + ' ' + 'get kalibr extrinsic %s',
extrinsic)
# compute observability
H_txt_path = '/home/felix/panda_ws/src/franka_cal_sim/python/kalibr/H.txt'
H_Aopt, H_Dopt, H_Eopt, H_eig = compute_optimality(H_txt_path)
# compute entropy
orientation_x_en, orientation_y_en, orientation_z_en, orientation_w_en, angular_velocity_x_en, angular_velocity_y_en, angular_velocity_z_en, linear_acceleration_x_en, linear_acceleration_y_en, linear_acceleration_z_en = compute_imu_entropy(
req, imu_data)
# # compute the coverage
# if len(good_corners) > 0:
res = estimateSrvResponse()
# for c, b in good_corners:
# imgpoints.append(c) # just for coverage calculation.
# rospy.loginfo(rospy.get_caller_id() + 'I get corners %s',
# len(imgpoints))
# ####progress measures camera coverage
# res.coverage = np.sum(progress) - cov_n
# cov_n = np.sum(progress)
# get parameter update
# compute the observation
res.obs = H_Aopt * 100000000
res.par_upd = np.concatenate([
np.asarray(extrinsic).reshape((12, )),
np.asarray(H_eig * 100000000)
])
res.coverage = (orientation_x_en + orientation_w_en +
orientation_y_en + orientation_z_en) * 0.01
rospy.loginfo(rospy.get_caller_id() + 'I get par_upd %s', res.par_upd)
rospy.loginfo(rospy.get_caller_id() + 'I get obs %s', res.obs)
rospy.loginfo(rospy.get_caller_id() + 'I get coverage %s',
res.coverage)
# res.obs = 1.0 * len(db) / 20.0 - obs_n
# obs_n = 1.0 * len(db) / 20.0
# rospy.loginfo(rospy.get_caller_id() + 'I get db %s', len(db))
# rospy.loginfo(rospy.get_caller_id() + 'I get good corners %s',
# len(good_corners))
return res
def estimate_callback(req):
global image_data
global state_data
global imu_data
global objpoints
global imgpoints
rospy.loginfo(rospy.get_caller_id() + 'I heard image %s', len(image_data))
rospy.loginfo(rospy.get_caller_id() + 'I heard imu %s', len(imu_data))
rospy.loginfo(rospy.get_caller_id() + 'I heard state %s', len(state_data))
local_img_data = image_data
t = 0
if req.reset==1:
#clear all the record data
global obs_n
del image_data[:]
del imu_data[:]
del state_data[:]
del objpoints[:]
del imgpoints[:]
max_x = 0
max_y = 0
min_x = 0
min_y = 0
obs_n = 0
#feed back the update
res = estimateSrvResponse()
res.par_upd=[]
res.obs = 0
res.coverage = 0
return res
if not req.reset==1:
if len(image_data)==0:
res = estimateSrvResponse()
res.par_upd=[0,0,0,0]
res.obs = 0
res.coverage = 0
return res
##estimation
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
criteria_cal = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((6*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)
rospy.loginfo(rospy.get_caller_id() + 'get corner')
for img in local_img_data:
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (7,6),flags=cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_FAST_CHECK)
# If found, add object points, image points (after refining them)
if ret == True and len(corners)<50 and len(local_img_data)>30:
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray,corners,(9,9),(-1,-1),criteria)
imgpoints.append(corners2)
rospy.loginfo(rospy.get_caller_id() + 'compute intrinsics')
list1 = np.arange(0,len(imgpoints),1)
mtx = np.zeros((3,3))
if len(req.params)>3:
mtx[0,0] = req.params[0]
mtx[1,1] = req.params[1]
mtx[0,2] = req.params[2]
mtx[1,2] = req.params[3]
mtx[2,2] = 1
#optimize data step by step based on sampled imgs, get best one
min_error = 1000
best_mtx = mtx
if len(imgpoints)<200 and len(local_img_data)>30:
for i in range(1):
cur_data = list1
if len(imgpoints)>40:
cur_data = sample(list1,40)
cur_obj = list(objpoints[i] for i in cur_data)
cur_img = list(imgpoints[i] for i in cur_data)
try:
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(cur_obj, cur_img, gray.shape[::-1],cameraMatrix=mtx, distCoeffs=None, rvecs=None, tvecs=None, flags=0, criteria=criteria_cal)
except:
rvecs = np.asarray([1.0,0,0])
tvecs = np.asarray([0,0,0])
dist = np.asarray([0,0,0,0,0])
rospy.loginfo(rospy.get_caller_id() + 'No image')
# #evaluate
# tot_error = 0
# mean_error = 0
# for j in range(len(cur_obj)):
# imgpoints2, _ = cv2.projectPoints(cur_obj[j], rvecs[j], tvecs[j], mtx, dist)
# error = cv2.norm(cur_img[j],imgpoints2, cv2.NORM_L2)/len(imgpoints2)
# tot_error += error
# mean_error = tot_error/max([len(cur_obj),1])
# if mean_error<min_error:
# min_error = mean_error
# best_mtx = mtx
best_mtx = mtx
rospy.loginfo(rospy.get_caller_id() + 'I get corners %s', len(imgpoints))
rospy.loginfo(rospy.get_caller_id() + 'I get parameters %s', best_mtx)
##compute the results
#compute the coverage
rospy.loginfo(rospy.get_caller_id() + 'send back')
if len(imgpoints)>0:
res = estimateSrvResponse()
global max_x
global max_y
global min_x
global min_y
global obs_n
img_flat = np.reshape(np.asarray(imgpoints),(-1,3))
res.coverage = np.max(img_flat,axis = 0)[0]-max_x-np.min(img_flat,axis = 0)[0]+min_x
res.coverage = res.coverage+np.max(img_flat,axis = 0)[1]-max_y-np.min(img_flat,axis = 0)[0]+min_y
res.obs = 0
if(len(imgpoints)<100):
res.obs = 1.0*len(imgpoints)/len(local_img_data)
max_x = np.max(img_flat,axis = 0)[0]
max_y = np.max(img_flat,axis = 0)[1]
min_x = np.min(img_flat,axis = 0)[0]
min_y = np.min(img_flat,axis = 0)[1]
obs_n = res.obs
#get parameter update
res.par_upd = [best_mtx[0,0],best_mtx[1,1],best_mtx[0,2],best_mtx[1,2]]
return res
else:
res = estimateSrvResponse()
res.obs = 0
res.coverage = 0
res.par_upd = [0,0,0,0]
return res
