def get_polar_line(self, line, odom): ''' Transforms a line from [x1 y1 x2 y2] from the world frame to the vehicle frame using odometry [x y ang]. Returns [range theta] ''' # Line points x1 = line[0] y1 = line[1] x2 = line[2] y2 = line[3] # Compute line (a, b, c) and range line = np.array([y1-y2, x2-x1, x1*y2-x2*y1]) pt = np.array([odom[0], odom[1], 1]) dist = np.dot(pt, line) / np.linalg.norm(line[:2]) # Compute angle if dist < 0: ang = np.arctan2(line[1], line[0]) else: ang = np.arctan2(-line[1], -line[0]) # Return in the vehicle frame return np.array([np.abs(dist), angle_wrap(ang - odom[2])])
def get_polar_line(self, line, odom): """ Transforms a line from [x1 y1 x2 y2] from the world frame to the vehicle frame using odomotrey [x y ang]. Returns [range theta] """ # Line points x1 = line[0] y1 = line[1] x2 = line[2] y2 = line[3] # Compute line (a, b, c) and range line = np.array([y1 - y2, x2 - x1, x1 * y2 - x2 * y1]) pt = np.array([odom[0], odom[1], 1]) dist = np.dot(pt, line) / np.linalg.norm(line[:2]) # Compute angle if dist > 0: ang = np.arctan2(line[1], line[0]) else: ang = np.arctan2(-line[1], -line[0]) # Return in the vehicle frame return np.array([np.abs(dist), angle_wrap(ang - odom[2])])
def tfPolarLine(self,tf,line): ''' Transforms a polar line in the robot frame to a polar line in the world frame ''' # Decompose transformation x_x = tf[0] x_y = tf[1] x_ang = tf[2] # Compute the new phi phi = angle_wrap(line[1] + x_ang) # Auxiliar computations sqrt2 = x_x**2+x_y**2 sqrt = np.sqrt(sqrt2) atan2 = np.arctan2(x_y,x_x) sin = np.sin(atan2 - phi) cos = np.cos(atan2 - phi) # Compute the new rho rho = line[0] + sqrt* cos # Allocate jacobians H_tf = np.zeros((2,3)) H_line = np.eye(2) # TODO: Evaluate jacobian respect to transformation # TODO: Evaluate jacobian respect to line return np.array([rho,phi]), H_tf, H_line
def odom_callback(self, msg): ''' Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame. ''' # Save time self.time = msg.header.stamp # Translation trans = (msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z) # Rotation rot = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w) # Publish transform self.tfBroad.sendTransform(translation = (self.robotToSensor[0],self.robotToSensor[1],0), rotation = tf.transformations.quaternion_from_euler(0,0,self.robotToSensor[2]), time = msg.header.stamp, child = '/openni_depth_frame', parent = '/robot') self.tfBroad.sendTransform(translation = trans, rotation = rot, time = msg.header.stamp, child = '/robot_original', parent = '/odom') self.tfBroad.sendTransform(translation = (self.x0[0]-0.1908,self.x0[1]+0.08481,self.x0[2]), rotation = tf.transformations.quaternion_from_euler(0,0,-0.034128), # rotation = (0,0,0,1), time = msg.header.stamp, child = '/odom', parent = '/world') # Incremental odometry if self.last_odom is not None and not self.new_odom: # Increment computation delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y yaw = yaw_from_quaternion(msg.pose.pose.orientation) lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation) # Odometry seen from vehicle frame self.uk = np.array([delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw), - delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw), angle_wrap(yaw - lyaw)]) # Flag self.new_odom = True # For next loop self.last_odom = msg if self.last_odom is None: self.last_odom = msg
def predict(self, odom): ''' Moves particles with the given odometry. odom: incremental odometry [delta_x delta_y delta_yaw] in the vehicle frame ''' # Add Gaussian noise to odometry measures lin_noise = np.random.randn(self.num, 2) * self.odom_lin_sigma ang_noise = angle_wrap( np.random.randn(self.num) * self.odom_ang_sigma + odom[2]) # Increment particle positions in correct frame odom_noise = np.tile(odom[0:2], (self.num, 1)) odom_noise += lin_noise self.p_xy[0, :] += odom_noise[:, 0] * np.cos( self.p_ang) - odom_noise[:, 1] * np.sin(self.p_ang) self.p_xy[1, :] += odom_noise[:, 0] * np.sin( self.p_ang) + odom_noise[:, 1] * np.cos(self.p_ang) # Increment angley self.p_ang += ang_noise self.p_ang = angle_wrap(self.p_ang)
def odom_callback(self, msg): ''' Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame. ''' # Save time self.time = msg.header.stamp # Translation trans = (msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z) # Rotation rot = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w) # Publish transform self.tfBroad.sendTransform(translation = trans, rotation = rot, time = msg.header.stamp, child = '/openni_depth_frame', parent = '/world') # Incremental odometry if self.last_odom is not None: # Increment computation delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y yaw = yaw_from_quaternion(msg.pose.pose.orientation) lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation) # Odometry seen from vehicle frame self.uk = np.array([delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw), - delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw), angle_wrap(yaw - lyaw)]) # Flag self.new_odom = True # For next loop self.last_odom = msg
def odom_callback(self, msg): ''' Publishes a tf based on the odometry of the robot and calculates the incremental odometry as seen from the vehicle frame. ''' # Save time self.time = msg.header.stamp # Translation trans = (msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z) # Rotation rot = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w) # Publish transform self.tfBroad.sendTransform(translation=trans, rotation=rot, time=msg.header.stamp, child='/openni_depth_frame', parent='/world') # Incremental odometry if self.last_odom is not None: # Increment computation delta_x = msg.pose.pose.position.x - self.last_odom.pose.pose.position.x delta_y = msg.pose.pose.position.y - self.last_odom.pose.pose.position.y yaw = yaw_from_quaternion(msg.pose.pose.orientation) lyaw = yaw_from_quaternion(self.last_odom.pose.pose.orientation) # Odometry seen from vehicle frame self.uk = np.array([ delta_x * np.cos(lyaw) + delta_y * np.sin(lyaw), -delta_x * np.sin(lyaw) + delta_y * np.cos(lyaw), angle_wrap(yaw - lyaw) ]) # Flag self.new_odom = True # For next loop self.last_odom = msg
def tfPolarLine(self,tf,line): ''' Transforms a polar line in the robot frame to a polar line in the world frame ''' # Decompose transformation x_x = tf[0] x_y = tf[1] x_ang = tf[2] # Compute the new phi phi = angle_wrap(line[1] + x_ang) # Auxiliar computations sqrt2 = x_x**2+x_y**2 sqrt = np.sqrt(sqrt2) atan2 = np.arctan2(x_y,x_x) sin = np.sin(atan2 - phi) cos = np.cos(atan2 - phi) # Compute the new rho rho = line[0] + sqrt* cos # Allocate jacobians H_tf = np.zeros((2,3)) H_line = np.eye(2) # Evaluate jacobian respect to transformation H_tf[0,0] = ( x_x / sqrt ) * cos + ( x_y / sqrt ) * sin H_tf[0,1] = ( x_y / sqrt ) * cos - ( x_x / sqrt ) * sin H_tf[0,2] = sqrt * sin H_tf[1,2] = 1 # Evaluate jacobian respect to line H_line[0,0] = 1 H_line[0,1] = sqrt * sin H_line[1,1] = 1 return np.array([rho,phi]), H_tf, H_line
def predict(self, odom): """ Moves particles with the given odometry. odom: incremental odometry [delta_x delta_y delta_yaw] in the vehicle frame """ # TODO: code here!! # Add Gaussian noise to odometry measures lineNoise = self.odom_lin_sigma * np.random.randn(2, self.num) / 15 angNoise = self.odom_ang_sigma * np.random.randn(self.num) / 15 # Increment particle positions in correct frame for i in range(self.p_xy.shape[1]): odom[0] = odom[0] + lineNoise[0, i] odom[1] = odom[1] + lineNoise[1, i] odom[2] = odom[2] + angNoise[i] # print ('The odom is %f and %f and %f ' %(odom[0],odom[1],odom[2])) self.p_xy[0, i] += np.cos(self.p_ang[i]) * odom[0] - np.sin(self.p_ang[i]) * odom[1] self.p_xy[1, i] += np.sin(self.p_ang[i]) * odom[0] + np.cos(self.p_ang[i]) * odom[1] # Increment angle self.p_ang[i] += odom[2] self.p_ang[i] = angle_wrap(self.p_ang[i])
def predict(self, uk): ''' Predicts the position of the robot according to the previous position and the odometry measurements. It also updates the uncertainty of the position ''' # - Update self.x_B_1 and self.P_B_1 using uk and self.Qk # number of observed features n = self.get_number_of_features_in_map() # Compute jacobians of the composition with respect to robot (A_k) # and odometry (W_k) # A_k A_k = np.array([[1, 0, -np.sin(self.x_B_1[2]) * uk[0] - np.cos(self.x_B_1[2]) * uk[1]],\ [0, 1, np.cos(self.x_B_1[2]) * uk[0] - np.sin(self.x_B_1[2]) * uk[1]],\ [0, 0, 1]]) # W_k W_k = np.array([[np.cos(self.x_B_1[2]), -np.sin(self.x_B_1[2]), 0],\ [np.sin(self.x_B_1[2]), np.cos(self.x_B_1[2]), 0],\ [0,0,1]]) # Prepare the F_k and G_k matrix for the new uncertainty computation # F_k F_k = scipy.linalg.block_diag(A_k, np.eye(2*n)) # G_k G_k = np.vstack((W_k,np.zeros((2*n,3)))) # Update the class variables # Compound robot with odometry self.x_B_1[0] += ( np.cos(self.x_B_1[2]) * uk[0] ) - ( np.sin(self.x_B_1[2]) * uk[1] ) self.x_B_1[1] += ( np.sin(self.x_B_1[2]) * uk[0] ) + ( np.cos(self.x_B_1[2]) * uk[1] ) self.x_B_1[2] = angle_wrap( self.x_B_1[2] + uk[2] ) # Compute uncertainty #ipdb.set_trace() self.P_B_1 = ( F_k * np.mat(self.P_B_1) * F_k.T ) + ( G_k * np.mat(self.Qk) * G_k.T )