EKF/SLAM Object Tracking & Map Publishing #190
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
…tected point clouds from the image to the point cloud.
…nto pliam1105/improved_perception
…de each detected object's bounding box (when projected onto the image), with the respective labels and probabilities, colcon build succesful, didn't test yet
…throws some out of range errors but I catched them and made them warnings and it seems to work fine)
…ions in resize, may be the wrong order in terms of channels so may need to change the order both in resize() and in at() again), seems to correctly select the points in the objects detected
…l meaningful clusters based on the detections
…on, need to find a euclidean clustering algorithm for n-dimensional data or with customizable distance function, or just one that takes color into account (there are some in PCL but they mostly rely on color thresholds or difference limits rather than adding their squared difference to the distance) or implement it from scratch (and KD-tree from scratch?)
…nce based condition)
… from the bounding box ones
… to publish it now
…izing them in RViz (not tested or built yet)
… test & troubleshoot/tune the matching
…formulas for the choosing & matching metrics
…nd those projected to cluster points)
…n the actual object, but more testing/tuning would be good)
… by publishing unorganized point clouds (channels don't matter anyways in visualization)
…le_detector.cpp that will serve the same purpose here) for untracked & tracked obstacle map publishing
…p format (builds, not tested)
…cleMap message format (and visualized in RViz appropriately)
…y and pretty consistent with occlusions/missing objects for some time, especially when out of camera FoV, though not perfect, may need fine-tuning)
…sensor measurements and navigation message pose considered to be accurate so no motion updates, just the map), not built or tested yet, WIP
…o fix the SLAM implementation (most probably the initialization of the covariance for new objects) to resolve the overflow (?) errors
…e duplicates, maybe including the robot pose in the kalman filter may help with that)
…n and buoy positions in state, with the full covariance matrix
…object and reducing the matrices
…ently or doing SLAM with the GPS and the object detections to track them all together, including covariances between them
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
I implemented an Extended Kalman Filter object tracking algorithm with maximum-likelihood (ML) matching, performing Bayesian Inference to the predictions of each individual buoy with the (range, bearing) detections of the obstacles from 'bbox_project_pcloud' for the measurement updates, also using the global GPS position and IMU-calculated angle. It can also keep track of the covariance of the positions of the buoys as well as the robot pose and orientation, computing the full (2N+3)x(2N+3) covariance matrix and perform motion updates as well, to hopefully also solve the double detection problem which is especially apparent when the robot is turning. The latter capability can be toggled to be on and off using the appropriate parameter.