コード例 #1
0
    def publish_one_transform(self):
        t = rospy.Time.now()        
        try:
            self.listener.waitForTransform(
                self.ref_frame, self.orig_frame, t, rospy.Duration(1.0))
            (trans, rot) = self.listener.lookupTransform(
                self.ref_frame, self.orig_frame, t)
            base_H_cam = np.matrix(self.listener.fromTranslationRotation(trans, rot))
        except tf.Exception as ex:
            rospy.logerr('%s: Unable to get transform' % self.name)
            return

        # cache the last corrected transform to save on computation
        if (not self.prev_base_H_cam == None) and np.sum(np.abs(self.prev_base_H_cam - base_H_cam)) < 1e-10:
            base_H_ccam = self.prev_base_H_ccam
        else:
            base_H_ccam = self.correcter.get_corrected_transform(base_H_cam)
            self.prev_base_H_cam = base_H_cam
            self.prev_base_H_ccam = base_H_ccam
            
        transform_msg = ros_util.matrix_to_transform(base_H_ccam)
        trans = transform_msg.translation
        rot = transform_msg.rotation

        # broadcast the corrected transform
        self.broadcaster.sendTransform(
            (trans.x, trans.y, trans.z), (rot.x, rot.y, rot.z, rot.w), t, self.corrected_frame, self.ref_frame)
コード例 #2
0
opt = FCOptimizer(frames, params.tf_target_points, est_base_H_target, est_cam_H_neck, fix_target_transform=True,
                  W=np.diag([1.0, 1.0, 10.0]))
new_est_base_H_target, new_est_cam_H_neck = opt.optimize()

opt.print_stats()

print np.dot(linalg.inv(new_est_cam_H_neck), est_cam_H_neck)

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)

fcviz.draw_frames(upright_frames, est_cam_H_neck, 'new_initial_camera_poses', initial_color, cam_mark_lines=False)

fcviz.draw_target(est_base_H_target, 'estimated_target_pose', true_color)
fcviz.draw_frames(upright_frames, new_est_cam_H_neck, 'new_estimated_head_poses', estimated_color, cam_mark_lines=False)

# write out the resulting cal file to a urdf
urdf_path = os.path.join(
    roslib.packages.get_pkg_dir("arm_robot_model"), "models/sensorsValues.urdf.xacro")
print 'Cal Successful! Writing result to URDF to %s' % urdf_path
bb_left_neck_tf = ros_util.matrix_to_transform(linalg.inv(new_est_cam_H_neck))
update_sensors_values_urdf(urdf_path, bb_left_neck_tf, None, None, None)

while not rospy.is_shutdown():
    fcviz.update()
    rospy.sleep(0.1)