def dist_ray_plane(r, abcd):
"""
Finds the distance along a ray (or rays) at which it interescts with
the plane with coeffcients abcd.
r - the unit vector direction of the ray (assumed to start at the origin)
abcd - numpy array of the four plane coefficients: ax + by + cz + d = 0
"""
return -abcd[3] / np.dot(r, abcd[:3])
params = FCParams()
rospy.init_node("FiducialCal")
rospy.sleep(1.0)
fcviz = FCViz("/FiducialCal/markers", params.tf_target_points)
if len(sys.argv) > 1:
bag_file = sys.argv[1]
else:
bag_dir = roslib.packages.get_pkg_subdir("arm_calibrate_extrinsics", "data")
bag_file = os.path.join(bag_dir, "ar_target_frames.bag")
bag = rosbag.Bag(bag_file)
print "Loading data from %s" % bag_file
ros2cv = CvBridge()
# load the data from the bagfile
frames = []
for topic, msg, t in bag.read_messages():
f = FCFrame()
from matplotlib import pyplot as plt
from scipy import linalg, optimize
import rospy, rosbag
from tf import transformations
from cv_bridge import CvBridge
from pier import geom, ros_util, markers, store
from arm_fiducial_cal.urdf_utils import update_sensors_values_urdf
import arm_fiducial_cal.planes
from arm_fiducial_cal import FCOptimizer, FCFrame, FCViz, FCParams
params = FCParams()
rospy.init_node('FiducialCal')
rospy.sleep(1.0)
fcviz = FCViz('/FiducialCal/markers', params.tf_target_points)
# for our own purposes, save the estimated transform in a shelf
cache_dir = roslib.packages.get_pkg_subdir('arm_fiducial_cal', 'cache')
s = store.Store(cache_dir)
frames = s['frames']
est_base_H_target = s['est_base_H_target']
est_cam_H_neck = s['est_cam_H_neck']
upright_frames = []
for f_i, f in enumerate(frames):
bf_head_origin = geom.transform_points(np.zeros(3), linalg.inv(f.neck_H_base))
# hack to filter out poses where the lower pan-tilt isn't straight up
if bf_head_origin[2] > 1.7:
from arm_fiducial_cal import FCOptimizer, FCLoader, FCViz, FCParams, FCParametrizer, FCSingleFrameOpt, dist_ray_plane
if len(sys.argv) > 1:
bag_file = sys.argv[1]
else:
bag_dir = roslib.packages.get_pkg_subdir('arm_calibrate_extrinsics', 'data')
bag_file = os.path.join(bag_dir, 'ar_target_frames_3d.bag')
true_color = (1.0, 0.0, 1.0, 1.0)
initial_color = (0.0, 1.0, 1.0, 1.0)
estimated_color = (1.0, 1.0, 0.0, 1.0)
corrected_color = (0.0, 1.0, 0.0, 1.0)
params = FCParams()
rospy.init_node('FiducialCal')
fcviz = FCViz('/FiducialCal/markers', params.tf_target_points)
print ''
print '===================================================================='
print 'Loading data from %s' % bag_file
print '===================================================================='
loader = FCLoader(bag_file, params)
frames = loader.frames
for f_i in range(len(frames)):
markers_i = set([m_k for (m_k, p) in frames[f_i].visible_markers])
print 'Markers visible in frame %d:' % f_i, sorted(markers_i)
print ''
print '===================================================================='