def test_straight_right(self): motion_model = MotionModel(0, 0, 0, 0) odometry = [0, 0, 0] for _ in range(0, 100): odometry[0] += 1 motion_model.update(self.particle, odometry, 0) self.assertEqual(self.particle.x, 100) self.assertEqual(self.particle.y, 40) self.assertEqual(self.particle.theta, 0)
def test_turn_right(self): motion_model = MotionModel(0, 0, 0, 0) odometry = [0, 0, 0] self.particle.x = 0 self.particle.y = 40 self.particle.theta = 0 for _ in range(0, 10): odometry[2] -= math.pi/20 motion_model.update(self.particle, odometry, 0) self.assertEqual(self.particle.x, 0) self.assertEqual(self.particle.y, 40) self.assertEqual(self.particle.theta, -math.pi/2)
def test_sidestep_right(self): motion_model = MotionModel(0, 0, 0, 0) odometry = [0, 0, 0] self.particle.x = 0 self.particle.y = 40 self.particle.theta = math.pi/2 for _ in range(0, 100): odometry[1] -= 1 motion_model.update(self.particle, odometry, 0) self.assertEqual(self.particle.x, 100) self.assertEqual(self.particle.y, 40) self.assertEqual(self.particle.theta, math.pi/2)
def test_straight_up(self): motion_model = MotionModel(0, 0, 0, 0) odometry = [0, 0, 0] self.particle.x = 40 self.particle.y = 0 self.particle.theta = math.pi/2 for _ in range(0, 100): odometry[0] += 1 motion_model.update(self.particle, odometry, 0) self.assertEqual(self.particle.x, 40) self.assertEqual(self.particle.y, 100) self.assertEqual(self.particle.theta, math.pi/2)
def test_distribution_line(self): motion_model = MotionModel(0.1, 0.1, 0.1, 0.1) odometry = [0, 0, 0] p, = plt.plot([], [], 'r.') for _ in range(0, 3): odometry[0] += 1 x = list() y = list() for particle in self.particles: motion_model.update(particle, odometry, 0) x.append(particle.x) y.append(particle.y) self.map.display(self.particles, title='Line w/ motion model')
def test_distribution_circle(self): motion_model = MotionModel(0.01, 0.01, 0.01, 0.01) odometry = [0, 0, 0] for particle in self.particles: particle.x = 0 particle.y = 40 particle.theta = math.pi/2 for _ in range(0, 4): odometry[0] += 10*math.sqrt(2)/2 odometry[1] -= 10*(1-math.sqrt(2)/2) odometry[2] -= math.pi/4 for particle in self.particles: motion_model.update(particle, odometry, 0) self.map.display(self.particles, title='Upper Circle w/ Motion Model')
def test_circle(self): motion_model = MotionModel(0, 0, 0, 0) odometry = [0, 0, 0] self.particle.x = 0 self.particle.y = 40 self.particle.theta = math.pi/2 for _ in range(0, 4): odometry[0] += 10*math.sqrt(2)/2 odometry[1] -= 10*(1-math.sqrt(2)/2) odometry[2] -= math.pi/4 motion_model.update(self.particle, odometry, 0) #self.map.display([self.particle], title='Zero Variance Upper Circle') self.assertAlmostEqual(self.particle.x, 20) self.assertEqual(self.particle.y, 40) self.assertEqual(self.particle.theta, -math.pi/2)
def __init__(self, map_file='../data/map/wean.dat'): self.map = Map(map_file) #self.map.display_gaussian([], 'Gaussian Blurred Map') self._number_particles = 1000 self.particles = list() self._particle_probabilities = [] for _ in range(0, self._number_particles): print 'Initializing particle', _ particle = Particle(self.map) #particle.x = 4080 + np.random.normal(scale=35) #particle.y = 3980 + np.random.normal(scale=15) #particle.theta = math.pi + 0.1 + np.random.normal(scale=.1) self.particles.append(particle) self._particle_probabilities.append(1) self._motion_model = MotionModel(0.001, 0.001, 0.1, 0.1) self._sensor_model = SensorModel(self.map) self._ranges = []
def test_forward_back(self): motion_model = MotionModel(0, 0, 0, 0) odometry = [0, 0, 0] self.particle.x = 0 self.particle.y = 40 self.particle.theta = 0 for _ in range(0, 10): odometry[0] += 1 motion_model.update(self.particle, odometry, 0) for _ in range(0, 10): odometry[2] += math.pi/10 motion_model.update(self.particle, odometry, 0) for _ in range(0, 10): odometry[0] += 1 motion_model.update(self.particle, odometry, 0) self.assertEqual(self.particle.x, 0) self.assertEqual(self.particle.y, 40) self.assertEqual(self.particle.theta, math.pi)
import numpy as np import matplotlib.pyplot as plt from motion_model import MotionModel from regression import load_data, get_norm_emg from activation import Activation if __name__ == '__main__': # Constants Activation parameters frequency = np.arange(20, 55, 5) duty_cycle = np.arange(0, 1.0, 0.1) scaling = 1 non_linearity = -1 shape = "monophasic" # Motion Model motion_model = MotionModel(0.58, 1) # Tile for plotting independent_1 = np.tile(frequency, (len(duty_cycle), 1)) independent_1 = np.transpose(independent_1) independent_2 = np.tile(duty_cycle, (len(frequency), 1)) # Metrics rmse_ankle_angle = [] above_0 = [] rmse_toe_height = [] for i in range(len(independent_1)): for j in range(len(independent_1[0])): motion_model.set_activation(frequency[i], duty_cycle[j], scaling, non_linearity, shape)
def __init__(self): # Get parameters self.particle_filter_frame = rospy.get_param("~particle_filter_frame") self.MAX_PARTICLES = int(rospy.get_param("~num_particles")) self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles")) self.PUBLISH_ODOM = bool(rospy.get_param("~publish_odom", "1")) self.ANGLE_STEP = int(rospy.get_param("~angle_step")) self.DO_VIZ = bool(rospy.get_param("~do_viz")) # MCL algorithm self.iters = 0 self.lidar_initialized = False self.odom_initialized = False self.map_initialized = False self.last_odom_pose = None # last received odom pose self.last_odom_msg = None self.laser_angles = None self.downsampled_angles = None self.state_lock = Lock() # paritcles self.inferred_pose = None self.particles = np.zeros((self.MAX_PARTICLES, 3)) self.particle_indices = np.arange(self.MAX_PARTICLES) self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) # Initialize the models self.motion_model = MotionModel() self.sensor_model = SensorModel() # initialize the state self.smoothing = Utils.CircularArray(10) self.timer = Utils.Timer(10) self.initialize_global() # map self.permissible_region = None self.SHOW_FINE_TIMING = False # these topics are for visualization self.pose_pub = rospy.Publisher("/pf/viz/inferred_pose", PoseStamped, queue_size=1) self.particle_pub = rospy.Publisher("/pf/viz/particles", PoseArray, queue_size=1) self.pub_fake_scan = rospy.Publisher("/pf/viz/fake_scan", LaserScan, queue_size=1) self.path_pub = rospy.Publisher('/pf/viz/path', Path, queue_size=1) self.path = Path() if self.PUBLISH_ODOM: self.odom_pub = rospy.Publisher("/pf/pose/odom", Odometry, queue_size=1) # these topics are for coordinate space things self.pub_tf = tf.TransformBroadcaster() # these topics are to receive data from the racecar self.laser_sub = rospy.Subscriber(rospy.get_param( "~scan_topic", "/scan"), LaserScan, self.callback_lidar, queue_size=1) self.odom_sub = rospy.Subscriber(rospy.get_param( "~odom_topic", "/odom"), Odometry, self.callback_odom, queue_size=1) self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose, queue_size=1) self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=1) print "Finished initializing, waiting on messages..."
class ParticleFilter: ''' Monte Carlo Localization based on odometry and a laser scanner. ''' def __init__(self): # Get parameters self.particle_filter_frame = rospy.get_param("~particle_filter_frame") self.MAX_PARTICLES = int(rospy.get_param("~num_particles")) self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles")) self.PUBLISH_ODOM = bool(rospy.get_param("~publish_odom", "1")) self.ANGLE_STEP = int(rospy.get_param("~angle_step")) self.DO_VIZ = bool(rospy.get_param("~do_viz")) # MCL algorithm self.iters = 0 self.lidar_initialized = False self.odom_initialized = False self.map_initialized = False self.last_odom_pose = None # last received odom pose self.last_odom_msg = None self.laser_angles = None self.downsampled_angles = None self.state_lock = Lock() # paritcles self.inferred_pose = None self.particles = np.zeros((self.MAX_PARTICLES, 3)) self.particle_indices = np.arange(self.MAX_PARTICLES) self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) # Initialize the models self.motion_model = MotionModel() self.sensor_model = SensorModel() # initialize the state self.smoothing = Utils.CircularArray(10) self.timer = Utils.Timer(10) self.initialize_global() # map self.permissible_region = None self.SHOW_FINE_TIMING = False # these topics are for visualization self.pose_pub = rospy.Publisher("/pf/viz/inferred_pose", PoseStamped, queue_size=1) self.particle_pub = rospy.Publisher("/pf/viz/particles", PoseArray, queue_size=1) self.pub_fake_scan = rospy.Publisher("/pf/viz/fake_scan", LaserScan, queue_size=1) self.path_pub = rospy.Publisher('/pf/viz/path', Path, queue_size=1) self.path = Path() if self.PUBLISH_ODOM: self.odom_pub = rospy.Publisher("/pf/pose/odom", Odometry, queue_size=1) # these topics are for coordinate space things self.pub_tf = tf.TransformBroadcaster() # these topics are to receive data from the racecar self.laser_sub = rospy.Subscriber(rospy.get_param( "~scan_topic", "/scan"), LaserScan, self.callback_lidar, queue_size=1) self.odom_sub = rospy.Subscriber(rospy.get_param( "~odom_topic", "/odom"), Odometry, self.callback_odom, queue_size=1) self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose, queue_size=1) self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=1) print "Finished initializing, waiting on messages..." def initialize_global(self): ''' Spread the particle distribution over the permissible region of the state space. ''' print("GLOBAL INITIALIZATION") # randomize over grid coordinate space self.state_lock.acquire() print('Waiting for map..') while not self.sensor_model.map_set: continue self.map_initialized = True self.permissible_region = self.sensor_model.permissible_region self.map = self.sensor_model.map self.map_info = self.sensor_model.map_info permissible_x, permissible_y = np.where(self.permissible_region == 1) indices = np.random.randint(0, len(permissible_x), size=self.MAX_PARTICLES) permissible_states = np.zeros((self.MAX_PARTICLES, 3)) permissible_states[:, 0] = permissible_y[indices] permissible_states[:, 1] = permissible_x[indices] permissible_states[:, 2] = np.random.random( self.MAX_PARTICLES) * np.pi * 2.0 Utils.map_to_world(permissible_states, self.map_info) self.particles = permissible_states self.weights[:] = 1.0 / self.MAX_PARTICLES self.state_lock.release() def initialize_particles_pose(self, pose): ''' Initialize particles in the general region of the provided pose. ''' print "SETTING POSE" print pose self.state_lock.acquire() self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES) self.particles[:, 0] = pose.position.x + np.random.normal( loc=0.0, scale=0.5, size=self.MAX_PARTICLES) self.particles[:, 1] = pose.position.y + np.random.normal( loc=0.0, scale=0.5, size=self.MAX_PARTICLES) self.particles[:, 2] = Utils.quaternion_to_angle( pose.orientation) + np.random.normal( loc=0.0, scale=0.4, size=self.MAX_PARTICLES) # This is supposed to be used only when in simulation env (due to simulation bugs) # if isinstance(self.last_odom_pose, np.ndarray): # self.last_odom_pose[0] = pose.position.x # self.last_odom_pose[1] = pose.position.y # self.last_odom_pose[2] = Utils.quaternion_to_angle(pose.orientation) self.state_lock.release() def clicked_pose(self, msg): ''' Receive pose messages from RViz and initialize the particle distribution in response. ''' print('clicked_pose') if isinstance(msg, PointStamped): #self.initialize_particles_pose(msg.pose.pose) self.initialize_global() elif isinstance(msg, PoseWithCovarianceStamped): self.initialize_particles_pose(msg.pose.pose) def publish_tf(self, pose, stamp=None): """ Publish a tf for the car. This tells ROS where the car is with respect to the map. """ if stamp == None: stamp = rospy.Time.now() # this may cause issues with the TF tree. If so, see the below code. self.pub_tf.sendTransform( (pose[0], pose[1], 0), tf.transformations.quaternion_from_euler(0, 0, pose[2]), stamp, "/laser", "/map") # also publish odometry to facilitate getting the localization pose if self.PUBLISH_ODOM: odom = Odometry() odom.header = Utils.make_header("/map", stamp) odom.pose.pose.position.x = pose[0] odom.pose.pose.position.y = pose[1] odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2]) self.odom_pub.publish(odom) """ Our particle filter provides estimates for the "laser" frame since that is where our laser range estimates are measure from. Thus, We want to publish a "map" -> "laser" transform. However, the car's position is measured with respect to the "base_link" frame (it is the root of the TF tree). Thus, we should actually define a "map" -> "base_link" transform as to not break the TF tree. """ # Get map -> laser transform. map_laser_pos = np.array((pose[0], pose[1], 0)) map_laser_rotation = np.array( tf.transformations.quaternion_from_euler(0, 0, pose[2])) # Apply laser -> base_link transform to map -> laser transform # This gives a map -> base_link transform laser_base_link_offset = (0.265, 0, 0) map_laser_pos -= np.dot( tf.transformations.quaternion_matrix( tf.transformations.unit_vector(map_laser_rotation))[:3, :3], laser_base_link_offset).T # Publish transform self.pub_tf.sendTransform(map_laser_pos, map_laser_rotation, stamp, self.particle_filter_frame, "/map") def publish_particles(self, particles): pa = PoseArray() pa.header = Utils.make_header("map") pa.poses = Utils.particles_to_poses(particles) self.particle_pub.publish(pa) def publish_scan(self, angles, ranges): ls = LaserScan() ls.header = Utils.make_header("laser", stamp=self.last_stamp) ls.angle_min = np.min(angles) ls.angle_max = np.max(angles) ls.angle_increment = np.abs(angles[0] - angles[1]) ls.range_min = 0 ls.range_max = np.max(ranges) ls.ranges = ranges self.pub_fake_scan.publish(ls) def visualize(self): ''' Publish various visualization messages. ''' if not self.DO_VIZ: return if self.pose_pub.get_num_connections() > 0 and isinstance( self.inferred_pose, np.ndarray): ps = PoseStamped() ps.header = Utils.make_header("map") ps.pose.position.x = self.inferred_pose[0] ps.pose.position.y = self.inferred_pose[1] ps.pose.orientation = Utils.angle_to_quaternion( self.inferred_pose[2]) self.pose_pub.publish(ps) if self.iters % 10 == 0: self.path.header = ps.header self.path.poses.append(ps) self.path_pub.publish(self.path) if self.particle_pub.get_num_connections() > 0: if self.MAX_PARTICLES > self.MAX_VIZ_PARTICLES: # randomly downsample particles proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES, p=self.weights) # proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES) self.publish_particles(self.particles[proposal_indices, :]) else: self.publish_particles(self.particles) if self.pub_fake_scan.get_num_connections() > 0 and isinstance( self.sensor_model.particle_scans, np.ndarray): # generate the scan from the point of view of the inferred position for visualization inferred_scans = self.sensor_model.get_scans( self.inferred_pose[None, :]) self.publish_scan(self.downsampled_angles, inferred_scans[0, :]) def callback_lidar(self, msg): ''' Initializes reused buffers, and stores the relevant laser scanner data for later use. ''' if not isinstance(self.laser_angles, np.ndarray): print "...Received first LiDAR message" self.laser_angles = np.linspace(msg.angle_min, msg.angle_max, len(msg.ranges)) self.downsampled_angles = np.copy( self.laser_angles[0::self.ANGLE_STEP]).astype(np.float32) #self.viz_queries = np.zeros((self.downsampled_angles.shape[0],3), dtype=np.float32) #self.viz_ranges = np.zeros(self.downsampled_angles.shape[0], dtype=np.float32) #print self.downsampled_angles.shape[0] self.downsampled_ranges = np.array(msg.ranges[::self.ANGLE_STEP]) self.lidar_initialized = True def callback_odom(self, msg): ''' Store deltas between consecutive odometry messages in the coordinate space of the car. Odometry data is accumulated via dead reckoning, so it is very inaccurate on its own. ''' if self.last_odom_msg: last_time = self.last_odom_msg.header.stamp.secs this_time = msg.header.stamp.secs dt = this_time - last_time self.odometry_data = np.array([ msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.angular.z ]) * dt self.last_odom_msg = msg self.last_stamp = msg.header.stamp self.update() else: print "...Received first Odometry message" self.last_odom_msg = msg self.last_stamp = msg.header.stamp self.odom_initialized = True def expected_pose(self, particles): # returns the expected value of the pose given the particle distribution return np.dot(particles.transpose(), self.weights) def MCL(self, odom, scans): # Implement the MCL algorithm # using the sensor model and the motion model # # Make sure you include some way to initialize # your particles, ideally with some sort # of interactive interface in rviz proposal_indices = np.random.choice(self.particle_indices, self.MAX_PARTICLES, p=self.weights) proposal_distribution = self.particles[proposal_indices, :] if self.SHOW_FINE_TIMING: t_propose = time.time() #print("resample",self.expected_pose(proposal_distribution)) # compute the motion model to update the proposal distribution proposal_distribution = self.motion_model.evaluate( proposal_distribution, odom) if self.SHOW_FINE_TIMING: t_motion = time.time() #print("motion model:", self.expected_pose(proposal_distribution)) # compute the sensor model self.weights = self.sensor_model.evaluate(proposal_distribution, scans) if self.SHOW_FINE_TIMING: t_sensor = time.time() # normalize importance weights self.weights /= np.sum(self.weights) #print("sensor model:", self.expected_pose(proposal_distribution)) if self.SHOW_FINE_TIMING: t_norm = time.time() t_total = (t_norm - t) / 100.0 if self.SHOW_FINE_TIMING and self.iters % 10 == 0: print "MCL: propose: ", np.round((t_propose-t)/t_total, 2), "motion:", np.round((t_motion-t_propose)/t_total, 2), \ "sensor:", np.round((t_sensor-t_motion)/t_total, 2), "norm:", np.round((t_norm-t_sensor)/t_total, 2) # save the particles self.particles = proposal_distribution #print(self.expected_pose(self.particles)) # Publish a transformation frame between the map # and the particle_filter_frame. def update(self): ''' Apply the MCL function to update particle filter state. Ensures the state is correctly initialized, and acquires the state lock before proceeding. ''' if self.lidar_initialized and self.odom_initialized and self.map_initialized: if self.state_lock.locked(): print("Concurrency error avoided") else: self.state_lock.acquire() self.timer.tick() self.iters += 1 t1 = time.time() scans = np.copy(self.downsampled_ranges).astype(np.float32) odom = np.copy(self.odometry_data) self.odometry_data = np.zeros(3) # run the MCL update algorithm self.MCL(odom, scans) # compute the expected value of the robot pose self.inferred_pose = self.expected_pose(self.particles) self.state_lock.release() t2 = time.time() # publish transformation frame based on inferred pose self.publish_tf(self.inferred_pose, self.last_stamp) # this is for tracking particle filter speed ips = 1.0 / (t2 - t1) self.smoothing.append(ips) #if self.iters % 10 == 0: # print "iters per sec:", int(self.timer.fps()), " possible:", int(self.smoothing.mean()) self.visualize()
def forward(self, param, p_c, p_e, priors, times): # param.view(param.size(0), -1, config["num_motion_model_param"]), # parameter predicts # # self.softmax(p_c.view(p_c.size(0), -1, 2)), # motion possibility # self.softmax(p_e.view(p_e.size(0), -1, self.num_classes)), # classification possiblity # self.priors # default boxes loc_datas = MotionModel.get_bbox_by_frames_pytorch(param, times) # find the times scale if loc_datas.size(1) % p_e.size(1) != 0: raise AssertionError( "time scales should be the int in nms_with_frames") time_scales = loc_datas.size(1) // p_e.size(1) if time_scales > 1: p_e = p_e[:, :, None, :, :].repeat(1, 1, time_scales, 1, 1)\ .view(p_e.size(0), -1, 1, p_e.size(2), p_e.size(3)).squeeze(2) num = loc_datas.size(0) # batch size num_priors = priors.size(0) num_frames = times.size(1) param_shape_1 = param.size(2) param_shape_2 = param.size(3) output_boxes = torch.zeros(num, self.num_classes, num_frames, self.top_k, 4) output_p_e = torch.zeros(num, 2, num_frames, self.top_k) output_params = torch.zeros(num, self.num_classes, self.top_k, param_shape_1, param_shape_2) output_p_c = torch.zeros(num, self.num_classes, self.top_k) conf_preds = p_c.squeeze(1).view(num, num_priors, self.num_classes).transpose(2, 1) conf_exists = p_e.transpose(3, 2) # Decode predictions into bboxes. decoded_locs = decode_with_frames(loc_datas, priors, self.variance) for i in range(num): # For each class, perform nms conf_scores = conf_preds[i] decoded_boxes = decoded_locs[i, :] for cl in range(1, self.num_classes): # filter boxes by confidence c_mask = conf_scores[cl].gt(self.conf_thresh) if c_mask.sum() == 0: continue scores = conf_scores[cl][c_mask] exists = conf_exists[i, :, 1, c_mask] boxes = decoded_boxes[:, c_mask, :] # filter boxes by visibility v_mask = (exists > self.exist_thresh).sum( dim=0) / exists.shape[0] >= self.min_valid_node_rate if v_mask.sum() == 0: continue scores = scores[v_mask] exists = exists[:, v_mask] boxes = boxes[:, v_mask, :] # if there are exists the reasonable boxes. # print(c_mask.sum().item()) # if c_mask.sum().item() > 0: # idx of highest scoring and non-overlapping boxes per class ids, count = nms_with_frames(boxes, scores, exists, self.nms_thresh, self.top_k, self.exist_thresh) if count > 0: output_boxes[i, cl, :, :count, :] = boxes[:, ids[:count]] output_p_e[i, 1, :, :count] = exists[:, ids[:count]] output_params[i, cl, :count, :] = param[i, c_mask, :][ v_mask, :][ids[:count], :] output_p_c[i, cl, :count] = scores[ids[:count]] output_p_c_1 = output_p_c.contiguous().view(num, -1) _, idx = output_p_c_1.sort(1, descending=True) _, rank = idx.sort(1) mask = rank > self.top_k output_p_c_1.masked_fill_(mask, 0) return (output_params, output_p_c, output_p_e, output_boxes)
def __init__(self, system): if kDebugDrawMatches: Frame.is_store_imgs = True self.system = system self.camera = system.camera self.map = system.map self.local_mapping = system.local_mapping self.intializer = Initializer() self.motion_model = MotionModel( ) # motion model for current frame pose prediction without damping #self.motion_model = MotionModelDamping() # motion model for current frame pose prediction with damping self.dyn_config = SLAMDynamicConfig() self.descriptor_distance_sigma = Parameters.kMaxDescriptorDistance self.reproj_err_frame_map_sigma = Parameters.kMaxReprojectionDistanceMap self.max_frames_between_kfs = int(system.camera.fps) self.min_frames_between_kfs = 0 self.state = SlamState.NO_IMAGES_YET self.num_matched_kps = None # current number of matched keypoints self.num_inliers = None # current number of matched points self.num_matched_map_points = None # current number of matched map points (matched and found valid in current pose optimization) self.num_kf_ref_tracked_points = None # number of tracked points in k_ref (considering a minimum number of observations) self.mask_match = None self.pose_is_ok = False self.predicted_pose = None self.velocity = None self.f_cur = None self.idxs_cur = None self.f_ref = None self.idxs_ref = None self.kf_ref = None # reference keyframe (in general, different from last keyframe depending on the used approach) self.kf_last = None # last keyframe self.kid_last_BA = -1 # last keyframe id when performed BA self.local_keyframes = [] # local keyframes self.local_points = [] # local points self.tracking_history = TrackingHistory() self.timer_verbose = kTimerVerbose # set this to True if you want to print timings self.timer_main_track = TimerFps('Track', is_verbose=self.timer_verbose) self.timer_pose_opt = TimerFps('Pose optimization', is_verbose=self.timer_verbose) self.timer_seach_frame_proj = TimerFps('Search frame by proj', is_verbose=self.timer_verbose) self.timer_match = TimerFps('Match', is_verbose=self.timer_verbose) self.timer_pose_est = TimerFps('Ess mat pose estimation', is_verbose=self.timer_verbose) self.timer_frame = TimerFps('Frame', is_verbose=self.timer_verbose) self.timer_seach_map = TimerFps('Search map', is_verbose=self.timer_verbose) self.init_history = True self.poses = [] # history of poses self.t0_est = None # history of estimated translations self.t0_gt = None # history of ground truth translations (if available) self.traj3d_est = [ ] # history of estimated translations centered w.r.t. first one self.traj3d_gt = [ ] # history of estimated ground truth translations centered w.r.t. first one self.cur_R = None # current rotation w.r.t. world frame self.cur_t = None # current translation w.r.t. world frame self.trueX, self.trueY, self.trueZ = None, None, None self.groundtruth = system.groundtruth # not actually used here; could be used for evaluating performances if kLogKFinfoToFile: self.kf_info_logger = Logging.setup_file_logger( 'kf_info_logger', 'kf_info.log', formatter=Logging.simple_log_formatter)
class Tracking(object): def __init__(self, system): if kDebugDrawMatches: Frame.is_store_imgs = True self.system = system self.camera = system.camera self.map = system.map self.local_mapping = system.local_mapping self.intializer = Initializer() self.motion_model = MotionModel( ) # motion model for current frame pose prediction without damping #self.motion_model = MotionModelDamping() # motion model for current frame pose prediction with damping self.dyn_config = SLAMDynamicConfig() self.descriptor_distance_sigma = Parameters.kMaxDescriptorDistance self.reproj_err_frame_map_sigma = Parameters.kMaxReprojectionDistanceMap self.max_frames_between_kfs = int(system.camera.fps) self.min_frames_between_kfs = 0 self.state = SlamState.NO_IMAGES_YET self.num_matched_kps = None # current number of matched keypoints self.num_inliers = None # current number of matched points self.num_matched_map_points = None # current number of matched map points (matched and found valid in current pose optimization) self.num_kf_ref_tracked_points = None # number of tracked points in k_ref (considering a minimum number of observations) self.mask_match = None self.pose_is_ok = False self.predicted_pose = None self.velocity = None self.f_cur = None self.idxs_cur = None self.f_ref = None self.idxs_ref = None self.kf_ref = None # reference keyframe (in general, different from last keyframe depending on the used approach) self.kf_last = None # last keyframe self.kid_last_BA = -1 # last keyframe id when performed BA self.local_keyframes = [] # local keyframes self.local_points = [] # local points self.tracking_history = TrackingHistory() self.timer_verbose = kTimerVerbose # set this to True if you want to print timings self.timer_main_track = TimerFps('Track', is_verbose=self.timer_verbose) self.timer_pose_opt = TimerFps('Pose optimization', is_verbose=self.timer_verbose) self.timer_seach_frame_proj = TimerFps('Search frame by proj', is_verbose=self.timer_verbose) self.timer_match = TimerFps('Match', is_verbose=self.timer_verbose) self.timer_pose_est = TimerFps('Ess mat pose estimation', is_verbose=self.timer_verbose) self.timer_frame = TimerFps('Frame', is_verbose=self.timer_verbose) self.timer_seach_map = TimerFps('Search map', is_verbose=self.timer_verbose) self.init_history = True self.poses = [] # history of poses self.t0_est = None # history of estimated translations self.t0_gt = None # history of ground truth translations (if available) self.traj3d_est = [ ] # history of estimated translations centered w.r.t. first one self.traj3d_gt = [ ] # history of estimated ground truth translations centered w.r.t. first one self.cur_R = None # current rotation w.r.t. world frame self.cur_t = None # current translation w.r.t. world frame self.trueX, self.trueY, self.trueZ = None, None, None self.groundtruth = system.groundtruth # not actually used here; could be used for evaluating performances if kLogKFinfoToFile: self.kf_info_logger = Logging.setup_file_logger( 'kf_info_logger', 'kf_info.log', formatter=Logging.simple_log_formatter) # estimate a pose from a fitted essential mat; # since we do not have an interframe translation scale, this fitting can be used to detect outliers, estimate interframe orientation and translation direction # N.B. read the NBs of the method estimate_pose_ess_mat(), where the limitations of this method are explained def estimate_pose_by_fitting_ess_mat(self, f_ref, f_cur, idxs_ref, idxs_cur): # N.B.: in order to understand the limitations of fitting an essential mat, read the comments of the method self.estimate_pose_ess_mat() self.timer_pose_est.start() # estimate inter frame camera motion by using found keypoint matches # output of the following function is: Trc = [Rrc, trc] with ||trc||=1 where c=cur, r=ref and pr = Trc * pc Mrc, self.mask_match = estimate_pose_ess_mat( f_ref.kpsn[idxs_ref], f_cur.kpsn[idxs_cur], method=cv2.RANSAC, prob=kRansacProb, threshold=kRansacThresholdNormalized) #Mcr = np.linalg.inv(poseRt(Mrc[:3, :3], Mrc[:3, 3])) Mcr = inv_T(Mrc) estimated_Tcw = np.dot(Mcr, f_ref.pose) self.timer_pose_est.refresh() # remove outliers from keypoint matches by using the mask computed with inter frame pose estimation mask_idxs = (self.mask_match.ravel() == 1) self.num_inliers = sum(mask_idxs) #print('# inliers: ', self.num_inliers ) idxs_ref = idxs_ref[mask_idxs] idxs_cur = idxs_cur[mask_idxs] # if there are not enough inliers do not use the estimated pose if self.num_inliers < kNumMinInliersEssentialMat: #f_cur.update_pose(f_ref.pose) # reset estimated pose to previous frame Printer.red('Essential mat: not enough inliers!') else: # use the estimated pose as an initial guess for the subsequent pose optimization # set only the estimated rotation (essential mat computation does not provide a scale for the translation, see above) #f_cur.pose[:3,:3] = estimated_Tcw[:3,:3] # copy only the rotation #f_cur.pose[:,3] = f_ref.pose[:,3].copy() # override translation with ref frame translation Rcw = estimated_Tcw[:3, :3] # copy only the rotation tcw = f_ref.pose[:3, 3] # override translation with ref frame translation f_cur.update_rotation_and_translation(Rcw, tcw) return idxs_ref, idxs_cur def pose_optimization(self, f_cur, name=''): #print('pose opt %s ' % (name) ) pose_before = f_cur.pose.copy() # f_cur pose optimization 1 (here we use f_cur pose as first guess and exploit the matched map points of f_ref ) self.timer_pose_opt.start() pose_opt_error, self.pose_is_ok, self.num_matched_map_points = optimizer_g2o.pose_optimization( f_cur, verbose=False) self.timer_pose_opt.pause() #print(' error^2: %f, ok: %d' % (pose_opt_error, int(self.pose_is_ok)) ) if not self.pose_is_ok: # if current pose optimization failed, reset f_cur pose f_cur.update_pose(pose_before) return self.pose_is_ok # track camera motion of f_cur w.r.t. f_ref def track_previous_frame(self, f_ref, f_cur): print('>>>> tracking previous frame ...') is_search_frame_by_projection_failure = False use_search_frame_by_projection = self.motion_model.is_ok and kUseSearchFrameByProjection and kUseMotionModel if use_search_frame_by_projection: # search frame by projection: match map points observed in f_ref with keypoints of f_cur #print('search frame by projection') search_radius = Parameters.kMaxReprojectionDistanceFrame f_cur.reset_points() self.timer_seach_frame_proj.start() idxs_ref, idxs_cur, num_found_map_pts = search_frame_by_projection( f_ref, f_cur, max_reproj_distance=search_radius, max_descriptor_distance=self.descriptor_distance_sigma) self.timer_seach_frame_proj.refresh() self.num_matched_kps = len(idxs_cur) #print("# matched map points in prev frame: %d " % self.num_matched_kps) # if not enough map point matches consider a larger search radius if self.num_matched_kps < Parameters.kMinNumMatchedFeaturesSearchFrameByProjection: f_cur.remove_frame_views(idxs_cur) f_cur.reset_points() idxs_ref, idxs_cur, num_found_map_pts = search_frame_by_projection( f_ref, f_cur, max_reproj_distance=2 * search_radius, max_descriptor_distance=0.5 * self.descriptor_distance_sigma) self.num_matched_kps = len(idxs_cur) #Printer.orange("# matched map points in prev frame (wider search): %d " % self.num_matched_kps) if kDebugDrawMatches and True: img_matches = draw_feature_matches(f_ref.img, f_cur.img, f_ref.kps[idxs_ref], f_cur.kps[idxs_cur], f_ref.sizes[idxs_ref], f_cur.sizes[idxs_cur], horizontal=False) cv2.imshow('tracking frame by projection - matches', img_matches) cv2.waitKey(1) if self.num_matched_kps < Parameters.kMinNumMatchedFeaturesSearchFrameByProjection: f_cur.remove_frame_views(idxs_cur) f_cur.reset_points() is_search_frame_by_projection_failure = True Printer.red( 'Not enough matches in search frame by projection: ', self.num_matched_kps) else: # search frame by projection was successful if kUseDynamicDesDistanceTh: self.descriptor_distance_sigma = self.dyn_config.update_descriptor_stat( f_ref, f_cur, idxs_ref, idxs_cur) # store tracking info (for possible reuse) self.idxs_ref = idxs_ref self.idxs_cur = idxs_cur # f_cur pose optimization 1: # here, we use f_cur pose as first guess and exploit the matched map point of f_ref self.pose_optimization(f_cur, 'proj-frame-frame') # update matched map points; discard outliers detected in last pose optimization num_matched_points = f_cur.clean_outlier_map_points() #print(' # num_matched_map_points: %d' % (self.num_matched_map_points) ) #print(' # matched points: %d' % (num_matched_points) ) if not self.pose_is_ok or self.num_matched_map_points < kNumMinInliersPoseOptimizationTrackFrame: Printer.red( 'failure in tracking previous frame, # matched map points: ', self.num_matched_map_points) self.pose_is_ok = False if not use_search_frame_by_projection or is_search_frame_by_projection_failure: self.track_reference_frame(f_ref, f_cur, 'match-frame-frame') # track camera motion of f_cur w.r.t. f_ref # estimate motion by matching keypoint descriptors def track_reference_frame(self, f_ref, f_cur, name=''): print('>>>> tracking reference %d ...' % (f_ref.id)) if f_ref is None: return # find keypoint matches between f_cur and kf_ref #print('matching keypoints with ', Frame.feature_matcher.type.name) self.timer_match.start() idxs_cur, idxs_ref = match_frames(f_cur, f_ref) self.timer_match.refresh() self.num_matched_kps = idxs_cur.shape[0] #print("# keypoints matched: %d " % self.num_matched_kps) if kUseEssentialMatrixFitting: # estimate camera orientation and inlier matches by fitting and essential matrix (see the limitations above) idxs_ref, idxs_cur = self.estimate_pose_by_fitting_ess_mat( f_ref, f_cur, idxs_ref, idxs_cur) if kUseDynamicDesDistanceTh: self.descriptor_distance_sigma = self.dyn_config.update_descriptor_stat( f_ref, f_cur, idxs_ref, idxs_cur) # propagate map point matches from kf_ref to f_cur (do not override idxs_ref, idxs_cur) num_found_map_pts_inter_frame, idx_ref_prop, idx_cur_prop = propagate_map_point_matches( f_ref, f_cur, idxs_ref, idxs_cur, max_descriptor_distance=self.descriptor_distance_sigma) #print("# matched map points in prev frame: %d " % num_found_map_pts_inter_frame) if kDebugDrawMatches and True: img_matches = draw_feature_matches(f_ref.img, f_cur.img, f_ref.kps[idx_ref_prop], f_cur.kps[idx_cur_prop], f_ref.sizes[idx_ref_prop], f_cur.sizes[idx_cur_prop], horizontal=False) cv2.imshow('tracking frame (no projection) - matches', img_matches) cv2.waitKey(1) # store tracking info (for possible reuse) self.idxs_ref = idxs_ref self.idxs_cur = idxs_cur # f_cur pose optimization using last matches with kf_ref: # here, we use first guess of f_cur pose and propated map point matches from f_ref (matched keypoints) self.pose_optimization(f_cur, name) # update matched map points; discard outliers detected in last pose optimization num_matched_points = f_cur.clean_outlier_map_points() #print(' # num_matched_map_points: %d' % (self.num_matched_map_points) ) #print(' # matched points: %d' % (num_matched_points) ) if not self.pose_is_ok or self.num_matched_map_points < kNumMinInliersPoseOptimizationTrackFrame: f_cur.remove_frame_views(idxs_cur) f_cur.reset_points() #Printer.red('failure in tracking reference %d, # matched map points: %d' %(f_ref.id,self.num_matched_map_points)) self.pose_is_ok = False # track camera motion of f_cur w.r.t. given keyframe # estimate motion by matching keypoint descriptors def track_keyframe(self, keyframe, f_cur, name='match-frame-keyframe'): f_cur.update_pose(self.f_ref.pose.copy() ) # start pose optimization from last frame pose self.track_reference_frame(keyframe, f_cur, name) def update_local_map(self): self.f_cur.clean_bad_map_points() #self.local_points = self.map.local_map.get_points() self.kf_ref, self.local_keyframes, self.local_points = self.map.local_map.get_frame_covisibles( self.f_cur) self.f_cur.kf_ref = self.kf_ref # track camera motion of f_cur w.r.t. the built local map # find matches between {local map points} (points in the built local map) and {unmatched keypoints of f_cur} def track_local_map(self, f_cur): if self.map.local_map.is_empty(): return print('>>>> tracking local map...') self.timer_seach_map.start() self.update_local_map() num_found_map_pts, reproj_err_frame_map_sigma, matched_points_frame_idxs = search_map_by_projection( self.local_points, f_cur, max_reproj_distance=self. reproj_err_frame_map_sigma, #Parameters.kMaxReprojectionDistanceMap, max_descriptor_distance=self.descriptor_distance_sigma, ratio_test=Parameters.kMatchRatioTestMap ) # use the updated local map self.timer_seach_map.refresh() #print('reproj_err_sigma: ', reproj_err_frame_map_sigma, ' used: ', self.reproj_err_frame_map_sigma) #print("# new matched map points in local map: %d " % num_found_map_pts) #print("# local map points ", self.map.local_map.num_points()) if kDebugDrawMatches and True: img_matched_trails = f_cur.draw_feature_trails( f_cur.img.copy(), matched_points_frame_idxs, trail_max_length=3) cv2.imshow('tracking local map - matched trails', img_matched_trails) cv2.waitKey(1) # f_cur pose optimization 2 with all the matched local map points self.pose_optimization(f_cur, 'proj-map-frame') f_cur.update_map_points_statistics( ) # here we do not reset outliers; we let them reach the keyframe generation # and then bundle adjustment will possible decide if remove them or not; # only after keyframe generation the outliers are cleaned! #print(' # num_matched_map_points: %d' % (self.num_matched_map_points) ) if not self.pose_is_ok or self.num_matched_map_points < kNumMinInliersPoseOptimizationTrackLocalMap: Printer.red( 'failure in tracking local map, # matched map points: ', self.num_matched_map_points) self.pose_is_ok = False #if kUseDynamicDesDistanceTh: # self.reproj_err_frame_map_sigma = self.dyn_config.update_reproj_err_map_stat(reproj_err_frame_map_sigma) # store frame history in order to retrieve the complete camera trajectory def update_tracking_history(self): if self.state == SlamState.OK: isometry3d_Tcr = self.f_cur.isometry3d * self.f_cur.kf_ref.isometry3d.inverse( ) # pose of current frame w.r.t. current reference keyframe kf_ref self.tracking_history.relative_frame_poses.append(isometry3d_Tcr) self.tracking_history.kf_references.append(self.kf_ref) self.tracking_history.timestamps.append(self.f_cur.timestamp) else: self.tracking_history.relative_frame_poses.append( self.tracking_history.relative_frame_poses[-1]) self.tracking_history.kf_references.append( self.tracking_history.kf_references[-1]) self.tracking_history.timestamps.append( self.tracking_history.timestamps[-1]) self.tracking_history.slam_states.append(self.state) def need_new_keyframe(self, f_cur): num_keyframes = self.map.num_keyframes() nMinObs = kNumMinObsForKeyFrameDefault if num_keyframes <= 2: nMinObs = 2 # if just two keyframes then we can have just two observations num_kf_ref_tracked_points = self.kf_ref.num_tracked_points( nMinObs) # number of tracked points in k_ref num_f_cur_tracked_points = f_cur.num_matched_inlier_map_points( ) # number of inliers in f_cur #Printer.purple('F(%d) #points: %d, KF(%d) #points: %d ' %(f_cur.id, num_f_cur_tracked_points, self.kf_ref.id, num_kf_ref_tracked_points)) if kLogKFinfoToFile: self.kf_info_logger.info( 'F(%d) #points: %d, KF(%d) #points: %d ' % (f_cur.id, num_f_cur_tracked_points, self.kf_ref.id, num_kf_ref_tracked_points)) self.num_kf_ref_tracked_points = num_kf_ref_tracked_points is_local_mapping_idle = self.local_mapping.is_idle() local_mapping_queue_size = self.local_mapping.queue_size() #print('is_local_mapping_idle: ', is_local_mapping_idle,', local_mapping_queue_size: ', local_mapping_queue_size) # condition 1: more than "max_frames_between_kfs" have passed from last keyframe insertion cond1 = f_cur.id >= (self.kf_last.id + self.max_frames_between_kfs) # condition 2: more than "min_frames_between_kfs" have passed and local mapping is idle cond2 = (f_cur.id >= (self.kf_last.id + self.min_frames_between_kfs)) & is_local_mapping_idle #cond2 = (f_cur.id >= (self.kf_last.id + self.min_frames_between_kfs)) # condition 3: few tracked features compared to reference keyframe cond3 = (num_f_cur_tracked_points < num_kf_ref_tracked_points * Parameters.kThNewKfRefRatio) and ( num_f_cur_tracked_points > Parameters.kNumMinPointsForNewKf) #print('KF conditions: %d %d %d' % (cond1, cond2, cond3) ) ret = (cond1 or cond2) and cond3 if ret: if is_local_mapping_idle: return True else: self.local_mapping.interrupt_optimization() if True: if local_mapping_queue_size <= 3: return True else: return False else: return False else: return False # @ main track method @ def track(self, img, frame_id, timestamp=None): Printer.cyan('@tracking') time_start = time.time() # check image size is coherent with camera params #print("img.shape: ", img.shape) #print("camera ", self.camera.height," x ", self.camera.width) assert img.shape[0:2] == (self.camera.height, self.camera.width) #if timestamp is not None: #print('timestamp: ', timestamp) self.timer_main_track.start() # build current frame self.timer_frame.start() f_cur = Frame(img, self.camera, timestamp=timestamp) self.f_cur = f_cur #print("frame: ", f_cur.id) self.timer_frame.refresh() # reset indexes of matches self.idxs_ref = [] self.idxs_cur = [] if self.state == SlamState.NO_IMAGES_YET: # push first frame in the inizializer self.intializer.init(f_cur) self.state = SlamState.NOT_INITIALIZED return # EXIT (jump to second frame) if self.state == SlamState.NOT_INITIALIZED: # try to inizialize initializer_output, intializer_is_ok = self.intializer.initialize( f_cur, img) if intializer_is_ok: kf_ref = initializer_output.kf_ref kf_cur = initializer_output.kf_cur # add the two initialized frames in the map self.map.add_frame( kf_ref) # add first frame in map and update its frame id self.map.add_frame( kf_cur) # add second frame in map and update its frame id # add the two initialized frames as keyframes in the map self.map.add_keyframe( kf_ref) # add first keyframe in map and update its kid self.map.add_keyframe( kf_cur) # add second keyframe in map and update its kid kf_ref.init_observations() kf_cur.init_observations() # add points in map new_pts_count, _, _ = self.map.add_points( initializer_output.pts, None, kf_cur, kf_ref, initializer_output.idxs_cur, initializer_output.idxs_ref, img, do_check=False) #Printer.green("map: initialized %d new points" % (new_pts_count)) # update covisibility graph connections kf_ref.update_connections() kf_cur.update_connections() # update tracking info self.f_cur = kf_cur self.f_cur.kf_ref = kf_ref self.kf_ref = kf_cur # set reference keyframe self.kf_last = kf_cur # set last added keyframe self.map.local_map.update(self.kf_ref) self.state = SlamState.OK self.update_tracking_history() self.motion_model.update_pose(kf_cur.timestamp, kf_cur.position, kf_cur.quaternion) self.motion_model.is_ok = False # after initialization you cannot use motion model for next frame pose prediction (time ids of initialized poses may not be consecutive) self.intializer.reset() if kUseDynamicDesDistanceTh: self.descriptor_distance_sigma = self.dyn_config.update_descriptor_stat( kf_ref, kf_cur, initializer_output.idxs_ref, initializer_output.idxs_cur) return # EXIT (jump to next frame) # get previous frame in map as reference f_ref = self.map.get_frame(-1) #f_ref_2 = self.map.get_frame(-2) self.f_ref = f_ref # add current frame f_cur to map self.map.add_frame(f_cur) self.f_cur.kf_ref = self.kf_ref # reset pose state flag self.pose_is_ok = False with self.map.update_lock: # check for map point replacements in previous frame f_ref (some points might have been replaced by local mapping during point fusion) self.f_ref.check_replaced_map_points() if kUseDynamicDesDistanceTh: #print('descriptor_distance_sigma: ', self.descriptor_distance_sigma) self.local_mapping.descriptor_distance_sigma = self.descriptor_distance_sigma # udpdate (velocity) old motion model # c1=ref_ref, c2=ref, c3=cur; c=cur, r=ref #self.velocity = np.dot(f_ref.pose, inv_T(f_ref_2.pose)) # Tc2c1 = Tc2w * Twc1 (predicted Tcr) #self.predicted_pose = g2o.Isometry3d(np.dot(self.velocity, f_ref.pose)) # Tc3w = Tc2c1 * Tc2w (predicted Tcw) # set intial guess for current pose optimization if kUseMotionModel and self.motion_model.is_ok: #print('using motion model for next pose prediction') # update f_ref pose according to its reference keyframe (the pose of the reference keyframe could be updated by local mapping) self.f_ref.update_pose( self.tracking_history.relative_frame_poses[-1] * self.f_ref.kf_ref.isometry3d) # predict pose by using motion model self.predicted_pose, _ = self.motion_model.predict_pose( timestamp, self.f_ref.position, self.f_ref.orientation) f_cur.update_pose(self.predicted_pose) else: #print('setting f_cur.pose <-- f_ref.pose') # use reference frame pose as initial guess f_cur.update_pose(f_ref.pose) # track camera motion from f_ref to f_cur self.track_previous_frame(f_ref, f_cur) if not self.pose_is_ok: # if previous track didn't go well then track the camera motion from kf_ref to f_cur self.track_keyframe(self.kf_ref, f_cur) # now, having a better estimate of f_cur pose, we can find more map point matches: # find matches between {local map points} (points in the local map) and {unmatched keypoints of f_cur} if self.pose_is_ok: self.track_local_map(f_cur) # end block {with self.map.update_lock:} # TODO: add relocalization # HACK: since local mapping is not fast enough in python (and tracking is not in real-time) => give local mapping more time to process stuff self.wait_for_local_mapping( ) # N.B.: this must be outside the `with self.map.update_lock:` block with self.map.update_lock: # update slam state if self.pose_is_ok: self.state = SlamState.OK else: self.state = SlamState.LOST Printer.red('tracking failure') # update motion model state self.motion_model.is_ok = self.pose_is_ok if self.pose_is_ok: # if tracking was successful # update motion model self.motion_model.update_pose(timestamp, f_cur.position, f_cur.quaternion) f_cur.clean_vo_map_points() # do we need a new KeyFrame? need_new_kf = self.need_new_keyframe(f_cur) if need_new_kf: #Printer.green('adding new KF with frame id % d: ' %(f_cur.id)) if kLogKFinfoToFile: self.kf_info_logger.info( 'adding new KF with frame id % d: ' % (f_cur.id)) kf_new = KeyFrame(f_cur, img) self.kf_last = kf_new self.kf_ref = kf_new f_cur.kf_ref = kf_new self.local_mapping.push_keyframe(kf_new) if not kLocalMappingOnSeparateThread: self.local_mapping.do_local_mapping() #else: #Printer.yellow('NOT KF') # From ORBSLAM2: # Clean outliers once keyframe generation has been managed: # we allow points with high innovation (considered outliers by the Huber Function) # pass to the new keyframe, so that bundle adjustment will finally decide # if they are outliers or not. We don't want next frame to estimate its position # with those points so we discard them in the frame. f_cur.clean_outlier_map_points() if self.f_cur.kf_ref is None: self.f_cur.kf_ref = self.kf_ref self.update_tracking_history( ) # must stay after having updated slam state (self.state) #Printer.green("map: %d points, %d keyframes" % (self.map.num_points(), self.map.num_keyframes())) #self.update_history() self.timer_main_track.refresh() duration = time.time() - time_start #print('tracking duration: ', duration) # Since we do not have real-time performances, we can slow-down things and make tracking wait till local mapping gets idle # N.B.: this function must be called outside 'with self.map.update_lock' blocks, # since both self.track() and the local-mapping optimization use the RLock 'map.update_lock' # => they cannot wait for each other once map.update_lock is locked (deadlock) def wait_for_local_mapping(self): if kTrackingWaitForLocalMappingToGetIdle: #while not self.local_mapping.is_idle() or self.local_mapping.queue_size()>0: if not self.local_mapping.is_idle(): print('>>>> waiting for local mapping...') self.local_mapping.wait_idle() else: if not self.local_mapping.is_idle( ) and kTrackingWaitForLocalMappingSleepTime > 0: print('>>>> sleeping for local mapping...') time.sleep(kTrackingWaitForLocalMappingSleepTime)
class ParticleFilter: # toggle robot driving in a circle in the simulator DRIVING = True # set to True if working in the simulator, else False SIM = True """ *** IF WORKING ON THE REAL ROBOT: *** - in params.yaml: - change particle_filter_frame from 'base_link_pf' to 'base_link' - change odom_topic from '/odom' to '/vesc/odom' - in localize.launch: - uncomment out region that initializes map server to use stata basement map - use SIM = False and DRIVING = False """ def __init__(self): # using locks so threads can't access self.particles at the same time (avoiding race condition) self.lock = Lock() self.lock.acquire() # Get parameters self.particle_filter_frame = \ rospy.get_param("~particle_filter_frame") # should be '/base_link_pf' in sim and '/base_link' on car # Initialize the models self.motion_model = MotionModel() self.sensor_model = SensorModel() # Topics self.odom_topic = rospy.get_param("~odom_topic") self.scan_topic = rospy.get_param("~scan_topic") self.particle_marker_topic = '/particle_marker' self.click_topic = '/move_base_simple/goal' self.pose_array_topic = '/pose_array' self.inferred_pose_topic = '/inferred_pose' self.error_x_topic = '/error_x' self.error_y_topic = '/error_y' self.error_t_topic = '/error_t' # Subscribers and Publishers rospy.Subscriber( self.odom_topic, Odometry, self.odom_callback) # '/odom' for sim and 'vesc/odom' for car rospy.Subscriber(self.scan_topic, LaserScan, self.scan_callback) self.tf_broadcaster = TransformBroadcaster() # Listening to clicks in rviz and publishing particle markers to rviz rospy.Subscriber(self.click_topic, PoseStamped, self.click_callback) self.particle_marker_pub = rospy.Publisher(self.particle_marker_topic, Marker, queue_size=1) self.pose_array_pub = rospy.Publisher(self.pose_array_topic, PoseArray, queue_size=1) self.inferred_pose_pub = rospy.Publisher(self.inferred_pose_topic, Marker, queue_size=1) self.error_x_pub = rospy.Publisher(self.error_x_topic, Float32, queue_size=1) self.error_y_pub = rospy.Publisher(self.error_y_topic, Float32, queue_size=1) self.error_t_pub = rospy.Publisher(self.error_t_topic, Float32, queue_size=1) self.rate = rospy.Rate(30) # should be greater than 20 Hz # Implement the MCL algorithm # using the sensor model and the motion model # # Make sure you include some way to initialize # your particles, ideally with some sort # of interactive interface in rviz # # Publish a transformation frame between the map # and the particle_filter_frame. # Initializing particles and weights self.num_particles = rospy.get_param('~num_particles') # currently intializing all particles as origin self.particles = np.zeros((self.num_particles, 3)) # all particles are initially equally likely self.weights = np.full((self.num_particles), 1. / self.num_particles) # for error calculations self.groundtruth = [0, 0, 0] # send drive commands to test in sim self.drive_pub = rospy.Publisher('/drive', AckermannDriveStamped, queue_size=1) self.drive_msg = AckermannDriveStamped() self.drive_msg.drive.steering_angle = 0 self.drive_msg.drive.speed = 1 if self.SIM == False: # don't toggle on driving if working on real robot self.DRIVE = False self.publish_transform() self.lock.release() def odom_callback(self, odom_data): ''' whenever you get odometry data, use the motion model to update the particle positions determine the "average" particle pose and publish that transform :param odom_data: odometry data (type - Odometry) :return: no return, but updates self.particles positions and publishes transform ''' self.lock.acquire() #start = time() x = odom_data.pose.pose.position.x y = odom_data.pose.pose.position.y q_w = odom_data.pose.pose.orientation.w q_z = odom_data.pose.pose.orientation.z euler = transformations.euler_from_quaternion( np.array([0, 0, q_z, q_w])) self.groundtruth = [x, y, euler] self.particles = self.motion_model.evaluate( self.particles, self.motion_model.get_delta_x(odom_data), noise=0.2) self.publish_transform() #end = time() #print('odom_callback time:', end-start) self.lock.release() def scan_callback(self, scan_data): ''' whenever you get sensor data, use the sensor model to compute the particle probabilities. Then resample the particles based on these probabilities determine the "average" particle pose and publish that transform :param scan_data: scan data (type - LaserScan) :return: no return, but updates self.particles by resampling particles and publishes transform ''' self.lock.acquire() probabilities = self.sensor_model.evaluate(self.particles, scan_data, downsample=20, sim=self.SIM) # if map wasn't initialized in sensor_model, probabilites=None if isinstance(probabilities, type(None)): self.lock.release() rospy.loginfo('no probabilities to use in scan_callback') return smooth_p = np.power(probabilities, [1 / 3.] * len(probabilities)) self.weights = smooth_p / np.sum( smooth_p) # normalize weights so they sum to 1 # resample particles based on weights self.particles = self.resample_particles() self.publish_transform() self.lock.release() def resample_particles(self): ''' reseample particles based on their probability weights allow repeats of particles; to not allow, use replace=False ''' # choose with indices in particles array to keep using probabilities as weights indices = np.random.choice(self.num_particles, self.num_particles, p=self.weights) # return particles with chosen indices return self.particles[indices] def publish_transform(self): ''' anytime the particles are updated, determine the 'average' particle pose and broadcast that transform :return: no return, but broadcast transform between 'map' frame and self.particle_filter_frame ''' x_avg, y_avg, theta_avg = self.avg_pose() q = transformations.quaternion_from_euler(0, 0, theta_avg) # broadcast transformation self.tf_broadcaster.sendTransform( (x_avg, y_avg, 0), q, rospy.Time.now(), self.particle_filter_frame, 'map') self.publish_pose_array() #self.publish_particle_markers() # Driving in the simulator if self.DRIVING: self.drive_pub.publish(self.drive_msg) # publish inferred position in rviz as arrow if self.inferred_pose_pub.get_num_connections > 0: a = Marker() a.header.frame_id = 'map' a.type = Marker.ARROW a.color = ColorRGBA(0, 1, 1, 1) a.scale.x = 0.5 a.scale.y = 0.1 a.scale.z = 0.1 q = transformations.quaternion_from_euler(0, 0, theta_avg) quat = Quaternion() quat.x = q[0] quat.y = q[1] quat.z = q[2] quat.w = q[3] a.pose.position.x = x_avg a.pose.position.y = y_avg a.pose.orientation = quat self.inferred_pose_pub.publish(a) # for error plots try: x, y, theta = self.groundtruth x_error = x_avg - x y_error = y_avg - y t_error = theta_avg - theta[2] self.error_x_pub.publish(x_error) self.error_y_pub.publish(y_error) self.error_t_pub.publish(t_error) except Exception as e: rospy.loginfo(e) def avg_pose(self): ''' determine the average particle pose ** have to use mean of circular quantities for theta ** distirbution might be multi modal :param: no params, but uses instance variable self.particles :return: average particle pose as [x, y, theta] ''' x = np.mean(self.particles[:, 0]) y = np.mean(self.particles[:, 1]) # circular mean is angle between average x component and y component of the angles data theta = np.arctan2(np.mean(np.sin(self.particles[:, 2])), np.mean(np.cos(self.particles[:, 2]))) return [x, y, theta] def click_callback(self, pose_data): ''' use click in rviz to initialize particles :param pose_data: pose of rviz click (type - PoseStamped) use 2D Nav Goal to guess car location :return: no return, but updates self.particles ''' self.lock.acquire() x = pose_data.pose.position.x y = pose_data.pose.position.y q = pose_data.pose.orientation theta = transformations.euler_from_quaternion([q.x, q.y, q.z, q.w])[2] # random points from normal distribution centered at [x, y, theta] self.particles = np.random.normal([x, y, theta], 1, (self.num_particles, 3)) self.weights = np.full((self.num_particles), 1. / self.num_particles) self.publish_pose_array() #self.publish_particle_markers() self.lock.release() def publish_particle_markers(self): ''' publish particles as markers in rviz ''' # if something is listening for marker publications if self.particle_marker_pub.get_num_connections > 0: m = Marker() m.header.frame_id = 'map' m.type = Marker.POINTS #m.color = ColorRGBA(1, 0, 0, 1) m.scale.x = 0.2 m.scale.y = 0.2 m.scale.z = 0.2 for i in range(len(self.particles)): p = self.particles[i] w = self.weights[i] m.points.append(Point(p[0], p[1], 0)) m.colors.append(self.prob_to_color(w)) self.particle_marker_pub.publish(m) def prob_to_color(self, prob): ''' convert a probability to a color red is high probability orange yellow green cyan light blue blue purple magenta black is low :param prob: probability between 0 and 1 :return: ColorRBGA corresponding to prob ''' m = max(self.weights) # max weight if prob > m * 0.9: return ColorRGBA(1, 0, 0, 1) if prob > m * 0.8: return ColorRGBA(1, 0.5, 0, 1) if prob > m * 0.7: return ColorRGBA(1, 1, 0, 1) if prob > m * 0.6: return ColorRGBA(0, 1, 0, 1) if prob > m * 0.5: return ColorRGBA(0, 1, 1, 1) if prob > m * 0.4: return ColorRGBA(0, 0.5, 1, 1) if prob > m * 0.3: return ColorRGBA(0, 0, 1, 1) if prob > m * 0.2: return ColorRGBA(0.5, 0, 1, 1) if prob > m * 0.1: return ColorRGBA(1, 0, 1, 1) else: return ColorRGBA(0, 0, 0, 1) def publish_pose_array(self): ''' publish arrows in rviz for to visualize particles ''' particles = np.copy(self.particles) if self.pose_array_topic > 0: pa = PoseArray() pa.header.frame_id = 'map' for p in particles[:20]: pose = Pose() pose.position = Point(p[0], p[1], 0) q = transformations.quaternion_from_euler(0, 0, p[2]) quat = Quaternion() quat.x = q[0] quat.y = q[1] quat.z = q[2] quat.w = q[3] pose.orientation = quat pa.poses.append(pose) self.pose_array_pub.publish(pa)
help='Debug mode, only send out 10 beams') parser.add_argument('--adaptive', action='store_true', help='Adaptively select particles or not') args = parser.parse_args() src_path_map = args.path_to_map src_path_log = args.path_to_log os.makedirs(args.output, exist_ok=True) map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() with open(src_path_log, 'r') as f: logfile = f.readlines() motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() num_particles = args.num_particles X_bar = init_particles_freespace(num_particles, occupancy_map) num_beams = 10 if args.debug else 18 """ Monte Carlo Localization Algorithm : Main Loop """ if args.visualize: visualize_map(occupancy_map) visualize_timestep(X_bar, 0, args.output) first_time_idx = True
parser.add_argument('--path_to_map', default='../data/map/wean.dat') parser.add_argument('--path_to_log', default='../data/log/robotdata1.log') parser.add_argument('--output', default='results') parser.add_argument('--num_particles', default=500, type=int) parser.add_argument('--visualize', action='store_true') args = parser.parse_args() src_path_map = args.path_to_map src_path_log = args.path_to_log os.makedirs(args.output, exist_ok=True) map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() logfile = open(src_path_log, 'r') motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() num_particles = args.num_particles X_bar = init_particles_random(num_particles, occupancy_map) # X_bar = init_particles_freespace(num_particles, occupancy_map) """ Monte Carlo Localization Algorithm : Main Loop """ if args.visualize: visualize_map(occupancy_map) first_time_idx = True for time_idx, line in enumerate(logfile):
class MotionSub(): def __init__(self): self.modelMotion = MotionModel() self.controlFilter = ColorFiltering() self.controlPath = ControllerPath() self.entityDocument = EntityDocument() self.listFile = [] self.folder = "result_dir" self.burnin = 0 self.scan = 0 self.frame_rate = 0 self.def_area = 'draw' self.setROI = False self.includedROI = None self.set_areaCounter = False self.fileDest = " " self.play = False #folder destination self.folder_dest = "hasilUji" self.file_dest = '' self.saveVid = "both" self.minVal = [] self.maxVal = [] self.path_time = "/home/pi/coding/fire_detect/nilai_HSV.csv" def askFile(self): self.answer = askquestion('choose file', 'Input Video ?') if self.answer == 'yes': self.frame_seq = True self.pathname = askopenfilename() else: self.frame_seq = False self.pathname = askdirectory() self.images = True return self.pathname def readVids(self): ''' method ini digunakan unutk melakukan ekstraksi terhadap video yang kemungkinan akan diolah di proses filtering dan motion apabila menggunakan image sequence ''' #method to get input from user self.askFile() os.mkdir(self.folder) cap = cv2.VideoCapture(self.pathname) #cap.set(cv2.CAP_PROP_POS_MSEC,5000) #Skip to 5s self.count = 0 while True: #start reading frame ret, frame = cap.read() if not ret: break #set the name of image names = os.path.join(self.folder, "img%d.jpg" % self.count) cv2.imwrite(names, frame) self.listFile.append(names) self.count += 1 return self.listFile def preProc(self, lblImg): ''' this method provide to recognizing input file, set ROI for processing image set learning rate, threshold value,initiliaze background''' #set stamp lidt self.stamp = [] #write header row for stamp self.stamp.append(("Time", "Frame", 'Position')) #capture output for area detectiom self.result_img = [] #capture output self.frameHasil = 0 ##No contours, there was not enough motion compared to background, did not meet threshold self.noCountr = 0 #capture minimumbox size for plotting self.avg_box = [] #Empty list for area counter self.areaCounter = [] self.areaCounter.append(("Time", "Frame", "X", "Y")) #to set draw ROI self.top = 0 self.bottom = 1 self.left = 0 self.right = 1 #flag on self.noMotion = False #label Image self.labelImg = lblImg ################################# #Begin video capture ################################# #if self.frame_seq is False means using video if self.frame_seq: ext = os.path.splitext(self.pathname)[1] #get from video capture self.cap = cv2.VideoCapture(self.pathname) #get global frame rate self.frame_rate = round(self.cap.get(5)) #make sure extension is .avi if ext in ['.avi', '.AVI']: #self.frame_rate=1 print "frame rate set to 1" else: showinfo(title="Info", message="make sure your file is .avi extension") return 0 #get frame related to time of begin video self.frame_time = self.cap.get(0) #get total number of frame self.total_frames = self.cap.get(7) #skip x frame according to user input for x in range(1, int(float(self.burnin) * int(self.frame_rate) * 60)): self.cap.grab() #set frame skip counter self.frameSkip = 0 #get the lower and upper mask of video self.low = self.controlFilter.getLower() self.up = self.controlFilter.getUpper() print '{0}-{1}'.format(self.low, self.up) #capture the first image self.cap.set(1, 1) ret, firstImage = self.cap.read() #if not read if not ret: raise ValueError("no file image capture") #convert background to be like mask filtering rgbImg = self.controlFilter.bgr2rgb(firstImage) hsvImg = self.controlFilter.rgb2hsv(rgbImg) firstImage = self.controlFilter.filterMask(hsvImg, self.low, self.up) print "begin motion detection" else: #self.frame_sequence is True using image sequence self.cap = cv2.VideoCapture(self.pathname) #begin read file image sequence image_ext = ["*.jpg", "*.jpeg", "*.png"] pathImg = [os.path.join(self.pathname, x) for x in image_ext] self.jpgs = [] for extension in pathImg: ketemu = glob.glob(extension) self.jpgs.extend(ketemu) print("banyaknya image di folder : %d" % len(self.jpgs)) #set the first image as initialize background firstImage = cv2.imread(self.jpgs[0]) #convert to gray Img #self.showImg=cv2.cvtColor(firstImage,cv2.COLOR_BGR2GRAY) self.total_frames = len(self.jpgs) self.frame_rate = 1 #make copy for markup imageCopy = firstImage.copy() #self.includeRoi = raw_input(str("include ROI?")) #set ROI if self.setROI: self.selectedRoi = draw_controller.Udef(imageCopy, "Region of Interest") self.selectedRoi = self.selectedRoi[-4:] if len(self.selectedRoi) == 0: raise ValueError('Error:please select an area to be ROI') if self.includedROI == "include": print "cropping frame...complete" self.showImg = firstImage[ self.selectedRoi[1]:self.selectedRoi[1] + self.selectedRoi[3], self.selectedRoi[0]:self.selectedRoi[0] + self.selectedRoi[2]] print self.showImg.shape[0:2] else: firstImage[self.selectedRoi[1]:self.selectedRoi[3], self.selectedRoi[0]:self.selectedRoi[2]] = 255 self.showImg = firstImage else: self.showImg = firstImage #show image if self.setROI: cv2.imshow("ROI area", self.showImg) #cv2.imshow("user self area",self.cropImg) cv2.waitKey(5) cv2.destroyAllWindows() self.controlFilter.displayinTk(self.showImg, self.labelImg) self.width = np.size(self.showImg, 1) self.height = np.size(self.showImg, 0) frameSize = (self.width, self.height) print frameSize return self.showImg #=======================================# #compute background selama video looping #=======================================# def running(self, pathname, board, alpha, thresh, minVal, maxVal, imgLabel, mask, cap, roiLabel): self.mask = mask self.learningRate = alpha self.threshVal = thresh self.cap = cap start = time.time() self.images = False self.minVal = minVal self.maxVal = maxVal self.pathname = pathname self.times = [] self.backgroundInit = backSubs.Background(self.learningRate, self.mask, self.threshVal) #print self.backgroundInit[10:15,25:30] #hitung jumlah frames self.frame_total = 0 self.total_frames = 0 try: #read video input ret, oriFrame = self.cap.read() if not ret: print '=========last frame reach=======' return 0 #convert current frame to be like mask filtering rgbImg_cf = self.controlFilter.bgr2rgb(oriFrame) hsvImg_cf = self.controlFilter.rgb2hsv(rgbImg_cf) #put mask filtering image here currentFrame = self.controlFilter.filterMask( hsvImg_cf, self.minVal, self.maxVal) #if not the last frame, scan frames if not self.scan == 0: if self.noMotion: for x in range(1, self.scan): if not self.images: self.cap.grab() else: oriFrame = jpgs.pop() self.frame_total = self.frame_count + 1 else: print "theres no motion in video" return 0 else: #dibuat pass karena self.scan !=0 maka terbaca dan dilanjutkan k proses slnjutnya pass #capture from video if self.images is False: #skip read frame every n second frameId = int(round( self.cap.get(1))) #current frame per second t_frame = int(self.cap.get(0) / 1000) #current time fo frame #print "frame %d detik ke-%d" %(frameId,t_frame) else: currentFrame = cv2.imread(self.jpgs.pop()) for count, item in enumerate(self.jpgs): print 'frame ke-{0}'.format(count) self.frame_total += 1 #frame_t0=time.time() #============================# #background substraction #============================# #self.start = time.time() #do background substraction here-> foreground identification grayImg = self.backgroundInit.getForeground(currentFrame) #print grayImg[10:15,25:30] #if set Roi, subset the image if self.setROI: if self.includedROI == "include": mask = np.ones(grayImg.shape, np.bool) mask[self.selectedRoi[1]:self.selectedRoi[1] + self.selectedRoi[3], self.selectedRoi[0]:self.selectedRoi[0] + self.selectedRoi[2]] = False grayImg[mask] = 0 else: mask = cv2.bitwise_and(currentFrame, currentFrame, mask=grayImg) grayImg[mask] = 0 #==========================# #contour analysis and post procesing #==========================# coords = [] bbox_list = [] counter = [] #Empty list for position stamp stamp_position = [] imgCopy = grayImg.copy() #calculate white pixel as motion data _, contours, _ = cv2.findContours(imgCopy, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) length = len(contours) maxArea = -1 i = 0 # define minimum area Rect and minimum Enclosing circle for c in contours: #get bounding Rect rect = cv2.boundingRect(c) #set contour smaller if rect[2] < 50 or rect[3] < 50: continue point = rect #print "{0}x{1} + {2}x{3}".format(point[0],point[1],point[2],point[3]) self.setRoi(point[0], point[1], point[2], point[3], rgbImg_cf) #get minimum area rect #rect=cv2.minAreaRect(c) #box=cv2.boxPoints(rect) #print type(box) #convert all coord floating point to int #box=np.int0(box) #write to file frame has detected contour self.modelMotion.writeToFile(self.pathname, rgbImg_cf, frameId) #contours for imgContour = cv2.drawContours(imgCopy, contours, -1, (255, 0, 0), 1) #display in Tkinter format self.controlFilter.displayinTk(rgbImg_cf, imgLabel) self.controlFilter.displayinTk(imgCopy, roiLabel) board.after(100, func=lambda: self. running(pathname, board, alpha, thresh, minVal, maxVal, imgLabel, mask, self.cap, roiLabel)) #set for time except Exception, e: print str(e) end = time.time() - start self.times = [ "alpha: %.4f, frame %d detik ke-%d, lama:%.2f s" % (self.learningRate, frameId, t_frame, end) ] self.entityDocument.csv_writer(self.times, self.path_time)
class ParticleFilter(object): """ This is the main particle filter object. """ def __init__(self, map_file='../data/map/wean.dat'): self.map = Map(map_file) #self.map.display_gaussian([], 'Gaussian Blurred Map') self._number_particles = 1000 self.particles = list() self._particle_probabilities = [] for _ in range(0, self._number_particles): print 'Initializing particle', _ particle = Particle(self.map) #particle.x = 4080 + np.random.normal(scale=35) #particle.y = 3980 + np.random.normal(scale=15) #particle.theta = math.pi + 0.1 + np.random.normal(scale=.1) self.particles.append(particle) self._particle_probabilities.append(1) self._motion_model = MotionModel(0.001, 0.001, 0.1, 0.1) self._sensor_model = SensorModel(self.map) self._ranges = [] def log(self, file_handle): line = list() for particle in self.particles: loc = str(particle.x) + ',' + str(particle.y) line.append(loc) file_handle.write(';'.join(line)) file_handle.write('\n') def display(self): self.map.display(self.particles, ranges=self._ranges) def update(self, line): """ Update step. Reading is a single reading (i.e. line) from the log file Could be either an odometry or laser reading """ measurement_type = line[0] # O = odometry, L = laser scan measurements = np.fromstring(line[2:], sep=' ') odometry = measurements[0:3] time_stamp = measurements[-1] # last variable for particle in self.particles: self._motion_model.update(particle, odometry, time_stamp) if measurement_type == "L": odometry_laser = measurements[3:6] # coordinates of the laser in standard odometry frame self._ranges = measurements[6:-1] # exclude last variable, the time stamp self._particle_probabilities = list() # unnormalized sensor model probabilities of the particles for particle in self.particles: self._particle_probabilities.append(self._sensor_model.get_probability(particle, self._ranges)) argsorted_probabilities = np.argsort(-np.asarray(self._particle_probabilities)) self.particles[argsorted_probabilities[0]].debug = True self.particles[argsorted_probabilities[1]].debug = True self.particles[argsorted_probabilities[2]].debug = True def _resample(self): """ Resamples the particles given unnormalized particle probabilites """ particle_probabilities = np.asarray(self._particle_probabilities)/sum(self._particle_probabilities) # normalize indices = np.random.choice(range(0, self._number_particles), size=self._number_particles, replace=True, p=particle_probabilities) indices.sort() previous_index = -1 new_particles = list() for index in indices: if index != previous_index: new_particles.append(self.particles[index]) else: new_particles.append(deepcopy(self.particles[index])) previous_index = index self.particles = new_particles
class ParticleFilter(object): """ This is the main particle filter object. """ def __init__(self, map_file='../data/map/wean.dat'): self.map = Map(map_file) #self.map.display_gaussian([], 'Gaussian Blurred Map') self._number_particles = 1000 self.particles = list() self._particle_probabilities = [] for _ in range(0, self._number_particles): print 'Initializing particle', _ particle = Particle(self.map) #particle.x = 4080 + np.random.normal(scale=35) #particle.y = 3980 + np.random.normal(scale=15) #particle.theta = math.pi + 0.1 + np.random.normal(scale=.1) self.particles.append(particle) self._particle_probabilities.append(1) self._motion_model = MotionModel(0.001, 0.001, 0.1, 0.1) self._sensor_model = SensorModel(self.map) self._ranges = [] def log(self, file_handle): line = list() for particle in self.particles: loc = str(particle.x) + ',' + str(particle.y) line.append(loc) file_handle.write(';'.join(line)) file_handle.write('\n') def display(self): self.map.display(self.particles, ranges=self._ranges) def update(self, line): """ Update step. Reading is a single reading (i.e. line) from the log file Could be either an odometry or laser reading """ measurement_type = line[0] # O = odometry, L = laser scan measurements = np.fromstring(line[2:], sep=' ') odometry = measurements[0:3] time_stamp = measurements[-1] # last variable for particle in self.particles: self._motion_model.update(particle, odometry, time_stamp) if measurement_type == "L": odometry_laser = measurements[ 3:6] # coordinates of the laser in standard odometry frame self._ranges = measurements[ 6:-1] # exclude last variable, the time stamp self._particle_probabilities = list( ) # unnormalized sensor model probabilities of the particles for particle in self.particles: self._particle_probabilities.append( self._sensor_model.get_probability(particle, self._ranges)) argsorted_probabilities = np.argsort( -np.asarray(self._particle_probabilities)) self.particles[argsorted_probabilities[0]].debug = True self.particles[argsorted_probabilities[1]].debug = True self.particles[argsorted_probabilities[2]].debug = True def _resample(self): """ Resamples the particles given unnormalized particle probabilites """ particle_probabilities = np.asarray( self._particle_probabilities) / sum( self._particle_probabilities) # normalize indices = np.random.choice(range(0, self._number_particles), size=self._number_particles, replace=True, p=particle_probabilities) indices.sort() previous_index = -1 new_particles = list() for index in indices: if index != previous_index: new_particles.append(self.particles[index]) else: new_particles.append(deepcopy(self.particles[index])) previous_index = index self.particles = new_particles
def __init__(self): # using locks so threads can't access self.particles at the same time (avoiding race condition) self.lock = Lock() self.lock.acquire() # Get parameters self.particle_filter_frame = \ rospy.get_param("~particle_filter_frame") # should be '/base_link_pf' in sim and '/base_link' on car # Initialize the models self.motion_model = MotionModel() self.sensor_model = SensorModel() # Topics self.odom_topic = rospy.get_param("~odom_topic") self.scan_topic = rospy.get_param("~scan_topic") self.particle_marker_topic = '/particle_marker' self.click_topic = '/move_base_simple/goal' self.pose_array_topic = '/pose_array' self.inferred_pose_topic = '/inferred_pose' self.error_x_topic = '/error_x' self.error_y_topic = '/error_y' self.error_t_topic = '/error_t' # Subscribers and Publishers rospy.Subscriber( self.odom_topic, Odometry, self.odom_callback) # '/odom' for sim and 'vesc/odom' for car rospy.Subscriber(self.scan_topic, LaserScan, self.scan_callback) self.tf_broadcaster = TransformBroadcaster() # Listening to clicks in rviz and publishing particle markers to rviz rospy.Subscriber(self.click_topic, PoseStamped, self.click_callback) self.particle_marker_pub = rospy.Publisher(self.particle_marker_topic, Marker, queue_size=1) self.pose_array_pub = rospy.Publisher(self.pose_array_topic, PoseArray, queue_size=1) self.inferred_pose_pub = rospy.Publisher(self.inferred_pose_topic, Marker, queue_size=1) self.error_x_pub = rospy.Publisher(self.error_x_topic, Float32, queue_size=1) self.error_y_pub = rospy.Publisher(self.error_y_topic, Float32, queue_size=1) self.error_t_pub = rospy.Publisher(self.error_t_topic, Float32, queue_size=1) self.rate = rospy.Rate(30) # should be greater than 20 Hz # Implement the MCL algorithm # using the sensor model and the motion model # # Make sure you include some way to initialize # your particles, ideally with some sort # of interactive interface in rviz # # Publish a transformation frame between the map # and the particle_filter_frame. # Initializing particles and weights self.num_particles = rospy.get_param('~num_particles') # currently intializing all particles as origin self.particles = np.zeros((self.num_particles, 3)) # all particles are initially equally likely self.weights = np.full((self.num_particles), 1. / self.num_particles) # for error calculations self.groundtruth = [0, 0, 0] # send drive commands to test in sim self.drive_pub = rospy.Publisher('/drive', AckermannDriveStamped, queue_size=1) self.drive_msg = AckermannDriveStamped() self.drive_msg.drive.steering_angle = 0 self.drive_msg.drive.speed = 1 if self.SIM == False: # don't toggle on driving if working on real robot self.DRIVE = False self.publish_transform() self.lock.release()
def __init__(self, motion_model_noise_params, measurement_model_params, filter_params, measurement_generator_callback): self.filter_params = copy(filter_params) self.motion_model = MotionModel(motion_model_noise_params) self.measurement_model = MeasurementModel(measurement_model_params, measurement_generator_callback) self.particles = list() self.generate_new_particle_set()
def verify_rotation_matrices(t_start=0, t_end=1): motion_model = MotionModel() # Get real ankle angle data ankle_data = load_data('./data/ankle_vs_gait.csv') ankle_data = np.array(ankle_data) ankle_data = get_regress_general(ankle_data) # Get real ankle height data ankle_height = load_data('./data/Foot-Centroid-Height_OG-vs-Gait.csv') ankle_height = np.array(ankle_height) ankle_height = np.transpose(ankle_height) ankle_height[0] = ankle_height[0] / 5 ankle_height[1] = ankle_height[1] / 1000 # Get real ankle horizontal data ankle_hor = load_data('./data/Foot-Centroid-Horizontal_OG-vs-Gait.csv') ankle_hor = np.array(ankle_hor) ankle_hor = np.transpose(ankle_hor) ankle_hor[0] = ankle_hor[0] / 5 ankle_hor[1] = ankle_hor[1] / 1000 x = np.arange(t_start, t_end, .01) position = [[], []] for ite in x: coord = motion_model.get_global( ankle_data.eval(ite * 100)[0] * np.pi / 180, 0.06674, -0.03581, ite) #gets global coordinate of ankle position[0].append(coord[0]) position[1].append(coord[1]) # Plot ankle from literature plt.figure() plt.plot(x * 100, ankle_data.eval(x * 100) * np.pi / 180) plt.xlabel("% Gait Cycle") plt.ylabel("Ankle Angle Literature (rad)") plt.show() # Plot global horizontal of centroid plt.figure() plt.plot(ankle_hor[0][:-5] * 100, ankle_hor[1][:-5]) plt.plot(x * 100, position[0], '--') plt.legend(('Raw Data', 'Computed Trajectory')) plt.xlabel("% Gait Cycle") plt.ylabel("Horizontal Position (m)") plt.title("Horizontal Position of the COM over the Gait Cycle") plt.show() # Plot global vertical of centroid plt.figure() plt.plot(ankle_height[0][:-5] * 100, ankle_height[1][:-5]) plt.plot(x * 100, position[1], '--') plt.legend(('Raw Data', 'Computed Trajectory')) plt.xlabel("% Gait Cycle") plt.ylabel("Vertical Position (m)") plt.title("Vertical Position of the COM over the Gait Cycle") plt.show() # Plot phase portraits of centroid plt.figure() plt.plot(ankle_hor[1][:-5], ankle_height[1][:-5]) plt.plot(position[0], position[1], '--') # plt.scatter(position[0][0], position[1][0], marker='x', color='r') # plt.text(position[0][0], position[1][0], 'start') # plt.scatter(position[0][-1], position[1][-1], marker='x', color='g') # plt.text(position[0][-1], position[1][-1], 'end') # plt.scatter(ankle_hor[1][0], ankle_height[1][0], marker='x', color='r') # plt.text(ankle_hor[1][0], ankle_height[1][0], 'start') # plt.scatter(ankle_hor[1][-1], ankle_height[1][-1], marker='x', color='g') # plt.text(ankle_hor[1][-1], ankle_height[1][-1], 'end') plt.legend(('Raw Data', 'Computed Trajectory')) plt.xlabel("Horizontal Position (m)") plt.ylabel("Vertical Position (m)") plt.title("Phase Portrait of Centroid Trajectory over the Gait Cycle") plt.show()
class ParticleFilter(object): '''Defines a utility for localising a robot using a particle filter. Author -- Aleksandar Mitrevski ''' def __init__(self, motion_model_noise_params, measurement_model_params, filter_params, measurement_generator_callback): self.filter_params = copy(filter_params) self.motion_model = MotionModel(motion_model_noise_params) self.measurement_model = MeasurementModel(measurement_model_params, measurement_generator_callback) self.particles = list() self.generate_new_particle_set() def iterate_filter(self, motion_command, measurements): '''Performs an update of the filter with the given motion command and set of sensor measurements. Keyword arguments: motion_command -- A 'velocity.Velocity' object. measurements -- A dictionary where each key is a name of a sensor frame and the values are measurements obtained with the respective sensor. ''' self.move_particles(motion_command) resample = self.update_particle_weights(measurements) if resample: self.particles = self.resample_particles() def move_particles(self, motion_command): '''Moves all particles based on the given motion command. Keyword arguments: motion_command -- A 'velocity.Velocity' object. ''' for i in xrange(self.filter_params.number_of_particles): self.particles[i].pose = self.motion_model.sample_motion_model(self.particles[i].pose, motion_command) def update_particle_weights(self, measurements): '''Updates the particle weights based on the given sensor measurements. Keyword arguments: measurements -- A dictionary where each key is a name of a sensor frame and the values are measurements obtained with the respective sensor. ''' weight_sum = 0. for i in xrange(self.filter_params.number_of_particles): self.particles[i].weight = self.measurement_model.calculate_measurement_likelihood(self.particles[i].pose, measurements) weight_sum = weight_sum + self.particles[i].weight resample = True if weight_sum < self.filter_params.weight_sum_tolerance: resample = False self.generate_new_particle_set() else: for i in xrange(self.filter_params.number_of_particles): self.particles[i].weight = self.particles[i].weight / weight_sum return resample def resample_particles(self): '''Performs an importance particle resampling step based on the current particle weights. ''' number_resampled_particles = self.filter_params.number_of_particles - self.filter_params.number_of_random_particles step = 1. / number_resampled_particles start_weight = uniform(0, step) current_particle = 0 current_weight = self.particles[0].weight weight_sum = 0. #we add random particles to the particle set new_particles = list() for i in xrange(self.filter_params.number_of_random_particles): particle = Particle.random_particle(self.filter_params.neg_x_limit, self.filter_params.x_limit, self.filter_params.neg_y_limit, self.filter_params.y_limit, 1/self.filter_params.number_of_particles) weight_sum = weight_sum + particle.weight new_particles.append(particle) #we resample particles using low variance resampling for i in xrange(number_resampled_particles): weight = start_weight + i * step while weight > current_weight: current_particle = current_particle + 1 current_weight = current_weight + self.particles[current_particle].weight particle = deepcopy(self.particles[current_particle]) weight_sum = weight_sum + particle.weight new_particles.append(particle) for i in xrange(self.filter_params.number_of_particles): new_particles[i].weight = new_particles[i].weight / weight_sum return new_particles def generate_new_particle_set(self): '''Discards the current particle set and creates a new set of random particles. ''' self.particles = list() for i in xrange(self.filter_params.number_of_particles): self.particles.append(Particle.random_particle(self.filter_params.neg_x_limit, self.filter_params.x_limit, self.filter_params.neg_y_limit, self.filter_params.y_limit, 1./self.filter_params.number_of_particles)) def get_most_likely_pose(self): '''Returns the pose of the particle with the largest weight. ''' max_weight_particle = 0 max_weight = self.particles[0].weight for i,particle in enumerate(self.particles): if particle.weight > max_weight: max_weight_particle = i max_weight = particle.weight return copy(self.particles[max_weight_particle].pose)
from resampling import Resampling from readtable import ReadTable from matplotlib import pyplot as plt from matplotlib import figure as fig import time src_path_map = '../data/map/wean.dat' src_path_log = '../data/log/robotdata1.log' map_obj = MapReader(src_path_map) occupancy_map = map_obj.get_map() bool_occ_map = (occupancy_map < 0.35) & (occupancy_map >= 0) print(bool_occ_map[500][400]) logfile = open(src_path_log, 'r') motion_model = MotionModel() sensor_model = SensorModel(occupancy_map) resampler = Resampling() def visualize_ray_cast(x_t1, bool_occ_map, tstep, output_path): walk_stride = 8 angle_stride = 10 x, y, theta = x_t1 laser_x_t1 = x + 25 * math.cos(theta) laser_y_t1 = y + 25 * math.sin(theta) xout = laser_x_t1 / 10 yout = laser_y_t1 / 10 for deg in range(-90, 90, angle_stride): laser_theta = (deg + theta * 180 / math.pi) * (math.pi / 180) calc_distance, x_end, y_end = ray_cast_visual(laser_x_t1, laser_y_t1,