def test_free(self): st = SingleTransform([0, 0, 0, 0, 0, 0]) free_list = st.calc_free([0, 0, 1, 1, 0, 0]) self.assertEqual(free_list[0], False) self.assertEqual(free_list[1], False) self.assertEqual(free_list[2], True) self.assertEqual(free_list[3], True) self.assertEqual(free_list[4], False) self.assertEqual(free_list[5], False)
def test_inflate(self): st = SingleTransform([0, 0, 0, 0, 0, 0]) st.inflate(reshape(matrix([1, 0, 0, 0, 0, 0], float), (-1, 1))) expected = numpy.matrix( [[1, 0, 0, 1], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], float) print "" print st.transform self.assertAlmostEqual(numpy.linalg.norm(st.transform - expected), 0.0, 6)
def test_free(self): st = SingleTransform([0, 0, 0, 0, 0, 0]) free_list = st.calc_free( [0, 0, 1, 1, 0, 0] ) self.assertEqual(free_list[0], False) self.assertEqual(free_list[1], False) self.assertEqual(free_list[2], True) self.assertEqual(free_list[3], True) self.assertEqual(free_list[4], False) self.assertEqual(free_list[5], False)
def test_inflate(self): st = SingleTransform([0, 0, 0, 0, 0, 0]) st.inflate( reshape( matrix([1, 0, 0, 0, 0, 0], float), (-1,1) )) expected = numpy.matrix( [[ 1, 0, 0, 1], [ 0, 1, 0, 0], [ 0, 0, 1, 0], [ 0, 0, 0, 1]], float ) print "" print st.transform self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6)
def calculate_error(self, opt_all_vec): print "x ", sys.stdout.flush() opt_param_vec, full_pose_arr = self.split_all(opt_all_vec) full_param_vec = self.calculate_full_param_vec(opt_param_vec) # Update the primitives with the new set of parameters self._robot_params.inflate(full_param_vec) # Update all the blocks' configs for multisensor in self._multisensors: multisensor.update_config(self._robot_params) r_list = [] for multisensor, cb_pose_vec in zip(self._multisensors, list(full_pose_arr)): # Process cb pose cb_points = SingleTransform( cb_pose_vec).transform * self._robot_params.checkerboards[ multisensor.checkerboard].generate_points() if (self._use_cov): r_list.append(multisensor.compute_residual_scaled(cb_points)) else: r_list.append(multisensor.compute_residual(cb_points)) r_vec = concatenate(r_list) rms_error = numpy.sqrt(numpy.mean(r_vec**2)) print "%.3f " % rms_error, sys.stdout.flush() return array(r_vec)
def test_hard(self): sample_nums = range(1, 6) dir_name = os.path.dirname(os.path.realpath(__file__)) params_filenames = [ os.path.join(dir_name, 'data', 'single_transform_data', 'params_%02u.txt' % n) for n in sample_nums ] transform_filenames = [ os.path.join(dir_name, 'data', 'single_transform_data', 'transform_%02u.txt' % n) for n in sample_nums ] for params_filename, transform_filename in zip(params_filenames, transform_filenames): f_params = open(params_filename) f_transforms = open(transform_filename) param_elems = [float(x) for x in f_params.read().split()] transform_elems = [float(x) for x in f_transforms.read().split()] st = SingleTransform(param_elems) expected_result = numpy.matrix(transform_elems) expected_result.shape = 4, 4 self.assertAlmostEqual( numpy.linalg.norm(st.transform - expected_result), 0.0, 4, "Failed on %s" % params_filename)
def test_hard(self): sample_nums = range(1, 6) params_filenames = [ "test/data/single_transform_data/params_%02u.txt" % n for n in sample_nums ] transform_filenames = [ "test/data/single_transform_data/transform_%02u.txt" % n for n in sample_nums ] for params_filename, transform_filename in zip(params_filenames, transform_filenames): f_params = open(params_filename) f_transforms = open(transform_filename) param_elems = [float(x) for x in f_params.read().split()] transform_elems = [float(x) for x in f_transforms.read().split()] st = SingleTransform(param_elems) expected_result = numpy.matrix(transform_elems) expected_result.shape = 4, 4 self.assertAlmostEqual( numpy.linalg.norm(st.transform - expected_result), 0.0, 4, "Failed on %s" % params_filename)
def multisensor_pose_jacobian(self, opt_param_vec, pose_param_vec, multisensor): """ Generates the jacobian from a target pose to the multisensor's residual. Input: - opt_param_vec: Vector of the free system parameters (the parameters we're actuall optimizing) - pose_param_vec: Vector of length 6 encoding the target's pose 0:3=translation 3:6=rotation_axis - multisensor: The actual multisensor definition. Output: - J_scaled: An mx6 jacobian, where m is the length of multisensor's residual. There are 6 columns since the target's pose is encoded using 6 parameters. If covariance calculations are enabled, then the Jacobian is scaled by sqrt(Gamma), where Gamma is the information matrix for this measurement. """ # Update the primitives with the new set of parameters full_param_vec = self.calculate_full_param_vec(opt_param_vec) self._robot_params.inflate(full_param_vec) multisensor.update_config(self._robot_params) cb_model = self._robot_params.checkerboards[multisensor.checkerboard] local_cb_points = cb_model.generate_points() # based on code from scipy.slsqp x0 = pose_param_vec epsilon = 1e-6 world_pts = SingleTransform(x0).transform * local_cb_points f0 = multisensor.compute_residual(world_pts) if (self._use_cov): gamma_sqrt = multisensor.compute_marginal_gamma_sqrt(world_pts) Jt = numpy.zeros([len(x0), len(f0)]) dx = numpy.zeros(len(x0)) for i in range(len(x0)): dx[i] = epsilon test_vec = x0 + dx fTest = multisensor.compute_residual( SingleTransform(test_vec).transform * local_cb_points) Jt[i] = (fTest - f0) / epsilon #import code; code.interact(local=locals()) dx[i] = 0.0 J = Jt.transpose() if (self._use_cov): J_scaled = gamma_sqrt * J else: J_scaled = J return J_scaled
def test_easy_rot_1(self): st = SingleTransform([0, 0, 0, 0, 0, pi / 2]) expected = numpy.matrix( [[0, -1, 0, 0], [1, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], float) print "" print st.transform self.assertAlmostEqual(numpy.linalg.norm(st.transform - expected), 0.0, 6)
def setUp(self): print "" config = ''' root: x tip: y joints: [j1, j2, j3] active_joints: [j1, j2, j3] axis: [6, 6, 6] gearing: [1, 1, 1] cov: joint_angles: [1, 1, 1] ''' config_dict = yaml.load(config) config_dict['transforms'] = { 'j1': SingleTransform([1, 0, 0, 0, 0, 0]), 'j2': SingleTransform([1, 0, 0, 0, 0, 0]), 'j3': SingleTransform([1, 0, 2, 0, 0, 0]) } self.chain = JointChain(config_dict)
def test_easy_trans_1(self): st = SingleTransform([1, 2, 3, 0, 0, 0]) expected = numpy.matrix( [[1, 0, 0, 1], [0, 1, 0, 2], [0, 0, 1, 3], [0, 0, 0, 1]], float) print "" print st.transform self.assertAlmostEqual(numpy.linalg.norm(st.transform - expected), 0.0, 6)
def compute_pose(self, joint_pos): pose = matrix(numpy.eye(4)) for before_chain_T in self._before_chain_Ts: pose = pose * before_chain_T.transform pose = pose * SingleTransform([ 0, 0, 0, 0, joint_pos[0] * self._gearing, 0 ]).transform # TODO: remove assumption of Y axis for after_chain_T in self._after_chain_Ts: pose = pose * after_chain_T.transform return pose
def compute_errors_breakdown(error_calc, multisensors, opt_pose_arr): errors_dict = {} # Compute the error for each sensor type for ms, k in zip(multisensors, range(len(multisensors))): cb = error_calc._robot_params.checkerboards[ms.checkerboard] cb_pose_vec = opt_pose_arr[k] target_pts = SingleTransform(cb_pose_vec).transform * cb.generate_points() for sensor in ms.sensors: r_sensor = sensor.compute_residual(target_pts) * numpy.sqrt(sensor.terms_per_sample) # terms per sample is a hack to find rms distance, instead of a pure rms, based on each term if sensor.sensor_id not in errors_dict.keys(): errors_dict[sensor.sensor_id] = [] errors_dict[sensor.sensor_id].append(r_sensor) return errors_dict
def calculate_error(self, opt_all_vec): opt_param_vec, full_pose_arr = self.split_all(opt_all_vec) full_param_vec = self.calculate_full_param_vec(opt_param_vec) # Update the primitives with the new set of parameters self._robot_params.inflate(full_param_vec) # Update all the blocks' configs for multisensor in self._multisensors: multisensor.update_config(self._robot_params) r_list = [] id = 0 for multisensor, cb_pose_vec in zip(self._multisensors, list(full_pose_arr)): # Process cb pose cb_points = SingleTransform(cb_pose_vec).transform * self._robot_params.checkerboards[multisensor.checkerboard].generate_points() if (self._use_cov): r_list.append(multisensor.compute_residual_scaled(cb_points)) else: r_list.append(multisensor.compute_residual(cb_points)) cb_points_msgs = [ geometry_msgs.msg.Point(cur_pt[0,0], cur_pt[0,1], cur_pt[0,2]) for cur_pt in cb_points.T] m = Marker() m.header.frame_id = self._robot_params.base_link m.ns = "points_3d" m.id = id m.type = Marker.SPHERE_LIST m.action = Marker.MODIFY m.points = cb_points_msgs m.color.r = 1.0 m.color.g = 0.0 m.color.b = 1.0 m.color.a = 1.0 m.scale.x = 0.01 m.scale.y = 0.01 m.scale.z = 0.01 self.marker_pub.publish(m) id += 1 r_vec = concatenate(r_list) rms_error = numpy.sqrt( numpy.mean(r_vec**2) ) print "\t\t\t\t\tRMS error: %.3f \r" % rms_error, sys.stdout.flush() return array(r_vec)
def test_params_to_config(self): st = SingleTransform() p = reshape(matrix([1, 0, 0, 0, 0, 0], float), (-1, 1)) config = st.params_to_config(p) self.assertAlmostEqual(config[0], 1, 6) self.assertAlmostEqual(config[1], 0, 6)
def test_deflate(self): st = SingleTransform([0, 0, 0, 0, 0, 0]) p = reshape(matrix([1, 0, 0, 0, 0, 0], float), (-1, 1)) st.inflate(p) result = st.deflate() self.assertAlmostEqual(numpy.linalg.norm(p - result), 0.0, 6)
def calculate_jacobian(self, opt_all_vec): """ Full Jacobian: The resulting returned jacobian can be thought of as a set of several concatenated jacobians as follows: [ J_multisensor_0 ] J = [ J_multisensor_1 ] [ : ] [ J_multisensor_M ] where M is the number of multisensors, which is generally the number of calibration samples collected. Multisensor Jacobian: The Jacobian for a given multisensor is created by concatenating jacobians for each sensor [ J_sensor_m_0 ] J_multisensor_m = [ J_sensor_m_1 ] [ : ] [ J_sensor_m_S ] where S is the number of sensors in this multisensor. Sensor Jacobian: A single sensor jacobian is created by concatenating jacobians for the system parameters and checkerboards J_sensor_m_s = [ J_params_m_s | J_m_s_pose0 | J_m_s_pose1 | --- | J_m_s_CB_poseC ] The number of rows in J_sensor_m_s is equal to the number of rows in this sensor's residual. J_params_m_s shows how modifying the free system parameters affects the residual. Each J_m_s_pose_c is 6 columns wide and shows how moving a certain target affects the residual. All of the J_m_s_pose blocks are zero, except J_sensor_pose_m, since target m is the only target that was viewed by the sensors in this multisensor. """ self._j_count += 1; sys.stdout.write(" \r") sys.stdout.write("Computing Jacobian.. (cycle #%d)\r" % self._j_count) sys.stdout.flush() #import scipy.optimize.slsqp.approx_jacobian as approx_jacobian #J = approx_jacobian(opt_param_vec, self.calculate_error, 1e-6) opt_param_vec, full_pose_arr = self.split_all(opt_all_vec) # Allocate the full jacobian matrix ms_r_len = [ms.get_residual_length() for ms in self._multisensors] J = zeros([sum(ms_r_len), len(opt_all_vec)]) # Calculate at which row each multisensor starts and ends ms_end_row = list(cumsum(ms_r_len)) ms_start_row = [0] + ms_end_row[:-1] # Calculate at which column each multisensor starts and ends ms_end_col = list(cumsum([6]*len(self._multisensors)) + len(opt_param_vec)) ms_start_col = [x-6 for x in ms_end_col] for i,ms in enumerate(self._multisensors): # Populate the parameter section for this multisensor J_ms_params = J[ ms_start_row[i]:ms_end_row[i], 0:len(opt_param_vec) ] s_r_len = [s.get_residual_length() for s in ms.sensors] s_end_row = list(cumsum(s_r_len)) s_start_row = [0] + s_end_row[:-1] target_pose_T = SingleTransform(full_pose_arr[i,:]).transform # Fill in parameter section one sensor at a time for k,s in enumerate(ms.sensors): J_s_params = J_ms_params[ s_start_row[k]:s_end_row[k], :] J_s_params[:,:] = self.single_sensor_params_jacobian(opt_param_vec, target_pose_T, ms.checkerboard, s) # Populate the pose section for this multisensor J_ms_pose = J[ ms_start_row[i]:ms_end_row[i], ms_start_col[i]:ms_end_col[i]] assert(J_ms_pose.shape[1] == 6) J_ms_pose[:,:] = self.multisensor_pose_jacobian(opt_param_vec, full_pose_arr[i,:], ms) sys.stdout.flush() return J
def test_length(self): st = SingleTransform([0, 0, 0, 0, 0, 0]) self.assertEqual(st.get_length(), 6)
print " Sample %d: %.6f" % (i, rms_error) i += 1 error_cat = numpy.concatenate(error_list) rms_error = numpy.sqrt(numpy.mean(error_cat**2)) print " Total: %.6f" % rms_error # Calculate loop errors chain_sensors = [[ s for s in ms.sensors if s.sensor_id == cur_loop['3d'] ][0] for ms in multisensors_pruned] cam_sensors = [[ s for s in ms.sensors if s.sensor_id == cur_loop['cam'] ][0] for ms in multisensors_pruned] fk_points = [s.get_measurement() for s in chain_sensors] cb_points = [ SingleTransform(pose).transform * system_def.checkerboards[ms.checkerboard].generate_points() for pose, ms in zip(cb_poses_pruned, multisensors_pruned) ] points_list_fk = [ geometry_msgs.msg.Point(cur_pt[0, 0], cur_pt[0, 1], cur_pt[0, 2]) for cur_pt in list(numpy.concatenate(fk_points, 1).T) ] points_list_guess = [ geometry_msgs.msg.Point(cur_pt[0, 0], cur_pt[0, 1], cur_pt[0, 2]) for cur_pt in list(numpy.concatenate(cb_points, 1).T) ] m = Marker() m.header.frame_id = system_def.base_link
def test_params_to_config(self): st = SingleTransform() p = reshape( matrix([1, 0, 0, 0, 0, 0], float), (-1,1) ) config = st.params_to_config(p) self.assertAlmostEqual( config[0], 1, 6) self.assertAlmostEqual( config[1], 0, 6)
def configure(self, urdf, config_dict): self.urdf = urdf self.fakes = list() # list of fake joints which must be later removed. # set base_link to which all measurements are based try: self.base_link = config_dict['base_link'] except: self.base_link = 'base_link' transforms = config_dict['transforms'] checkerboards = config_dict["checkerboards"] config_dict = config_dict['sensors'] # length of parameter vector cur_index = 0 # clean up joints for joint_name in urdf.joints.keys(): j = urdf.joints[joint_name] if j.origin == None: j.origin = Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0]) if j.origin.rotation == None: j.origin.rotation = [0.0, 0.0, 0.0] if j.origin.position == None: j.origin.position = [0.0, 0.0, 0.0] # build our transforms self.transforms = dict() for joint_name in urdf.joints.keys(): joint_name = str(joint_name) j = urdf.joints[joint_name] rot = j.origin.rotation rot = RPY_to_angle_axis(rot) p = j.origin.position + rot self.transforms[joint_name] = SingleTransform(p, joint_name) self.transforms[joint_name].start = cur_index self.transforms[joint_name].end = cur_index + self.transforms[ joint_name].get_length() cur_index = self.transforms[joint_name].end for name, transform in transforms.items(): transform = [eval(str(x)) for x in transform] transform[3:6] = RPY_to_angle_axis(transform[3:6]) try: self.transforms[name].inflate( reshape(matrix(transform, float), (-1, 1))) except: rospy.logwarn("Transform not found in URDF %s", name) self.transforms[name] = SingleTransform(transform, name) self.transforms[name].start = cur_index self.transforms[ name].end = cur_index + self.transforms[name].get_length() cur_index = self.transforms[name].end # build our chains self.chains = dict() chain_dict = config_dict['chains'] for chain_name in config_dict['chains']: # create configuration this_config = config_dict['chains'][chain_name] this_config['joints'] = urdf.get_chain(this_config['root'], this_config['tip'], links=False) this_config['active_joints'] = list() this_config['axis'] = list() this_config['transforms'] = dict() this_config['gearing'] = config_dict['chains'][chain_name][ 'gearing'] # TODO: This always defaults to 1.0? this_config['cov'] = config_dict['chains'][chain_name]['cov'] for joint_name in this_config["joints"]: this_config['transforms'][joint_name] = self.transforms[ joint_name] if urdf.joints[joint_name].joint_type in [ 'revolute', 'continuous' ]: this_config["active_joints"].append(joint_name) axis = list(urdf.joints[joint_name].axis.split()) this_config["axis"].append( sum([i[0] * int(i[1]) for i in zip([4, 5, 6], axis)])) # we can handle limited rotations here rot = urdf.joints[joint_name].origin.rotation if rot != None and ( sum([abs(x) for x in rot]) - rot[abs(this_config["axis"][-1]) - 4]) > 0.001: print 'Joint origin is rotated, calibration will fail: ', joint_name elif urdf.joints[joint_name].joint_type == 'prismatic': this_config["active_joints"].append(joint_name) axis = list(urdf.joints[joint_name].axis.split()) this_config["axis"].append( sum([i[0] * int(i[1]) for i in zip([1, 2, 3], axis)])) elif urdf.joints[joint_name].joint_type != 'fixed': print 'Unknown joint type:', urdf.joints[ joint_name].joint_type # put a checkerboard in it's hand self.urdf.add_link(Link(chain_name + "_cb_link")) self.urdf.add_joint( Joint(chain_name + "_cb", this_config['tip'], chain_name + "_cb_link", "fixed", origin=Pose([0.0, 0.0, 0.0], [0.0, 0.0, 0.0]))) self.fakes += [chain_name + "_cb_link", chain_name + "_cb"] self.chains[chain_name] = JointChain(this_config) self.chains[chain_name].start = cur_index self.chains[chain_name].end = cur_index + self.chains[ chain_name].get_length() cur_index = self.chains[chain_name].end # build our lasers: self.tilting_lasers = dict() if 'tilting_lasers' in config_dict.keys(): self.tilting_lasers, cur_index = init_primitive_dict( cur_index, config_dict["tilting_lasers"], TiltingLaser) self.rectified_cams, cur_index = init_primitive_dict( cur_index, config_dict["rectified_cams"], RectifiedCamera) self.checkerboards, cur_index = init_primitive_dict( cur_index, checkerboards, Checkerboard) self.length = cur_index
def test_deflate(self): st = SingleTransform([0, 0, 0, 0, 0, 0]) p = reshape( matrix([1, 0, 0, 0, 0, 0], float), (-1,1) ) st.inflate(p) result = st.deflate() self.assertAlmostEqual(numpy.linalg.norm(p-result), 0.0, 6)