Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding support for SURF Features #739

Open
wants to merge 519 commits into
base: develop
Choose a base branch
from
Open

Conversation

luisa-mao
Copy link

Map matching pipeline supports SURF map with SURF DBoW2 vocabulary. Also made some bug fixes/warnings in the pipeline.

Note that the vocabulary-building pipeline is sensitive to the total number of features in the map--it won't construct a vocabulary if there are too many features. Choose a reasonable Hessian threshold for keypoint detection when constructing the map from images.

Added new tools in localization_analysis/scripts:
plot_detector_comparison compares matched poses using SURF, and BRISK, and optionally groundtruth
analyze_gaps plots a histogram of gaps in localization from a directory of bags.

Pedro-Roque and others added 30 commits December 15, 2021 14:15
* doc: added support for zsh

* fix: typos in configure.sh

* fix: added checks to .zshrc

Co-authored-by: Marina Moreira <[email protected]>
* use a previous docker image as a cache for build

* More changes and CI test (squash)

* docs

* revert ci to standard astrobee

* docs
* Fix: localization_node was missing a catkin dependency on ff_util.
* Check if a given value is a number before reading it as a real

The fix was trickier than that

* Update bumble and bsharp2 to new transforms

* Update the placeholder transforms for the rest of the bots

* iss.rivz: Sci cam is compressed

* astrobee readme: Point out to simulator page

* build_map: Print the robot name upfront, to avoid mistakes

* calibrate: Mention that pico_proxy is now started automatically

* simulation: wording fix

* Minor indentation fix in robot config files

* iss_viz.rviz: Sci cam is compressed

* build_map: Make the linter happy

* Move timestamp offsets from geometry to robot calibration namespace

* Minor typo fix

* Wipe file added by mistake
Added depth_odometry package that computes ICP and image feature based depth odometry as well as a point_cloud_common package with point to plane ICP and registration with known correspondences classes and a vision_common package with several feature tracking approaches. Also added a new depth odometry factor adder using either relative poses or corresponding points.
* interactive marker start

* fix build settings

* position command works

* add astrobee mesh

* orientation works

* cmake needs license

* docs start

* snap to astrobee and tf2

* (un)docking

* add stop command

* add some explanation

* clean unused lines

* reorganize headers

* rename orien->orientation

* remove cry for help

* clarify TODOs

* use namespace

* put everything into a class

* move namespace alias to cpp

* rename member variables

* don't use a default starting pose
instead, move the marker after init
* bsharp2 calibration refinement
* cameras.config: Remove comment which is no longer true, haz_cam is used
* Update bumble and bsharp config files
* Minor doc formatting tweaks
* make image names uniform; fix debian build script

* fixing condition
* Expand the Theia doc

* More Theia tweaks
Added depth odometry sweep tool, optional refinement of the image feature depth odometry estimate, and organized methods taking advantage of the grid like structure of point clouds to speed up computations.
* turning pico_proxy into a nodelet under mlp_depth_cam

* updating pico_proxy submodule

* adding pico_proxy to lib install list

* fixing sub and heartbear warn

* updating to correct platform submodule commit

* switching error log output and getting rid of rosparam

* removing extra config file

* fixing launch files

* fixes from testing on the robot

* initializing layers_ properly
* adding individual cpu nore readings

* plan tester script

* making analysis script start

* fix sscanf

* performance eval script

* fixing python lint errors

* fixing tool with feedback

* fix bug with index

* adding all nodelets for tracking - will slow down startup

* reverting accidental change in merge_bags

* adding documentation

* making logs ready to merge
* undistort_image: Fixes for huge distortion and out-of-bounds user input

* Minor fixes for deleting images with nvm_visualize
Copy link
Contributor

@trey0 trey0 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is looking good! I just had a couple of comments but don't feel enough up to speed on the context to really approve/reject or whatever.

It could be helpful to do a brief write-up of the approach you took, especially if it mentions some of the key areas where you had to do something surprising, or it wasn't obvious what approach you should follow, or you know of gotchas for future developers that use your code, That could naturally lead to a useful discussion. Thanks for all the hard work!


if __name__ == "__main__":

try:

if len(sys.argv) < 5:
print(
"Usage: activity_db_generator.py <activity date> <map name> <activity name> <location to save>"
"Usage: activity_db_generator.py <activity date> <map name> <activity name> <location to save> <bag_name>"
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's probably worth learning Python argparse for this kind of thing. It's the standard for modern Python scripts. Here are some random examples (search for argparse):

@@ -0,0 +1,134 @@
# **COVERAGE ANALYSIS README**
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So glad you changed this to Markdown! I considered doing that, but didn't want to become responsible for maintaining it :)

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Did this come from Andres' pr?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think so. I didn't make any changes to coverage_analysis

.gitignore Outdated
@@ -98,3 +98,8 @@ tags
.ctags

.vscode

submodules/
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You probably didn't mean to add these

#include <sparse_map/templated_image_database.h>

namespace sparse_mapping {
// TODO(rsoussan): Make sure we use surf 64!!!
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

do we need this todo?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't remember haha, probably not

@@ -0,0 +1,13 @@
import rosbag
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You might not know about rosbag filter, with that do we need this?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

oops didn't mean for that to be part of the commit. It was just more convenient for me than rosbag filter

@@ -0,0 +1,322 @@
"""Rank-biased overlap, a ragged sorted list similarity measure.
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

what is this used for?

@@ -46,7 +46,7 @@ then

# Add these packages to the apt sources (we remove them later, so apt update succeeds)

NO_TUNNEL=${NO_TUNNEL:-0} # Override this with 1 before invoking if the tunnel is not desired
NO_TUNNEL=1 # Override this with 1 before invoking if the tunnel is not desired
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

did you meant o commit this?

@@ -0,0 +1,222 @@
import rosbag
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

add comment about what this is

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe rename the file as well to something more descriptive, I think I have a script somewhere that also analyzes gaps but in bagfiles between messages and is names something similar

@@ -0,0 +1,173 @@
#!/usr/bin/python3
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

add comment about what this does

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think in other python scriptswe have examples for how to add the comment here to the arge parse description, you should use that pattern here

@@ -0,0 +1,120 @@
rmse,0.0
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

accidental commit?

* License for the specific language governing permissions and limitations
* under the License.
*/

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

add comment about what this is

@@ -0,0 +1,36 @@
rmse,0.02223592997076043
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

remove?

@bcoltin bcoltin requested a review from rsoussan July 14, 2023 18:26
@@ -50,7 +52,7 @@ DEFINE_int32(max_surf_features, 5000,
"Maximum number of features to be computed using SURF.");
DEFINE_double(min_surf_threshold, 1.1,
"Minimum threshold for feature detection using SURF.");
DEFINE_double(default_surf_threshold, 10,
DEFINE_double(default_surf_threshold, 400,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should probably make sure this doesn't adversely affect mapping

@@ -98,3 +98,8 @@ tags
.ctags

.vscode

# submodules/
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should these be here?

@@ -0,0 +1,120 @@
/* Copyright (c) 2017, United States Government, as represented by the
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this was recently merged to dev right? Do you know if you made changes to that version?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I just added more flags for run_map_matcher

@@ -0,0 +1,111 @@
/* Copyright (c) 2017, United States Government, as represented by the
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

(same comment, we should make sure the original pr is merged first

@@ -7,6 +7,7 @@
import sys
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should probably add andres to check this and the other coverage analysis changes

@@ -61,6 +61,9 @@ def integrate_velocities(localization_states):
for velocity, delta_t in zip(localization_states.velocities.zs, delta_times)
]

if len(localization_states.times) == 0:
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Does this happen?

@@ -104,6 +104,9 @@ def add_graph_plots(
orientation_plotter.add_pose_orientation(graph_localization_states)
orientation_plotter.plot(pdf)

if len(graph_localization_states.times) == 0:
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Oh, is this for when you plot the map matcher output?

@@ -0,0 +1,60 @@
/* Copyright (c) 2017, United States Government, as represented by the
* Administrator of the National Aeronautics and Space Administration.
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

(same as below)


int num_inverted_index = 0;
typename DBoW2::TemplatedDatabase<TDescriptor, F>::InvertedFile::const_iterator iit;
for (iit = this->m_ifile.begin(); iit != this->m_ifile.end(); ++iit) num_inverted_index += (*iit).size();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can this be changed to a range loop? (for (const auto& ifile: m_ifile)

}
typename DBoW2::TemplatedDatabase<TDescriptor, F>::IFRow::const_iterator irit;
int word_id = 0;
for (iit = this->m_ifile.begin(); iit != this->m_ifile.end(); ++iit) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

(range loop for these?)

LOG(FATAL) << "Failed to write db index entry to file.";
}
}
word_id++;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not important but prefer ++x over x++, x++ adds a tmp variable and is a little slower (probably compiled out here though)

@@ -0,0 +1,322 @@
"""Rank-biased overlap, a ragged sorted list similarity measure.
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this from an open source? We need to check the permissions and give credit if so

@@ -24,6 +24,7 @@
#include <sparse_mapping/reprojection.h>
#include <sparse_mapping/sparse_mapping.h>
#include <sparse_mapping/tensor.h>
#include <errno.h> // change later
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Still need to change later?


using namespace std; // NOLINT (trying to modify this as little as possible)

// stolen from file (this doesn't link for some reason?)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is there a header/cc file you were trying to link from?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This was from Brian's branch when he was working on the SURF vocab db. Not sure what it means.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think it's from DBoW2

memcpy(a.desc, s.c_str(), s.size());
}

// this is all a big hack
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What is this a hack for?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also from Brian's branch.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I have no idea

@@ -356,7 +501,7 @@ void MatDescrToVec(cv::Mat const& mat, std::vector<float> * vec) {
(*vec).reserve(mat.cols);
(*vec).clear();
for (int c = 0; c < mat.cols; c++) {
float val = static_cast<float>(mat.at<uchar>(0, c));
float val = static_cast<float>(mat.at<float>(0, c));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this conditional on the vocab using surf vs brisk features?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes. It's an overloaded function. The one that takes BriefDescriptor has uchar. I removed the unnecessary static cast.

matplotlib.use("pdf")
from matplotlib.backends.backend_pdf import PdfPages

def quat_to_rpy(quaternion):
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do these functions exist in other files? If so, we should put them in a common place.

@rsoussan
Copy link
Member

Nice work! Some comments mostly about files from other places, shared functions, etc, but looks good!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.