コード例 #1
0
ファイル: ekf_slam.py プロジェクト: evargasv/ekf-slam
    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])])
コード例 #2
0
    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])])
コード例 #3
0
    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
コード例 #4
0
ファイル: node.py プロジェクト: evargasv/ekf-slam
    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
コード例 #5
0
    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)
コード例 #6
0
 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
コード例 #7
0
    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
コード例 #8
0
ファイル: ekf_slam.py プロジェクト: evargasv/ekf-slam
    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
コード例 #9
0
    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])
コード例 #10
0
ファイル: ekf_slam.py プロジェクト: evargasv/ekf-slam
    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 )