Update for student code #5
Conversation
|
@josetoribio does this work on both the drone and the lx? I see several |
|
Also since we are using this in the lx we need to add a |
| from cv_bridge import CvBridge, CvBridgeError | ||
| from geometry_msgs.msg import PoseStamped | ||
| from pidrone_pkg.msg import State | ||
| from localization_helper import LocalizationParticleFilter, create_map, PROB_THRESHOLD |
There was a problem hiding this comment.
Why are we importing these from the localization_helper and not from student_localization_helper?
| x, y, z, w = tf.transformations.quaternion_from_euler(0, 0, self.pos[3]) | ||
| self.posemsg.pose.position.x = | ||
| self.posemsg.pose.position.y = | ||
| self.posemsg.pose.position.z = |
There was a problem hiding this comment.
Anytime we do not assign something that we want the student to fill we should set it = ....
|
|
||
|
|
||
| def is_almost_equal(x, y): | ||
| epsilon = 1e-8 | ||
| epsilon = 1*10**(-8) |
There was a problem hiding this comment.
These are equivalent but it's best to leave 1e-8
|
|
||
| # cannot find a match | ||
| if len(poses) == 0: | ||
| print "Random Initialization" |
There was a problem hiding this comment.
If we are using this file in the lx notebook we need to change all these print "blablabla" directives to print("blablabla").
| """"" | ||
| keeps angle within -pi to pi | ||
| """"" | ||
| while angle > math.pi: | ||
| angle -= 2 * math.pi | ||
| while angle <= -math.pi: | ||
| angle += 2 * math.pi | ||
|
|
||
| return angle |
There was a problem hiding this comment.
this can be replaced by return ( angle + np.pi) % (2 * np.pi ) - np.pi
| ############################################################################## | ||
| # TODO implement this method to update the map for FastSLAM! | ||
| # | ||
| # if this particle has no landmarks: | ||
| # add every currently observed feature as a landmark | ||
| # adjust particle's weight to reflect the change in certainty from adding landmarks | ||
| # else: | ||
| # close landmarks = list of particle's landmarks within self.perceptual_range of the particle's pose | ||
| # if close landmarks is not empty: | ||
| # part_descriptors = list of landmark.des for each landmark in close_landmarks | ||
| # matched_landmarks = boolean list with same length as close_landmarks, set to false | ||
| # | ||
| # for each observed feature: | ||
| # if part_descriptors is not empty: | ||
| # match = self.matcher.knnMatch(np.array([des]), np.array(part_descriptors), k=2) | ||
| # | ||
| # if there was not match or the match is poor quality: | ||
| # add the feature as a new landmark | ||
| # adjust particle weight to reflect the change in certainty | ||
| # else: | ||
| # use match.trainIdx to find the index of the matching landmark in the map | ||
| # set the boolean corresponding to this landmark in matched_landmarks to true | ||
| # update the landmark in particle.landmarks with information from the newly | ||
| # observed feature* | ||
| # update the weight of the particle to reflect the quality of the match | ||
| # hint: use scale_weight | ||
| # | ||
| # * this step is the most tricky because the index of the landmark in the close_landmarks list | ||
| # is not the same as its index in particle.landmarks! This is because close_landmarks only | ||
| # contains some of the landmarks in particle.landmarks | ||
| ############################################################################## |
There was a problem hiding this comment.
This kind of pseudocode could be helpful for students, I would keep it otherwise it's a lot of work to think about everything from scratch.
No description provided.