From 27751516350093f5e91bde804440ff26c9be177d Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Mon, 22 Nov 2021 09:56:10 -0800 Subject: [PATCH 01/29] Add NASA copyright notice, for consistency --- scripts/git/cpplint.py | 2 +- scripts/git/cpplint_repo.py | 18 ++++++++++++++++++ 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/scripts/git/cpplint.py b/scripts/git/cpplint.py index 8abae00375..300489ed0c 100755 --- a/scripts/git/cpplint.py +++ b/scripts/git/cpplint.py @@ -1,5 +1,5 @@ #!/usr/bin/python -# + # Copyright (c) 2009 Google Inc. All rights reserved. # # Redistribution and use in source and binary forms, with or without diff --git a/scripts/git/cpplint_repo.py b/scripts/git/cpplint_repo.py index 77948d438d..632d069348 100755 --- a/scripts/git/cpplint_repo.py +++ b/scripts/git/cpplint_repo.py @@ -1,5 +1,23 @@ #!/usr/bin/env python3 +# Copyright (c) 2021, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking +# platform" software is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + import fnmatch import importlib import os From 0862678f17b15d7db98eeb7f986a7c672bfc2328 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Mon, 22 Nov 2021 10:02:29 -0800 Subject: [PATCH 02/29] Use the right copyright notice --- scripts/git/cpplint_repo.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/scripts/git/cpplint_repo.py b/scripts/git/cpplint_repo.py index 632d069348..c60ed9f999 100755 --- a/scripts/git/cpplint_repo.py +++ b/scripts/git/cpplint_repo.py @@ -1,12 +1,11 @@ #!/usr/bin/env python3 -# Copyright (c) 2021, United States Government, as represented by the +# Copyright (c) 2017, United States Government, as represented by the # Administrator of the National Aeronautics and Space Administration. # # All rights reserved. # -# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking -# platform" software is licensed under the Apache License, Version 2.0 +# The Astrobee platform is licensed under the Apache License, Version 2.0 # (the "License"); you may not use this file except in compliance with the # License. You may obtain a copy of the License at # From 742c0f4243264e3ada71737a7a9bb9564fcb6567 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Wed, 24 Nov 2021 09:37:40 -0800 Subject: [PATCH 03/29] Bumble: Best sci and nav/sci to haz params so far --- astrobee/config/robots/bumble.config | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/astrobee/config/robots/bumble.config b/astrobee/config/robots/bumble.config index cd103a7908..901e5dbdb6 100644 --- a/astrobee/config/robots/bumble.config +++ b/astrobee/config/robots/bumble.config @@ -25,14 +25,14 @@ robot_i2c_bus = "/dev/i2c-1" robot_imu_drdy_pin = 4 robot_geometry = { - hazcam_to_navcam_transform = transform(vec3(0.070656217, -0.0053120391, -0.0086391367), quat4(-0.0033938631, 0.011939978, 0.99990009, -0.0067619677)), - scicam_to_hazcam_transform = transform(vec3(0.043949838, -0.026925687, -0.14459291), quat4(-0.009399956, 0.015955961, 0.99982772, -0.0012581665)), + hazcam_to_navcam_transform = transform(vec3(0.071292329, 0.0055312606, -0.0091406835), quat4(-0.0034653066, 0.0067303937, 0.99994366, -0.0074409497)), + scicam_to_hazcam_transform = transform(vec3(0.026700369, 0.0097009814, -0.058183141), quat4(0.0033118421, 0.0071084854, 0.99996918, -0.00037493164)), navcam_to_hazcam_timestamp_offset = -0.02, scicam_to_hazcam_timestamp_offset = 0.5, hazcam_depth_to_image_transform = { - 0.91122714, -0.00020224138, -0.00066071236, 0.00091477566, - 0.00020680677, 0.91120557, 0.0063030044, -0.0072143461, - 0.00065929762, -0.0063031525, 0.91120536, -0.0035329091, + 0.91382143, -0.00028777899, 0.00064911616, -0.00047542337, + 0.00028781297, 0.91382166, -4.7745161e-05, -5.8653007e-05, + -0.00064910109, 4.7949589e-05, 0.91382147, -0.0036829808, 0, 0, 0, 1}, -- Engineering positions with idealized orientations @@ -88,10 +88,10 @@ robot_camera_calibrations = { exposure=150 }, sci_cam = { - distortion_coeff = {0.10733913, -0.10935864, 0.0010663099, 0.0010407278}, + distortion_coeff = {0.0011993248, -0.064382091, -0.0033067002, -0.0090716356}, intrinsic_matrix = { - 1138.4943, 0.0, 680.36447, - 0.0, 1138.4943, 534.00133, + 1077.4585, 0.0, 673.35829, + 0.0, 1077.4585, 520.63092, 0.0, 0.0, 1.0 }, gain=50, From 06eb7af21ef1ffbc652775a4394c5410ef2e72a9 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Wed, 24 Nov 2021 15:21:06 -0800 Subject: [PATCH 04/29] Expand the doc of the undistort_image program --- localization/camera/readme.md | 30 +++++++++++++++++--- localization/camera/tools/undistort_image.cc | 10 +++---- 2 files changed, 31 insertions(+), 9 deletions(-) diff --git a/localization/camera/readme.md b/localization/camera/readme.md index a928647d89..8615710bce 100644 --- a/localization/camera/readme.md +++ b/localization/camera/readme.md @@ -1,4 +1,4 @@ -\page camera Camera Library +\page camera Camera library This library handles camera calibration parameters and transformations, providing functions to handle between undistorted and @@ -21,6 +21,28 @@ Usage: undistort_image input_dir/*[0-9].jpg --output_directory output_dir \ --robot_camera nav_cam -Other options, useful with the dense mapper, are --save_bgr, ---undistorted_crop_win, -scale, and --image_list. See -undistort_image.cc for more information. +Options: + + --image_list: A file having the list of images to undistort, one per + line. If not specified it is assumed they are passed in directly on + the command line. + + --output_directory: Output directory. If not specified, undistorted + images will saved in the same directory as the inputs. + + --scale: Undistort images at different resolution, with their width + being a multiple of this scale compared to the camera model. The + default is 1. + + --undistorted_crop_win: After undistorting, apply a crop window of + these dimensions centered at the undistorted image center. The + adjusted dimensions and optical center will be printed on screen. + Specify as: 'crop_x crop_y'. + + --save_bgr: Save the undistorted images as BGR instead of + grayscale. (Some tools expect BGR.) + + --histogram_equalization: If true, do histogram equalization. + + --robot_camera: Which of bot's cameras to use. Tested with nav_cam + and sci_cam. The default is nav_cam. diff --git a/localization/camera/tools/undistort_image.cc b/localization/camera/tools/undistort_image.cc index 151c48ecda..fdeb55c610 100644 --- a/localization/camera/tools/undistort_image.cc +++ b/localization/camera/tools/undistort_image.cc @@ -45,10 +45,10 @@ separate directory is specified. */ -DEFINE_string(image_list, "", "The list of images to undistort, one per line. If not specified, " - "it is assumed they are passed in directly on the command line."); +DEFINE_string(image_list, "", "A file having the list of images to undistort, one per line. " + "If not specified, it is assumed they are passed in directly on the command line."); -DEFINE_string(output_directory, "", "If not specified, undistorted images will " +DEFINE_string(output_directory, "", "Output directory. If not specified, undistorted images will " "saved in the same directory as the inputs."); DEFINE_double(scale, 1.0, "Undistort images at different resolution, with their width " @@ -57,7 +57,7 @@ DEFINE_double(scale, 1.0, "Undistort images at different resolution, with their DEFINE_string(undistorted_crop_win, "", "After undistorting, apply a crop window of these dimensions " "centered at the undistorted image center. The adjusted " - "dimensions and optical center will be displayed below. " + "dimensions and optical center will be printed on screen. " "Specify as: 'crop_x crop_y'."); DEFINE_bool(save_bgr, false, @@ -67,7 +67,7 @@ DEFINE_bool(histogram_equalization, false, "If true, do histogram equalization."); DEFINE_string(robot_camera, "nav_cam", - "Which of bot's cameras to use. Anything except nav_cam is experimental."); + "Which of bot's cameras to use. Tested with nav_cam and sci_cam."); int main(int argc, char ** argv) { ff_common::InitFreeFlyerApplication(&argc, &argv); From 190ba14470e3ef215be1994ac21f7fda75882832 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Wed, 24 Nov 2021 15:24:43 -0800 Subject: [PATCH 05/29] Add logic for permuting the order of images in a map and use it in reading nvm maps --- .../include/sparse_mapping/sparse_map.h | 3 + localization/sparse_mapping/src/sparse_map.cc | 177 ++++++++++++++++-- .../sparse_mapping/src/sparse_mapping.cc | 6 +- .../sparse_mapping/tools/build_map.cc | 2 +- 4 files changed, 167 insertions(+), 21 deletions(-) diff --git a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h index 10f55a4b2d..f31491ac3b 100644 --- a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h +++ b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h @@ -305,6 +305,9 @@ struct SparseMap { SparseMap(); SparseMap(SparseMap &); SparseMap& operator=(const SparseMap&); + + // Reorder the images in the map and the rest of the data accordingly + void reorderMap(std::map const& old_cid_to_new_cid); }; } // namespace sparse_mapping diff --git a/localization/sparse_mapping/src/sparse_map.cc b/localization/sparse_mapping/src/sparse_map.cc index 7bb386b10f..239a750aac 100644 --- a/localization/sparse_mapping/src/sparse_map.cc +++ b/localization/sparse_mapping/src/sparse_map.cc @@ -37,6 +37,9 @@ #include #include +#include +#include + #include #include #include @@ -119,24 +122,79 @@ SparseMap::SparseMap(const std::vector& cid_to_cam_t, } // Form a sparse map by reading a text file from disk. This is for comparing -// the bundler and theia maps with ours. -SparseMap::SparseMap(bool bundler_format, std::string const& filename, - std::vector const& files) : - camera_params_(Eigen::Vector2i(640, 480), Eigen::Vector2d::Constant(300), - Eigen::Vector2d(320, 240)), - num_similar_(FLAGS_num_similar), - num_ransac_iterations_(FLAGS_num_ransac_iterations), - ransac_inlier_tolerance_(FLAGS_ransac_inlier_tolerance), - early_break_landmarks_(FLAGS_early_break_landmarks), - histogram_equalization_(FLAGS_histogram_equalization) { - int num_cams; +// bundler, nvm or theia maps. +SparseMap::SparseMap(bool bundler_format, std::string const& filename, std::vector const& all_image_files) + : camera_params_(Eigen::Vector2i(640, 480), Eigen::Vector2d::Constant(300), + Eigen::Vector2d(320, 240)), // these are placeholders and must be changed + num_similar_(FLAGS_num_similar), + num_ransac_iterations_(FLAGS_num_ransac_iterations), + ransac_inlier_tolerance_(FLAGS_ransac_inlier_tolerance), + early_break_landmarks_(FLAGS_early_break_landmarks), + histogram_equalization_(FLAGS_histogram_equalization) { + std::string ext = ff_common::file_extension(filename); + boost::to_lower(ext); + + if (ext == "nvm") { + std::cout << "Reading a map in NVM format." << std::endl; + + sparse_mapping::ReadNVM(filename, &cid_to_keypoint_map_, &cid_to_filename_, &pid_to_cid_fid_, &pid_to_xyz_, + &cid_to_cam_t_global_); + + // Descriptors are not saved, so let them be empty + cid_to_descriptor_map_.resize(cid_to_keypoint_map_.size()); + + // When the NVM file is created by Theia, it saves the images + // without a path, in random order, and it may not have used up + // all the images, so need to adjust for that. + + std::map base_to_full_path; + std::map orig_order; + for (size_t it = 0; it < all_image_files.size(); it++) { + std::string image = all_image_files[it]; + std::string base = boost::filesystem::path(image).filename().string(); + if (base_to_full_path.find(base) != base_to_full_path.end()) + LOG(FATAL) << "Duplicate image: " << base << std::endl; + base_to_full_path[base] = image; + orig_order[it] = base; + } + + // Find the permutation which will tell how to reorder the images + // in the nvm file to be in the original order. This must happen + // before we change cid_to_filename_ below. + std::map old_cid_to_new_cid; + std::map base2cid; + for (size_t it = 0; it < cid_to_filename_.size(); it++) base2cid[cid_to_filename_[it]] = it; + int new_cid = 0; + for (auto order_it = orig_order.begin(); order_it != orig_order.end() ; order_it++) { + auto base_it = base2cid.find(order_it->second); + if (base_it == base2cid.end()) LOG(FATAL) << "Book-keeping error in ReadNVM"; // should not happen + int old_cid = base_it->second; + old_cid_to_new_cid[old_cid] = new_cid; + new_cid++; + } + + // Map the theia images to the actual image paths + for (size_t it = 0; it < cid_to_filename_.size(); it++) { + std::string base = cid_to_filename_[it]; + auto map_it = base_to_full_path.find(base); + if (map_it == base_to_full_path.end()) + LOG(FATAL) << "The input file list is missing the nvm map image: " << base << std::endl; + cid_to_filename_[it] = map_it->second; + } + + // Apply the permutation + reorderMap(old_cid_to_new_cid); + + } else if (bundler_format) { + std::cout << "Reading a map in Bundler format." << std::endl; + + int num_cams = 0; - if (bundler_format) { std::ifstream is(filename.c_str()); std::string line; std::getline(is, line); // empty line is >> num_cams; - cid_to_filename_ = files; + cid_to_filename_ = all_image_files; cid_to_cam_t_global_.resize(num_cams); cid_to_filename_.resize(num_cams); @@ -165,8 +223,17 @@ SparseMap::SparseMap(bool bundler_format, std::string const& filename, cid_to_cam_t_global_[i].translation() = P; } + // Initialize other data expected in the map + cid_to_keypoint_map_.resize(num_cams); + cid_to_descriptor_map_.resize(num_cams); + } else { - // Read the text dump of a theia map + // This is very old code and may need to be wiped. Now + // Theia writes in binary format and it provides a tool to export + // to NVM which we read as above. + std::cout << "Reading a map in Theia's text format." << std::endl; + + int num_cams = 0; std::ifstream is(filename.c_str()); is >> num_cams; @@ -207,11 +274,14 @@ SparseMap::SparseMap(bool bundler_format, std::string const& filename, pid_to_xyz_[i] = P; } is.close(); + + // Initialize other data expected in the map + cid_to_keypoint_map_.resize(num_cams); + cid_to_descriptor_map_.resize(num_cams); } - // Initialize other data expected in the map - cid_to_keypoint_map_.resize(num_cams); - cid_to_descriptor_map_.resize(num_cams); + // Initialize this convenient mapping + InitializeCidFidToPid(); } // Detect features in given images @@ -853,6 +923,79 @@ void SparseMap::PruneMap(void) { #endif } +// Reorder the images in the map and the rest of the data accordingly +void SparseMap::reorderMap(std::map const& old_cid_to_new_cid) { + int num_cid = cid_to_filename_.size(); + + // Sanity checks + if (old_cid_to_new_cid.size() != cid_to_filename_.size()) + LOG(FATAL) << "Wrong size of the permutation in SparseMap::reorderMap()."; + for (auto it = old_cid_to_new_cid.begin(); it != old_cid_to_new_cid.end(); it++) { + int new_cid = it->second; + if (new_cid >= num_cid) LOG(FATAL) << "Out of bounds in the permutation in SparseMap::reorderMap()."; + } + + // Wipe things that we won't reorder + vocab_db_ = sparse_mapping::VocabDB(); + db_to_cid_map_.clear(); + cid_to_cid_.clear(); + user_cid_to_keypoint_map_.clear(); + user_pid_to_cid_fid_.clear(); + user_pid_to_xyz_.clear(); + cid_fid_to_pid_.clear(); // Will recreate this later + + // Must create temporary structures + std::vector new_cid_to_filename(num_cid); + std::vector new_cid_to_keypoint_map(num_cid); + std::vector new_cid_to_cam_t_global(num_cid); + std::vector new_cid_to_descriptor_map(num_cid); + std::vector> new_pid_to_cid_fid(pid_to_cid_fid_.size()); + + // Note that pid_to_xyz_ is not changed by this reordering + + // Copy the data in new order + for (int old_cid = 0; old_cid < num_cid; old_cid++) { + auto it = old_cid_to_new_cid.find(old_cid); + if (it == old_cid_to_new_cid.end()) + LOG(FATAL) << "Cannot find desired index in permutation in SparseMap::reorderMap()."; + + int new_cid = it->second; + + new_cid_to_filename[new_cid] = cid_to_filename_[old_cid]; + new_cid_to_keypoint_map[new_cid] = cid_to_keypoint_map_[old_cid]; + new_cid_to_cam_t_global[new_cid] = cid_to_cam_t_global_[old_cid]; + new_cid_to_descriptor_map[new_cid] = cid_to_descriptor_map_[old_cid]; + } + + // pid_to_cid_fid needs special treatment + for (size_t pid = 0; pid < pid_to_cid_fid_.size(); pid++) { + auto const& cid_fid = pid_to_cid_fid_[pid]; // alias + + std::map new_cid_fid; + for (auto cid_fid_it = cid_fid.begin(); cid_fid_it != cid_fid.end(); cid_fid_it++) { + int old_cid = cid_fid_it->first; + auto cid_it = old_cid_to_new_cid.find(old_cid); + if (cid_it == old_cid_to_new_cid.end()) + LOG(FATAL) << "Bookkeeping error in SparseMap::reorderMap()"; + + int new_cid = cid_it->second; + new_cid_fid[new_cid] = cid_fid_it->second; + } + + new_pid_to_cid_fid[pid] = new_cid_fid; + } + + // Swap in the new values + cid_to_filename_.swap(new_cid_to_filename); + cid_to_keypoint_map_.swap(new_cid_to_keypoint_map); + cid_to_cam_t_global_.swap(new_cid_to_cam_t_global); + cid_to_descriptor_map_.swap(new_cid_to_descriptor_map); + pid_to_cid_fid_.swap(new_pid_to_cid_fid); + + // Recreate cid_fid_to_pid_ from pid_to_cid_fid_. + InitializeCidFidToPid(); +} + bool SparseMap::Localize(const cv::Mat & image, camera::CameraModel* pose, std::vector* inlier_landmarks, std::vector* inlier_observations, diff --git a/localization/sparse_mapping/src/sparse_mapping.cc b/localization/sparse_mapping/src/sparse_mapping.cc index 666c16bc7a..d37297ed5c 100644 --- a/localization/sparse_mapping/src/sparse_mapping.cc +++ b/localization/sparse_mapping/src/sparse_mapping.cc @@ -231,7 +231,7 @@ void sparse_mapping::ReadNVM(std::string const& input_filename, f >> c[0] >> c[1] >> c[2] >> dist1 >> dist2; cid_to_filename->at(cid) = token; - // Solve for t .. which is part of the affine transform + // Solve for t, which is part of the affine transform Eigen::Matrix3d r = q.matrix(); cid_to_cam_t_global->at(cid).linear() = r; cid_to_cam_t_global->at(cid).translation() = -r * c; @@ -241,7 +241,7 @@ void sparse_mapping::ReadNVM(std::string const& input_filename, ptrdiff_t number_of_pid; f >> number_of_pid; if (number_of_pid < 1) { - LOG(FATAL) << "NVM file is missing points"; + LOG(FATAL) << "The NVM file has no triangulated points."; } // Read the point @@ -270,7 +270,7 @@ void sparse_mapping::ReadNVM(std::string const& input_filename, } if (!f.good()) - LOG(FATAL) << "Unable to correctly read PID " << pid; + LOG(FATAL) << "Unable to correctly read PID: " << pid; } } diff --git a/localization/sparse_mapping/tools/build_map.cc b/localization/sparse_mapping/tools/build_map.cc index f13f05acd1..8dadb16eac 100644 --- a/localization/sparse_mapping/tools/build_map.cc +++ b/localization/sparse_mapping/tools/build_map.cc @@ -67,7 +67,7 @@ DEFINE_bool(incremental_ba, false, DEFINE_bool(loop_closure, false, "Take a map where images start repeating, and close the loop."); DEFINE_bool(bundle_adjustment, false, - "Perform update output_nvm with bundle adjustment."); + "Perform bundle adjustment."); DEFINE_bool(rebuild, false, "Rebuild the map with BRISK features."); DEFINE_bool(rebuild_replace_camera, false, From 19e2721d182c2a5f579aadb8ddcc37751282cd66 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Fri, 26 Nov 2021 14:21:27 -0800 Subject: [PATCH 06/29] Make import_map replace unidistorted with distorted images --- localization/sparse_mapping/readme.md | 71 ++++++++++++---- localization/sparse_mapping/src/sparse_map.cc | 59 +------------ .../sparse_mapping/tools/import_map.cc | 83 ++++++++++++++++--- 3 files changed, 131 insertions(+), 82 deletions(-) diff --git a/localization/sparse_mapping/readme.md b/localization/sparse_mapping/readme.md index dd96403657..a44b4a95ae 100644 --- a/localization/sparse_mapping/readme.md +++ b/localization/sparse_mapping/readme.md @@ -50,7 +50,8 @@ Usually the bags are acquired at a very high frame rate, and they are huge. A preliminary filtering of the bag images while still on the robot can be done with the command: - rosbag filter input.bag output.bag "(topic == '/hw/cam_nav') and (float(t.nsecs)/1e+9 <= 0.1)" + rosbag filter input.bag output.bag \ + "(topic == '/hw/cam_nav') and (float(t.nsecs)/1e+9 <= 0.1)" Here, for every second of recorded data, we keep only the first tenth of a second. This number may need to be adjusted. Later, a further @@ -62,7 +63,8 @@ From the local machine, fetch the bag: rsync -avzP astrobee@10.42.0.32:/data/bagfile.bag . -Here, the IP address of P4D was used, which may differ from your robot's IP address. +Here, the IP address of P4D was used, which may differ from your +robot's IP address. ### Merging bags @@ -70,18 +72,18 @@ The bags created on the ISS are likely split into many smaller bags, for easy and reliability of transfer. Those can be merged into one bag as follows: - astrobee_build/devel/lib/localization_node/merge_bags \ + astrobee/build/devel/lib/localization_node/merge_bags \ -output_bag ### Extracting images To extract images from a bag file: - extract_image_bag -use_timestamp_as_image_name \ - -image_topic /hw/cam_nav -output_directory + astrobee/build/devel/lib/localization_node/extract_image_bag \ + -use_timestamp_as_image_name \ + -image_topic /hw/cam_nav -output_directory -The above assumes that the software was built with ROS on. This tool should -exist in astrobee_build/native. +The above assumes that the software was built with ROS on. Please check using 'rosbag info' the nav cam topic in the bag, as its name can change. @@ -238,7 +240,7 @@ need to be rebuilt for the extracted submap using build_map -vocab_db -#### Merge maps +### Merge maps Given a set of SURF maps, they can be merged using the command: @@ -286,7 +288,7 @@ If the first of the two maps to merge is already registered, it may be desirable to keep that portion fixed during merging when bundle adjustment happens. That is accomplished with the flag -fix_first_map. -#### How to build a map efficiently +### How to build a map efficiently Often times map-building can take a long time, or it can fail. A cautious way of building a map is to build it in portions (perhaps on @@ -328,7 +330,7 @@ Only when a good enough map is obtained, a renamed copy of it should be rebuilt with BRISK features and a vocabulary database to be used on the robot. -#### Map strategy for the space station +### Map strategy for the space station For the space station, there exists one large SURF map with many images, and a small BRISK map with fewer images. If new images are @@ -348,7 +350,7 @@ for 80 of them localization failed. So, things will change as follows: The precise details are described in the next section. -#### Growing a map when more images are acquired +### Growing a map when more images are acquired Sometimes things in the desired environment change enough, or the lighting changes, and an existing map may no longer do as well in some @@ -409,7 +411,7 @@ batch if they are not strictly necessary. The following Python code implements this: - python ~/astrobee/localization/sparse_mapping/tools/grow_map.py \ + python astrobee/src/localization/sparse_mapping/tools/grow_map.py \ -histogram_equalization -small_map prev_brisk_vocab_hist.map \ -big_map curr_brisk_no_prune_hist.map -work_dir work \ -output_map curr_brisk_vocab_hist.map \ @@ -428,7 +430,7 @@ cohesiveness. Also note that the grow_map.py script takes a lot of other parameters on input that must be the same as in localization.config. -#### Reducing the number of images in a map +### Reducing the number of images in a map Sometimes a map has too many similar images. The tool reduce_map.py attempts to reduce their number without sacrificing the map quality. @@ -439,10 +441,11 @@ need not have a vocab db. Usage: - python reduce_map.py -input_map -min_brisk_threshold \ - -default_brisk_threshold -max_brisk_threshold \ - -localization_error -work_dir \ - -sample_rate -histogram_equalization + python astrobee/src/localization/sparse_mapping/tools/reduce_map.py \ + -input_map -min_brisk_threshold \ + -default_brisk_threshold -max_brisk_threshold \ + -localization_error -work_dir \ + -sample_rate -histogram_equalization The BRISK thresholds here must be as when the map was built (ideally like in localization.config). The -histogram_equalization flag is @@ -478,6 +481,40 @@ reduced map with a small list of desired images which can be set with -image_list, and then all images for which localization fails will be added back to it. +# Importing a map + +A map built with Theia using undistorted nav cam images can be +exported to the NVM format as:: + + /path/to/TheiaSfM/build/bin/export_to_nvm_file \ + -input_reconstruction_file map-0 \ + -output_nvm_file map.nvm + +The NVM map can be saved as an Astrobee sparse map with the command:: + + astrobee/build/devel/lib/sparse_mapping/import_map \ + -undistorted_camera_params "wid_x wid_y focal_len opt_ctr_x opt_ctr_y" \ + \ + -input_map map.nvm -output_map map.map + +This assumes that the images were acquired with the nav camera of the +robot given by $ASTROBEE_ROBOT and undistorted with the Astrobee +program ``undistort_image``. The undistorted camera parameters to use +should be as printed on the screen by ``undistort_image``. + +If desired to replace on importing the undistorted images with the +original distorted ones, as it is usually expected of a sparse map, +the above command should be called instead as:: + + astrobee/build/devel/lib/sparse_mapping/import_map \ + \ + -distorted_images_list list.txt \ + -input_map map.nvm -output_map map.map + +Here, the file list.txt must have one image per line. It is important +that both undistorted and distorted images be specified, as the former +are needed to look up camera poses and other data in the .nvm file +before being replaced with the distorted ones. \subpage map_building \subpage total_station diff --git a/localization/sparse_mapping/src/sparse_map.cc b/localization/sparse_mapping/src/sparse_map.cc index 239a750aac..d536fb436b 100644 --- a/localization/sparse_mapping/src/sparse_map.cc +++ b/localization/sparse_mapping/src/sparse_map.cc @@ -135,7 +135,7 @@ SparseMap::SparseMap(bool bundler_format, std::string const& filename, std::vect boost::to_lower(ext); if (ext == "nvm") { - std::cout << "Reading a map in NVM format." << std::endl; + std::cout << "NVM format detected." << std::endl; sparse_mapping::ReadNVM(filename, &cid_to_keypoint_map_, &cid_to_filename_, &pid_to_cid_fid_, &pid_to_xyz_, &cid_to_cam_t_global_); @@ -167,7 +167,8 @@ SparseMap::SparseMap(bool bundler_format, std::string const& filename, std::vect int new_cid = 0; for (auto order_it = orig_order.begin(); order_it != orig_order.end() ; order_it++) { auto base_it = base2cid.find(order_it->second); - if (base_it == base2cid.end()) LOG(FATAL) << "Book-keeping error in ReadNVM"; // should not happen + if (base_it == base2cid.end()) continue; // Not all input images may be present in the map + int old_cid = base_it->second; old_cid_to_new_cid[old_cid] = new_cid; new_cid++; @@ -186,7 +187,7 @@ SparseMap::SparseMap(bool bundler_format, std::string const& filename, std::vect reorderMap(old_cid_to_new_cid); } else if (bundler_format) { - std::cout << "Reading a map in Bundler format." << std::endl; + std::cout << "Bundler format detected." << std::endl; int num_cams = 0; @@ -223,58 +224,6 @@ SparseMap::SparseMap(bool bundler_format, std::string const& filename, std::vect cid_to_cam_t_global_[i].translation() = P; } - // Initialize other data expected in the map - cid_to_keypoint_map_.resize(num_cams); - cid_to_descriptor_map_.resize(num_cams); - - } else { - // This is very old code and may need to be wiped. Now - // Theia writes in binary format and it provides a tool to export - // to NVM which we read as above. - std::cout << "Reading a map in Theia's text format." << std::endl; - - int num_cams = 0; - - std::ifstream is(filename.c_str()); - is >> num_cams; - cid_to_cam_t_global_.resize(num_cams); - cid_to_filename_.resize(num_cams); - - for (int i = 0; i < num_cams; i++) { - std::string line; - std::getline(is, line); // empty line - std::getline(is, line); // actual data - cid_to_filename_[i] = line; - - Eigen::Matrix3d T; - for (int row = 0; row < T.rows(); row++) { - for (int col = 0; col < T.cols(); col++) { - is >> T(row, col); - } - } - - Eigen::Vector3d P; - for (int row = 0; row < P.size(); row++) - is >> P[row]; - - cid_to_cam_t_global_[i].linear() = T.transpose(); // not sure - cid_to_cam_t_global_[i].translation() = P; - cid_to_cam_t_global_[i] = cid_to_cam_t_global_[i].inverse(); - } - - int num_pts; - is >> num_pts; - pid_to_xyz_.resize(num_pts); - pid_to_cid_fid_.resize(num_pts); - for (int i = 0; i < num_pts; i++) { - Eigen::Vector3d P; - for (int row = 0; row < P.size(); row++) { - is >> P[row]; - } - pid_to_xyz_[i] = P; - } - is.close(); - // Initialize other data expected in the map cid_to_keypoint_map_.resize(num_cams); cid_to_descriptor_map_.resize(num_cams); diff --git a/localization/sparse_mapping/tools/import_map.cc b/localization/sparse_mapping/tools/import_map.cc index 865a91c775..f3f699cd88 100644 --- a/localization/sparse_mapping/tools/import_map.cc +++ b/localization/sparse_mapping/tools/import_map.cc @@ -17,6 +17,7 @@ */ #include #include +#include #include #include #include @@ -31,26 +32,88 @@ #include #include -// outputs DEFINE_string(input_map, "", - "text file."); + "Input map created with undistorted images, in a text file."); DEFINE_string(output_map, "output.map", - "Output file containing the matches and control network."); + "Output sparse map as expected by Astrobee software."); DEFINE_bool(bundler_map, false, - "If true, read the bundler or theia format."); + "If true, read the Bundler format. This will be ignored for input .nvm files."); +DEFINE_string(undistorted_camera_params, "", + "Intrinsics of the undistorted camera. Specify as: " + "'wid_x wid_y focal_len opt_ctr_x opt_ctr_y'."); +DEFINE_string(distorted_images_list, "", + "Replace the undistorted images specified on input with distorted images " + "from this list. The correct value of ASTROBEE_ROBOT must be set."); + +// Replace the undistorted images which Theia used with distorted images +void replaceWithDistortedImages(std::vector const& undist_images, + std::string const& distorted_images_list, + sparse_mapping::SparseMap & map) { + // Must overwrite the camera params for the distorted images + std::cout << "Using distorted camera parameters for nav_cam for robot: " + << getenv("ASTROBEE_ROBOT") << ".\n"; + config_reader::ConfigReader config; + config.AddFile("cameras.config"); + if (!config.ReadFiles()) LOG(FATAL) << "Failed to read config files.\n"; + + camera::CameraParameters cam_params(&config, "nav_cam"); + map.SetCameraParameters(cam_params); + + // Replace the undistorted images with distorted ones + std::vector distorted_images; + std::string image; + std::ifstream ifs(distorted_images_list.c_str()); + while (ifs >> image) + distorted_images.push_back(image); + + if (undist_images.size() != distorted_images.size()) + LOG(FATAL) << "The number of distorted images in the list and undistorted ones " + << "passed on the command line must be the same.\n"; + + std::map undist_to_dist; + for (size_t it = 0; it < undist_images.size(); it++) undist_to_dist[undist_images[it]] = distorted_images[it]; + + // Replace the images in the map. Keep in mind that the map + // may have just a subset of the input images. + for (size_t it = 0; it < map.cid_to_filename_.size(); it++) { + auto map_it = undist_to_dist.find(map.cid_to_filename_[it]); + if (map_it == undist_to_dist.end()) + LOG(FATAL) << "This map image was not specified on input: " + << map.cid_to_filename_[it] << ".\n"; + map.cid_to_filename_[it] = map_it->second; + } +} int main(int argc, char** argv) { ff_common::InitFreeFlyerApplication(&argc, &argv); GOOGLE_PROTOBUF_VERIFY_VERSION; - std::vector files; - for (int i = 1; i < argc; i++) { - std::cout << "adding " << argv[i] << std::endl; - files.push_back(argv[i]); + // Read the images from the command line + std::vector undist_images; + for (int i = 1; i < argc; i++) + undist_images.push_back(argv[i]); + + std::cout << "Reading map: " << FLAGS_input_map << std::endl; + sparse_mapping::SparseMap map(FLAGS_bundler_map, FLAGS_input_map, undist_images); + + if (FLAGS_distorted_images_list == "") { + // Must overwrite the camera parameters with what is passed on input + std::vector vals; + ff_common::parseStr(FLAGS_undistorted_camera_params, vals); + if (vals.size() < 5) + LOG(FATAL) << "Could not parse --undistorted_camera_params.\n"; + double widx = vals[0], widy = vals[1], f = vals[2], cx = vals[3], cy = vals[4]; + LOG(INFO) << "Using undistorted camera parameters: " + << widx << ' ' << widy << ' ' << f << ' ' << cx << ' ' << cy; + camera::CameraParameters cam_params = camera::CameraParameters(Eigen::Vector2i(widx, widy), + Eigen::Vector2d::Constant(f), + Eigen::Vector2d(cx, cy)); + map.SetCameraParameters(cam_params); + } else { + replaceWithDistortedImages(undist_images, FLAGS_distorted_images_list, map); } - std::cout << "input and output " << FLAGS_input_map << ' ' << FLAGS_output_map << std::endl; - sparse_mapping::SparseMap map(FLAGS_bundler_map, FLAGS_input_map, files); + std::cout << "Writing map: " << FLAGS_output_map << std::endl; map.Save(FLAGS_output_map); google::protobuf::ShutdownProtobufLibrary(); From a646c814f576ecb8dd81ae3cda45b5309a2725df Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Sun, 28 Nov 2021 12:05:39 -0800 Subject: [PATCH 07/29] One more update to bumble.config, validated for isaac2 testases --- astrobee/config/robots/bumble.config | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/astrobee/config/robots/bumble.config b/astrobee/config/robots/bumble.config index 901e5dbdb6..9aaa23737a 100644 --- a/astrobee/config/robots/bumble.config +++ b/astrobee/config/robots/bumble.config @@ -25,14 +25,14 @@ robot_i2c_bus = "/dev/i2c-1" robot_imu_drdy_pin = 4 robot_geometry = { - hazcam_to_navcam_transform = transform(vec3(0.071292329, 0.0055312606, -0.0091406835), quat4(-0.0034653066, 0.0067303937, 0.99994366, -0.0074409497)), - scicam_to_hazcam_transform = transform(vec3(0.026700369, 0.0097009814, -0.058183141), quat4(0.0033118421, 0.0071084854, 0.99996918, -0.00037493164)), + hazcam_to_navcam_transform = transform(vec3(0.07142937, 0.00058221217, -0.001373669), quat4(-0.0030431141, 0.0092646368, 0.99993195, -0.0064039206)), + scicam_to_hazcam_transform = transform(vec3(-0.0052887445, 0.010298013, -0.043606689), quat4(0.0018545621, 0.012796392, 0.99991616, -0.00069204825)), navcam_to_hazcam_timestamp_offset = -0.02, - scicam_to_hazcam_timestamp_offset = 0.5, + scicam_to_hazcam_timestamp_offset = -0.2, hazcam_depth_to_image_transform = { - 0.91382143, -0.00028777899, 0.00064911616, -0.00047542337, - 0.00028781297, 0.91382166, -4.7745161e-05, -5.8653007e-05, - -0.00064910109, 4.7949589e-05, 0.91382147, -0.0036829808, + 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, + 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, + 0.0028479115, -0.0019383659, 0.91602652, -0.0042296964, 0, 0, 0, 1}, -- Engineering positions with idealized orientations @@ -88,10 +88,10 @@ robot_camera_calibrations = { exposure=150 }, sci_cam = { - distortion_coeff = {0.0011993248, -0.064382091, -0.0033067002, -0.0090716356}, + distortion_coeff = {0.029948958, -0.056854916, -0.00077885482, -0.0039909885}, intrinsic_matrix = { - 1077.4585, 0.0, 673.35829, - 0.0, 1077.4585, 520.63092, + 1048.499, 0.0, 705.22914, + 0.0, 1048.499, 511.03079, 0.0, 0.0, 1.0 }, gain=50, From d908691f7906ddc0f70abec29f267b59c5320e64 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Sun, 28 Nov 2021 15:13:57 -0800 Subject: [PATCH 08/29] Add tool to build map with theia and import it --- .../sparse_mapping/tools/build_theia_map.py | 199 ++++++++++++++++++ 1 file changed, 199 insertions(+) create mode 100644 localization/sparse_mapping/tools/build_theia_map.py diff --git a/localization/sparse_mapping/tools/build_theia_map.py b/localization/sparse_mapping/tools/build_theia_map.py new file mode 100644 index 0000000000..19e21d2942 --- /dev/null +++ b/localization/sparse_mapping/tools/build_theia_map.py @@ -0,0 +1,199 @@ +#!/usr/bin/env python +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +''' +Undistort the images, create a theia map, then convert it to a sparse +map as understood by astrobee software, and rebuild it with distorted +images and SURF features, while reoptimizing the cameras. +''' + +import sys, os, argparse, subprocess, re, shutil, glob + +def process_args(args): + ''' + Set up the parser and parse the args. + ''' + + # Extract some paths before the args are parsed + src_path = os.path.dirname(args[0]) + exec_path = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(src_path)))) + build_map_path = os.path.join(exec_path, 'build/devel/lib/sparse_mapping/build_map') + undistort_image_path = os.path.join(exec_path, 'build/devel/lib/camera/undistort_image') + + parser = argparse.ArgumentParser(description='Parameters for the geometry mapper.') + parser.add_argument('--image_list', dest = 'image_list', default = '', + help = 'The list of distorted nav cam images to use, one per line.') + parser.add_argument('--output_map', dest = 'output_map', default = '', + help = 'The resulting output map.') + parser.add_argument('--work_dir', dest = 'work_dir', default = '', + help = 'A temporary work directory to be deleted by the user.') + + args = parser.parse_args() + + return (src_path, build_map_path, undistort_image_path, args) + +def sanity_checks(build_map_path, undistort_image_path, args): + + # Check if the environemnt was set + for var in 'ASTROBEE_RESOURCE_DIR', 'ASTROBEE_CONFIG_DIR', 'ASTROBEE_WORLD', 'ASTROBEE_ROBOT': + if var not in os.environ: + raise Exception("Must set " + var) + + if not os.path.exists(build_map_path): + raise Exception("Executable does not exist: " + build_map_path) + + if not os.path.exists(undistort_image_path): + raise Exception("Executable does not exist: " + undistort_image_path) + + if args.image_list == "": + raise Exception("The path to the output map was not specified.") + + if args.output_map == "": + raise Exception("The path to the output map was not specified.") + + if args.work_dir == "": + raise Exception("The path to the work directory was not specified.") + +def mkdir_p(path): + if path == "": + return # this can happen when path is os.path.dirname("myfile.txt") + try: + os.makedirs(path) + except OSError: + if os.path.isdir(path): + pass + else: + raise Exception('Could not make directory ' + path + ' as a file with this name exists.') + +# Theia likes its images undistorted and in one directory +def gen_undist_image_list(work_dir, dist_image_list): + undist_dir = work_dir + "/undist" + mkdir_p(undist_dir) + + # Wipe any preexisting images to not confuse theia later + for image in glob.glob(undist_dir + '/*.jpg'): + os.remove(image) + + undist_image_list = work_dir + "/undistorted_list.txt" + print("Writing: " + undist_image_list) + + count = 10000 # to have the created images show up nicely + undist_images = [] + with open(dist_image_list, 'r') as dfh: + dist_image_files = dfh.readlines() + with open(undist_image_list, 'w') as ufh: + for image in dist_image_files: + base_image = str(count) + ".jpg" + undist_images.append(base_image) + undist_image = undist_dir + "/" + base_image + ufh.write(undist_image + "\n") + count += 1 + + return undist_image_list, undist_dir, undist_images + +def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): + + calib_file = work_dir + "/" + "theia_calibration.json"; + + # Parse the intrinsics + with open(undist_intrinsics_file, 'r') as fh: + for line in fh: + if len(line) == 0: + continue + if line[0] == '#': + continue + + intrinsics = line.split() + if len(intrinsics) < 5: + raise Exception('Expecting 5 intrinsics numbers in: ' + undist_intrinsics_file) + + width = intrinsics[0] + height = intrinsics[1] + focal_length = intrinsics[2] + opt_ctr_x = intrinsics[3] + opt_ctr_y = intrinsics[4] + + break + + print("Writing: " + calib_file) + with open(calib_file, 'w') as fh: + fh.write("{\n") + fh.write("\"priors\" : [\n") + + num_images = len(undist_images) + for it in range(num_images): + image = undist_images[it] + fh.write("{\"CameraIntrinsicsPrior\" : {\n"); + fh.write("\"image_name\" : \"" + image + "\",\n"); + fh.write("\"width\" : " + str(width) + ",\n"); + fh.write("\"height\" : " + str(height) + ",\n"); + fh.write("\"camera_intrinsics_type\" : \"PINHOLE\",\n"); + fh.write("\"focal_length\" : " + str(focal_length) + ",\n"); + fh.write("\"principal_point\" : [" + str(opt_ctr_x) + ", " + str(opt_ctr_y) + "]\n"); + + if it < num_images - 1: + fh.write("}},\n") + else: + fh.write("}}\n") + + fh.write("]\n") + fh.write("}\n") + + return calib_file + +def run_cmd(cmd): + ''' + Run a command. + ''' + print(" ".join(cmd) + "\n") + process = subprocess.Popen(cmd, stdout=subprocess.PIPE) + process.wait() + if (process.returncode != 0): + print("Failed execution of: " + " ".join(cmd)) + sys.exit(1) + +if __name__ == '__main__': + + (src_path, build_map_path, undistort_image_path, args) = process_args(sys.argv) + + sanity_checks(build_map_path, undistort_image_path, args) + + mkdir_p(args.work_dir) + undist_image_list, undist_dir, undist_images + = gen_undist_image_list(args.work_dir, args.image_list) + + # Undistort the images and crop to a central region + undist_intrinsics_file = args.work_dir + "/undistorted_intrinsics.txt" + cmd = [undistort_image_path, '-image_list', args.image_list, + "--undistorted_crop_win", "1100 776", + '-output_list', undist_image_list, + '-undistorted_intrinsics', undist_intrinsics_file] + run_cmd(cmd) + + calib_file = gen_theia_calib_file(args.work_dir, undist_images, undist_intrinsics_file) + + print("--must Have theia in path") + print("Must have a flag file in the repo") + print("all broken below!") + cmd = ['build_reconstruction', '--flagfile', 'build_reconstruction_flags.txt --images' + undist_dir + '/*jpg', '--calibration_file', calib_file, + '--output_reconstruction', + 'small_theia_work/run', + '--matching_working_directory', + 'small_theia_work/work', + '--intrinsics_to_optimize', 'NONE'] From 52f362c627168771f7d348cd7af7342d6f80f93e Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Mon, 29 Nov 2021 14:33:30 -0800 Subject: [PATCH 09/29] undistort_image: Can specify the input images in a list --- localization/camera/tools/undistort_image.cc | 31 +++++++++++++++++--- 1 file changed, 27 insertions(+), 4 deletions(-) diff --git a/localization/camera/tools/undistort_image.cc b/localization/camera/tools/undistort_image.cc index fdeb55c610..1101bb0fc7 100644 --- a/localization/camera/tools/undistort_image.cc +++ b/localization/camera/tools/undistort_image.cc @@ -51,6 +51,11 @@ DEFINE_string(image_list, "", "A file having the list of images to undistort, on DEFINE_string(output_directory, "", "Output directory. If not specified, undistorted images will " "saved in the same directory as the inputs."); +DEFINE_string(output_list, "", "Save the undistorted images with names given in this list, " + "instead of using the output directory."); + +DEFINE_string(undistorted_intrinsics, "", "Save to this file the undistorted camera intrinsics."); + DEFINE_double(scale, 1.0, "Undistort images at different resolution, with their width " "being a multiple of this scale compared to the camera model."); @@ -87,6 +92,18 @@ int main(int argc, char ** argv) { if (images.empty()) LOG(FATAL) << "Expecting at least one input image."; + std::vector undist_images; + if (FLAGS_output_list != "") { + std::ifstream ifs(FLAGS_output_list); + std::string image; + while (ifs >> image) + undist_images.push_back(image); + + if (undist_images.size() != images.size()) + LOG(FATAL) << "There must be as many output undistorted " + << "images as input distorted images.\n"; + } + // Read the camera config_reader::ConfigReader config; config.AddFile("cameras.config"); @@ -243,7 +260,10 @@ int main(int argc, char ** argv) { // The output file name std::string undist_file; - if (!FLAGS_output_directory.empty()) { + if (!undist_images.empty()) { + // Was specified via a list + undist_file = undist_images[i]; + } else if (!FLAGS_output_directory.empty()) { // A separate output directory was specified undist_file = FLAGS_output_directory + "/" + ff_common::basename(filename); } else { @@ -272,15 +292,18 @@ int main(int argc, char ** argv) { std::cout << "Focal length: " << focal_length << "\n"; std::cout << "Undistorted optical center: " << optical_center.transpose() << "\n"; - if (!FLAGS_output_directory.empty()) { - std::string intrinsics_file = FLAGS_output_directory + "/undistorted_intrinsics.txt"; + std::string intrinsics_file = FLAGS_undistorted_intrinsics; + if (intrinsics_file.empty() && !FLAGS_output_directory.empty()) + intrinsics_file = FLAGS_output_directory + "/undistorted_intrinsics.txt"; + + if (!intrinsics_file.empty()) { + std::cout << "Writing: " << intrinsics_file << std::endl; std::ofstream ofs(intrinsics_file.c_str()); ofs.precision(17); ofs << "# Unidistored width and height, focal length, undistorted optical center\n"; ofs << undist_size.transpose() << " " << focal_length << " " << optical_center.transpose() << "\n"; ofs.close(); - std::cout << "Wrote: " << intrinsics_file << std::endl; } return 0; From 7973aa7278e0185bc8f4ba86a9acf11def769fb6 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Mon, 29 Nov 2021 14:36:18 -0800 Subject: [PATCH 10/29] localization/sparse_mapping/theia_map.md --- localization/sparse_mapping/readme.md | 39 +--- .../sparse_mapping/tools/build_theia_map.py | 212 ++++++++++++------ .../sparse_mapping/tools/import_map.cc | 107 +++++---- 3 files changed, 213 insertions(+), 145 deletions(-) diff --git a/localization/sparse_mapping/readme.md b/localization/sparse_mapping/readme.md index a44b4a95ae..ffd1e8b106 100644 --- a/localization/sparse_mapping/readme.md +++ b/localization/sparse_mapping/readme.md @@ -57,7 +57,7 @@ Here, for every second of recorded data, we keep only the first tenth of a second. This number may need to be adjusted. Later, a further selection of the images can be done. -### Copy the bag from the robot: +### Copy the bag from the robot From the local machine, fetch the bag: @@ -137,7 +137,7 @@ collecting a subset of the images. (After clicking, a bug in OpenCV disables the arrow keys, then one can navigate with the "Ins" and "Del" keys on the numpad.) -This tool can be invoked to just look at images, without any map being +This tool can be invoked to look at just images, without any map being built. It can also delete images in this mode, with the 'Delete' and 'x' keys, if invoked as: @@ -481,41 +481,6 @@ reduced map with a small list of desired images which can be set with -image_list, and then all images for which localization fails will be added back to it. -# Importing a map - -A map built with Theia using undistorted nav cam images can be -exported to the NVM format as:: - - /path/to/TheiaSfM/build/bin/export_to_nvm_file \ - -input_reconstruction_file map-0 \ - -output_nvm_file map.nvm - -The NVM map can be saved as an Astrobee sparse map with the command:: - - astrobee/build/devel/lib/sparse_mapping/import_map \ - -undistorted_camera_params "wid_x wid_y focal_len opt_ctr_x opt_ctr_y" \ - \ - -input_map map.nvm -output_map map.map - -This assumes that the images were acquired with the nav camera of the -robot given by $ASTROBEE_ROBOT and undistorted with the Astrobee -program ``undistort_image``. The undistorted camera parameters to use -should be as printed on the screen by ``undistort_image``. - -If desired to replace on importing the undistorted images with the -original distorted ones, as it is usually expected of a sparse map, -the above command should be called instead as:: - - astrobee/build/devel/lib/sparse_mapping/import_map \ - \ - -distorted_images_list list.txt \ - -input_map map.nvm -output_map map.map - -Here, the file list.txt must have one image per line. It is important -that both undistorted and distorted images be specified, as the former -are needed to look up camera poses and other data in the .nvm file -before being replaced with the distorted ones. - \subpage map_building \subpage total_station \subpage granite_lab_registration diff --git a/localization/sparse_mapping/tools/build_theia_map.py b/localization/sparse_mapping/tools/build_theia_map.py index 19e21d2942..a4fe7daa63 100644 --- a/localization/sparse_mapping/tools/build_theia_map.py +++ b/localization/sparse_mapping/tools/build_theia_map.py @@ -19,47 +19,57 @@ ''' Undistort the images, create a theia map, then convert it to a sparse map as understood by astrobee software, and rebuild it with distorted -images and SURF features, while reoptimizing the cameras. +images and SURF features, while reoptimizing the cameras. The +Theia build_reconstruction tool must be in the PATH. ''' import sys, os, argparse, subprocess, re, shutil, glob -def process_args(args): - ''' - Set up the parser and parse the args. - ''' - - # Extract some paths before the args are parsed - src_path = os.path.dirname(args[0]) - exec_path = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(src_path)))) - build_map_path = os.path.join(exec_path, 'build/devel/lib/sparse_mapping/build_map') - undistort_image_path = os.path.join(exec_path, 'build/devel/lib/camera/undistort_image') +def which(program): + '''Find if a program is in the PATH''' + def is_exe(fpath): + return os.path.isfile(fpath) and os.access(fpath, os.X_OK) - parser = argparse.ArgumentParser(description='Parameters for the geometry mapper.') - parser.add_argument('--image_list', dest = 'image_list', default = '', - help = 'The list of distorted nav cam images to use, one per line.') - parser.add_argument('--output_map', dest = 'output_map', default = '', - help = 'The resulting output map.') - parser.add_argument('--work_dir', dest = 'work_dir', default = '', - help = 'A temporary work directory to be deleted by the user.') + fpath, fname = os.path.split(program) + if fpath: + if is_exe(program): + return program + else: + for path in os.environ["PATH"].split(os.pathsep): + path = path.strip('"') + exe_file = os.path.join(path, program) + if is_exe(exe_file): + return exe_file - args = parser.parse_args() + return None - return (src_path, build_map_path, undistort_image_path, args) +def run_cmd(cmd): + ''' + Run a command. + ''' + print(" ".join(cmd) + "\n") + process = subprocess.Popen(cmd, stdout=subprocess.PIPE) + process.wait() + if (process.returncode != 0): + print("Failed execution of: " + " ".join(cmd)) + sys.exit(1) -def sanity_checks(build_map_path, undistort_image_path, args): +def sanity_checks(undistort_image_path, import_map_path, build_map_path, args): - # Check if the environemnt was set - for var in 'ASTROBEE_RESOURCE_DIR', 'ASTROBEE_CONFIG_DIR', 'ASTROBEE_WORLD', 'ASTROBEE_ROBOT': + # Check if the environment was set + for var in ['ASTROBEE_RESOURCE_DIR', 'ASTROBEE_CONFIG_DIR', 'ASTROBEE_WORLD', 'ASTROBEE_ROBOT']: if var not in os.environ: raise Exception("Must set " + var) - if not os.path.exists(build_map_path): - raise Exception("Executable does not exist: " + build_map_path) - if not os.path.exists(undistort_image_path): raise Exception("Executable does not exist: " + undistort_image_path) + if not os.path.exists(import_map_path): + raise Exception("Executable does not exist: " + import_map_path) + + if not os.path.exists(build_map_path): + raise Exception("Executable does not exist: " + build_map_path) + if args.image_list == "": raise Exception("The path to the output map was not specified.") @@ -69,6 +79,50 @@ def sanity_checks(build_map_path, undistort_image_path, args): if args.work_dir == "": raise Exception("The path to the work directory was not specified.") + if not os.path.exists(args.theia_flags): + raise Exception("Cannot find the Theia flags file: " + args.theia_flags) + + if which('build_reconstruction') is None: + raise Exception("Cannot find the 'build_reconstruction' program in PATH.") + + if which('export_to_nvm_file') is None: + raise Exception("Cannot find the 'export_to_nvm_file' program in PATH.") + +def process_args(args): + ''' + Set up the parser and parse the args. + ''' + + # Extract some paths before the args are parsed + src_path = os.path.dirname(args[0]) + exec_path = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(src_path)))) + undistort_image_path = os.path.join(exec_path, "build/devel/lib/camera/undistort_image") + import_map_path = os.path.join(exec_path, "build/devel/lib/sparse_mapping/import_map") + build_map_path = os.path.join(exec_path, "build/devel/lib/sparse_mapping/build_map") + + parser = argparse.ArgumentParser(description="Parameters for the geometry mapper.") + parser.add_argument("--theia_flags", dest = "theia_flags", default = "", + help = "The flags to pass to Theia. If not specified, use " + + "localization/sparse_mapping/theia_flags.txt in the astrobee repo.") + parser.add_argument("--image_list", dest = "image_list", default = "", + help = "The list of distorted (original) nav cam images to use, " + + "one per line.") + parser.add_argument("--output_map", dest = "output_map", default = "", + help = "The resulting output map.") + parser.add_argument('--skip_rebuilding', dest = 'skip_rebuilding', action = 'store_true', + help = 'Do not rebuild the map after importing it from Theia.') + parser.add_argument("--work_dir", dest = "work_dir", default = "", + help = "A temporary work directory to be deleted by the user later.") + + args = parser.parse_args() + + if args.theia_flags == "": + args.theia_flags = os.path.dirname(src_path) + "/theia_flags.txt" + + sanity_checks(undistort_image_path, import_map_path, build_map_path, args) + + return (src_path, undistort_image_path, import_map_path, build_map_path, args) + def mkdir_p(path): if path == "": return # this can happen when path is os.path.dirname("myfile.txt") @@ -78,7 +132,8 @@ def mkdir_p(path): if os.path.isdir(path): pass else: - raise Exception('Could not make directory ' + path + ' as a file with this name exists.') + raise Exception("Could not make directory " + path + \ + " as a file with this name exists.") # Theia likes its images undistorted and in one directory def gen_undist_image_list(work_dir, dist_image_list): @@ -86,7 +141,11 @@ def gen_undist_image_list(work_dir, dist_image_list): mkdir_p(undist_dir) # Wipe any preexisting images to not confuse theia later - for image in glob.glob(undist_dir + '/*.jpg'): + count = 0 + for image in glob.glob(undist_dir + "/*.jpg"): + if count == 0: + print("Wiping old images in: " + undist_dir) + count += 1 os.remove(image) undist_image_list = work_dir + "/undistorted_list.txt" @@ -94,9 +153,9 @@ def gen_undist_image_list(work_dir, dist_image_list): count = 10000 # to have the created images show up nicely undist_images = [] - with open(dist_image_list, 'r') as dfh: + with open(dist_image_list, "r") as dfh: dist_image_files = dfh.readlines() - with open(undist_image_list, 'w') as ufh: + with open(undist_image_list, "w") as ufh: for image in dist_image_files: base_image = str(count) + ".jpg" undist_images.append(base_image) @@ -111,16 +170,16 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): calib_file = work_dir + "/" + "theia_calibration.json"; # Parse the intrinsics - with open(undist_intrinsics_file, 'r') as fh: + with open(undist_intrinsics_file, "r") as fh: for line in fh: if len(line) == 0: continue - if line[0] == '#': + if line[0] == "#": continue intrinsics = line.split() if len(intrinsics) < 5: - raise Exception('Expecting 5 intrinsics numbers in: ' + undist_intrinsics_file) + raise Exception("Expecting 5 intrinsics numbers in: " + undist_intrinsics_file) width = intrinsics[0] height = intrinsics[1] @@ -131,7 +190,7 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): break print("Writing: " + calib_file) - with open(calib_file, 'w') as fh: + with open(calib_file, "w") as fh: fh.write("{\n") fh.write("\"priors\" : [\n") @@ -156,44 +215,67 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): return calib_file -def run_cmd(cmd): - ''' - Run a command. - ''' - print(" ".join(cmd) + "\n") - process = subprocess.Popen(cmd, stdout=subprocess.PIPE) - process.wait() - if (process.returncode != 0): - print("Failed execution of: " + " ".join(cmd)) - sys.exit(1) -if __name__ == '__main__': +if __name__ == "__main__": - (src_path, build_map_path, undistort_image_path, args) = process_args(sys.argv) + (src_path, undistort_image_path, import_map_path, build_map_path, args) \ + = process_args(sys.argv) - sanity_checks(build_map_path, undistort_image_path, args) - mkdir_p(args.work_dir) - undist_image_list, undist_dir, undist_images - = gen_undist_image_list(args.work_dir, args.image_list) + undist_image_list, undist_dir, undist_images \ + = gen_undist_image_list(args.work_dir, args.image_list) # Undistort the images and crop to a central region undist_intrinsics_file = args.work_dir + "/undistorted_intrinsics.txt" - cmd = [undistort_image_path, '-image_list', args.image_list, + cmd = [undistort_image_path, "-image_list", args.image_list, "--undistorted_crop_win", "1100 776", - '-output_list', undist_image_list, - '-undistorted_intrinsics', undist_intrinsics_file] + "-output_list", undist_image_list, + "-undistorted_intrinsics", undist_intrinsics_file] + run_cmd(cmd) + + calib_file = gen_theia_calib_file(args.work_dir, undist_images, undist_intrinsics_file) + recon_file = args.work_dir + "/run" + matching_dir = args.work_dir + "/matches" + + # Wipe old data + for old_recon in glob.glob(recon_file + "*"): + print("Deleting old reconstruction: " + old_recon) + os.remove(old_recon) + + count = 0 + for old_matches in glob.glob(matching_dir + "/*"): + if count == 0: + print("Wiping old matches in: " + matching_dir) + count += 1 + os.remove(old_matches) + + cmd = ["build_reconstruction", "--flagfile", args.theia_flags, + "--images", undist_dir + "/*jpg", + "--calibration_file", calib_file, + "--output_reconstruction", recon_file, + "--matching_working_directory", matching_dir, + "--intrinsics_to_optimize", "NONE"] + run_cmd(cmd) + + nvm_file = recon_file + ".nvm" + cmd = ["export_to_nvm_file", + "-input_reconstruction_file", recon_file + "-0", + '-output_nvm_file', nvm_file] + run_cmd(cmd) + + cmd = [import_map_path, + '-input_map', nvm_file, + '-output_map', args.output_map, + '-undistorted_images_list', undist_image_list, + '-distorted_images_list', args.image_list] + run_cmd(cmd) + + if not args.skip_rebuilding: + cmd = [build_map_path, + '-output_map', args.output_map, + '-rebuild', '-rebuild_refloat_cameras', + '-rebuild_detector', 'SURF', + '--min_valid_angle 1.0' # to avoid features only seen in close-by images + ] run_cmd(cmd) - calib_file = gen_theia_calib_file(args.work_dir, undist_images, undist_intrinsics_file) - - print("--must Have theia in path") - print("Must have a flag file in the repo") - print("all broken below!") - cmd = ['build_reconstruction', '--flagfile', 'build_reconstruction_flags.txt --images' - undist_dir + '/*jpg', '--calibration_file', calib_file, - '--output_reconstruction', - 'small_theia_work/run', - '--matching_working_directory', - 'small_theia_work/work', - '--intrinsics_to_optimize', 'NONE'] diff --git a/localization/sparse_mapping/tools/import_map.cc b/localization/sparse_mapping/tools/import_map.cc index f3f699cd88..a70b76babf 100644 --- a/localization/sparse_mapping/tools/import_map.cc +++ b/localization/sparse_mapping/tools/import_map.cc @@ -39,59 +39,80 @@ DEFINE_string(output_map, "output.map", DEFINE_bool(bundler_map, false, "If true, read the Bundler format. This will be ignored for input .nvm files."); DEFINE_string(undistorted_camera_params, "", - "Intrinsics of the undistorted camera. Specify as: " + "Intrinsics of the undistorted camera. Not needed if --distorted_images_list " + "is specified, as then the camera is set via ASTROBEE_ROBOT. Specify as: " "'wid_x wid_y focal_len opt_ctr_x opt_ctr_y'."); +DEFINE_string(undistorted_images_list, "", + "The full list of undistorted images used to create the sparse map. " + "If not specified, the images should be passed on the command line. " + "Note that not all of them may have been used in the map."); DEFINE_string(distorted_images_list, "", "Replace the undistorted images specified on input with distorted images " - "from this list. The correct value of ASTROBEE_ROBOT must be set."); - -// Replace the undistorted images which Theia used with distorted images -void replaceWithDistortedImages(std::vector const& undist_images, - std::string const& distorted_images_list, - sparse_mapping::SparseMap & map) { - // Must overwrite the camera params for the distorted images - std::cout << "Using distorted camera parameters for nav_cam for robot: " - << getenv("ASTROBEE_ROBOT") << ".\n"; - config_reader::ConfigReader config; - config.AddFile("cameras.config"); - if (!config.ReadFiles()) LOG(FATAL) << "Failed to read config files.\n"; - - camera::CameraParameters cam_params(&config, "nav_cam"); - map.SetCameraParameters(cam_params); - - // Replace the undistorted images with distorted ones - std::vector distorted_images; - std::string image; - std::ifstream ifs(distorted_images_list.c_str()); - while (ifs >> image) - distorted_images.push_back(image); - - if (undist_images.size() != distorted_images.size()) - LOG(FATAL) << "The number of distorted images in the list and undistorted ones " - << "passed on the command line must be the same.\n"; - - std::map undist_to_dist; - for (size_t it = 0; it < undist_images.size(); it++) undist_to_dist[undist_images[it]] = distorted_images[it]; - - // Replace the images in the map. Keep in mind that the map - // may have just a subset of the input images. - for (size_t it = 0; it < map.cid_to_filename_.size(); it++) { - auto map_it = undist_to_dist.find(map.cid_to_filename_[it]); - if (map_it == undist_to_dist.end()) - LOG(FATAL) << "This map image was not specified on input: " - << map.cid_to_filename_[it] << ".\n"; - map.cid_to_filename_[it] = map_it->second; + "from this list (one file per line). The correct value of ASTROBEE_ROBOT " + "must be set."); + +namespace { + // Keep these utilities in a local namespace + + void readLines(std::string const& list, std::vector & lines) { + lines.clear(); + std::string line; + std::ifstream ifs(list.c_str()); + while (ifs >> line) + lines.push_back(line); } -} + + // Replace the undistorted images which Theia used with distorted images. + // The interest points need not be modified as those are always undistorted + // when saved, even if the map has distorted images. + void replaceWithDistortedImages(std::vector const& undist_images, + std::string const& distorted_images_list, + sparse_mapping::SparseMap & map) { + // Must overwrite the camera params for the distorted images + std::cout << "Using distorted camera parameters for nav_cam for robot: " + << getenv("ASTROBEE_ROBOT") << ".\n"; + config_reader::ConfigReader config; + config.AddFile("cameras.config"); + if (!config.ReadFiles()) LOG(FATAL) << "Failed to read config files.\n"; + + camera::CameraParameters cam_params(&config, "nav_cam"); + map.SetCameraParameters(cam_params); + + // Replace the undistorted images with distorted ones + std::vector distorted_images; + readLines(distorted_images_list, distorted_images); + + if (undist_images.size() != distorted_images.size()) + LOG(FATAL) << "The number of distorted images in the list and undistorted ones " + << "passed on the command line must be the same.\n"; + + std::map undist_to_dist; + for (size_t it = 0; it < undist_images.size(); it++) undist_to_dist[undist_images[it]] = distorted_images[it]; + + // Replace the images in the map. Keep in mind that the map + // may have just a subset of the input images. + for (size_t it = 0; it < map.cid_to_filename_.size(); it++) { + auto map_it = undist_to_dist.find(map.cid_to_filename_[it]); + if (map_it == undist_to_dist.end()) + LOG(FATAL) << "This map image was not specified on input: " + << map.cid_to_filename_[it] << ".\n"; + map.cid_to_filename_[it] = map_it->second; + } + } +} // namespace int main(int argc, char** argv) { ff_common::InitFreeFlyerApplication(&argc, &argv); GOOGLE_PROTOBUF_VERIFY_VERSION; - // Read the images from the command line + // Read the images from the .list or the command line std::vector undist_images; - for (int i = 1; i < argc; i++) - undist_images.push_back(argv[i]); + if (FLAGS_undistorted_images_list != "") { + readLines(FLAGS_undistorted_images_list, undist_images); + } else { + for (int i = 1; i < argc; i++) + undist_images.push_back(argv[i]); + } std::cout << "Reading map: " << FLAGS_input_map << std::endl; sparse_mapping::SparseMap map(FLAGS_bundler_map, FLAGS_input_map, undist_images); From ef16ad6447d2ede51621afb3c033dcd66d4438ad Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Mon, 29 Nov 2021 14:37:11 -0800 Subject: [PATCH 11/29] Previous commit made code changes to building with theia, now adding doc --- localization/sparse_mapping/theia_map.md | 134 +++++++++++++++++++++++ 1 file changed, 134 insertions(+) create mode 100644 localization/sparse_mapping/theia_map.md diff --git a/localization/sparse_mapping/theia_map.md b/localization/sparse_mapping/theia_map.md new file mode 100644 index 0000000000..56ce9813d1 --- /dev/null +++ b/localization/sparse_mapping/theia_map.md @@ -0,0 +1,134 @@ +\page theia_map Building a map with Theia + +Theia (https://github.com/sweeneychris/TheiaSfM) is a package for +global structure-from-motion (SfM). It may be faster and require less +user effort than the method used in Astrobee which consists of +creating submaps using incremental SfM on sequences of images that +overlap with their immediate neighbors, followed by merging the +submaps. Theia uses "cascade matching" to handle non-sequential image +acquisitions. + +The Theia approach was tested on about 700 images acquired at +different times and it did well. It is currently studied as an +alternative to Astrobee's approach. + +# Install Theia's prerequisites + +Fetch and install ``conda`` from: + + https://docs.conda.io/projects/conda/en/latest/user-guide/install/linux.html + +Create and activate the environment: + + conda create -n theia + conda activate theia + +(Restart your shell if it is suggested to do so.) + +Run the following command to install some packages and GCC 11: + + conda install -c conda-forge cmake \ + gcc_linux-64==11.1.0 gxx_linux-64==11.1.0 \ + lapack blas eigen==3.3.7 suitesparse rapidjson \ + glog gflags rocksdb OpenImageIO \ + ceres-solver=1.14.0=h0948850_10 + +The Ceres package can be quite particular about the version of Eigen +it uses, and some versions of Ceres are not built with suitesparse +which is a sparse solver that is needed for best performance, so some +care is needed with choosing the versions of the packages. + +# Fetch and build Theia + + git clone git@github.com:sweeneychris/TheiaSfM.git + cd TheiaSfM + git checkout d2112f1 # this version was tested + +Edit the file: + + applications/CMakeLists.txt + +and comment out all the lines regarding OpenGL, GLEW, GLUT, and +view_reconstruction. That visualizer logic is not easy to compile +and is not needed. + +Run ``which cmake`` to ensure its version in the ``theia`` environemnt +installed earlier is used. Do: + + mkdir build + cd build + cmake .. + make -j 20 + +This will create the executables ``build_reconstruction`` and +``export_to_nvm_file`` in the ``bin`` subdirectory of ``build``. That +directory needs to be added to the PATH. + +# Run the Astrobee wrapper around the Theia tools + +First set the environment. The following lines should be adjusted as needed, +especially the robot name: + + export ASTROBEE_SOURCE_PATH=$HOME/projects/astrobee/src + export ASTROBEE_BUILD_PATH=$HOME/projects/astrobee/build + source $ASTROBEE_BUILD_PATH/devel/setup.zsh + export ASTROBEE_RESOURCE_DIR=$ASTROBEE_SOURCE_PATH/astrobee/resources + export ASTROBEE_CONFIG_DIR=$ASTROBEE_SOURCE_PATH/astrobee/config + export ASTROBEE_WORLD=iss + export ASTROBEE_ROBOT=bumble + + python ASTROBEE_SOURCE_PATH/localization/sparse_mapping/tools/build_theia_map.py \ + --output_map theia.map --work_dir theia_work --image_list image_list.txt + +This will take care of preparing everything Theia needs, will run it, +and will export the resulting map to Astrobee's expected format. This +map will need to be registered and visualized as described in other +documentation. The work directory can be deleted later. + +This tool has the following command-line options: + + --theia_flags: The flags to pass to Theia. If not specified, use + localization/sparse_mapping/theia_flags.txt in the Astrobee repo. + --image_list: The list of distorted (original) nav cam images to + use, one per line. + --output_map: The resulting output map. + --skip_rebuilding: Do not rebuild the map after importing it from + Theia. + --work_dir: A temporary work directory to be deleted by the user + later. + --help: Show this help message and exit. + +# Auxiliary import_map tool + +This tool is used to import a map from the NVM format, which Theia +exports to. These operations are done automatically by the +``build_theia_map.py'' tool. This documentation is provided for +reference only. + +An NVM map exported by Theia (or some other SfM tool) can be saved as +an Astrobee sparse map with the command:: + + astrobee/build/devel/lib/sparse_mapping/import_map \ + -undistorted_camera_params "wid_x wid_y focal_len opt_ctr_x opt_ctr_y" \ + \ + -input_map map.nvm -output_map map.map + +This assumes that the images were acquired with the nav camera of the +robot given by $ASTROBEE_ROBOT and undistorted with the Astrobee +program ``undistort_image``. The undistorted camera parameters to use +should be as printed on the screen by ``undistort_image``. + +If desired to replace on importing the undistorted images with the +original distorted ones, as it is usually expected of a sparse map, +the above command should be called instead as:: + + astrobee/build/devel/lib/sparse_mapping/import_map \ + -undistorted_images_list undist_list.txt \ + -distorted_images_list dist_list.txt \ + -input_map map.nvm -output_map map.map + +Here, the files undist_list.txt and dist_list.txt must have one image +per line and be in one-to-one correspondence. It is important that +both undistorted and distorted images be specified, as the former are +needed to look up camera poses and other data in the .nvm file before +being replaced with the distorted ones. From 8c0f022276d9db0026c2562235fbf01c2441415a Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 09:28:21 -0800 Subject: [PATCH 12/29] Fix formatting of cross-links in documentation --- localization/sparse_mapping/build_map.md | 39 ++++++++++---------- localization/sparse_mapping/readme.md | 38 ++++++++----------- localization/sparse_mapping/total_station.md | 22 +++++------ localization/sparse_mapping/using_faro.md | 2 +- 4 files changed, 46 insertions(+), 55 deletions(-) diff --git a/localization/sparse_mapping/build_map.md b/localization/sparse_mapping/build_map.md index 216c821687..77610a4b17 100644 --- a/localization/sparse_mapping/build_map.md +++ b/localization/sparse_mapping/build_map.md @@ -30,14 +30,14 @@ The higher the value of the density factor, the more images will be kept. Some experimentation with this number is necessary. A value of 1.4 seems to work well. It may be needed to decrease this to 1.0 if images appear to be too dense. Ideally the images should have perhaps -on the order of 2/3 to 3/4 of overlap. This tool is not perfect. One -should inspect the images in the `eog` viewer, and delete redundant +on the order of 3/4 to 4/5 of overlap. This tool is not perfect. One +should inspect the images in the ``eog`` viewer, and delete redundant ones from it manually, using the Delete key. -The images can also be inspected and deleted with nvm_visualize, a -tool included with this software. See readme.md for details. This -tool, unlike eog, echoes each image name as it is displayed, which can -be useful with image manipulation tasks. +The images can also be inspected and deleted with ``nvm_visualize``, a +tool included with this software. See \subpage sparsemapping for +details. This tool, unlike ``eog``, echoes each image name as it is +displayed, which can be useful with image manipulation tasks. If the images do not have enough overlap, the selection tool needs to be run again with a different value of this factor, or otherwise @@ -81,9 +81,8 @@ and none of the other available config files apply, you can just temporarily modify the above files to reflect your camera's parameters (without checking in your changes). -More details on these and other environmental variables can be found in - - $SOURCE_PATH/astrobee/readme.md +More details on these and other environmental variables can be found +in the \subpage astrobee configuration documentation. ## Building a map @@ -110,8 +109,8 @@ before doing feature detection. It was shown to create maps that are more robust to illumination changes. In practice, the map is build in pieces, and then merged. Then the -above process needs to be modified. See readme.md in this directory -for how this approach should go. +above process needs to be modified. See \subpage sparsemapping for the +procedure. ### Map building pipeline @@ -198,8 +197,8 @@ would drift from each other. If it is desired to take out images from the map, it should happen at this stage, before the vocabulary database and pruning happens at the -next step. See readme.md when it comes to such operations, where the -script grow_map.py is used. +next step. See \subpage sparsemapping when it comes to such +operations, where the script grow_map.py is used. #### Vocabulary database @@ -290,8 +289,8 @@ The xyz locations of the control points for the granite lab, the ISS and MGTF are mentioned below. If a set of world coordinates needs to be acquired, one can use the -Total Station, as described in the [total station](total_station.md) -documentation. +\subpage total_station. (Alternatively one can can try the +\subpage faro instrument but that is more technically involved.) Register the map with the command: @@ -352,7 +351,7 @@ new image set. ### Registration in the granite lab See the xyz coordinates of the control points used for registration in -[granite_lab_registration.md](granite_lab_registration.md) +the \subpage granite_lab_registration section. ### Registration on the ISS @@ -363,7 +362,7 @@ be visualized in the ISS as follows: Open two terminals, and in each one type: - export BUILD_PATH=$HOME/astrobee_build/native + export BUILD_PATH=$HOME/astrobee/build source $BUILD_PATH/devel/setup.bash In the first terminal start the simulator: @@ -570,9 +569,9 @@ and compared to the old ones via: ## Evaluating the map without running the localization node -See astrobee/tools/ekf_bag/readme.md for how to run the -sparse_map_eval tool that takes as inputs a bag and a BRISK map and -prints the number of detected features. +See the \subpage ekfbag page for how to run the ``sparse_map_eval`` +tool that takes as inputs a bag and a BRISK map and prints the number +of detected features. Note that this approach may give slightly different results than using the localization node, and even with using this node, things can diff --git a/localization/sparse_mapping/readme.md b/localization/sparse_mapping/readme.md index ffd1e8b106..ec21929964 100644 --- a/localization/sparse_mapping/readme.md +++ b/localization/sparse_mapping/readme.md @@ -1,4 +1,4 @@ -\page sparsemapping Sparse Mapping +\page sparsemapping Sparse mapping # Creation of sparse maps for robot localization @@ -19,8 +19,9 @@ detected in the image and their 3D coordinates. ### Inputs -* `/hw/cam_nav`: Camera images -* The map file. See [build_map](build_map.md) at the bottom for its assumed location. +* `/hw/cam_nav`: Camera images The map file. See the \subpage + map_building section (towards the bottom) for its assumed location + on the robot. ### Outputs @@ -90,8 +91,8 @@ name can change. ### Building a map -The `build_map` tools aids in constructing a map. See -[build_map](build_map.md) for further details. +The ``build_map`` tools is used to construct a map. See \subpage +map_building for further details. ### Visualization @@ -146,7 +147,7 @@ built. It can also delete images in this mode, with the 'Delete' and ### Localize a single frame All the commands below assume that the environment was set up, -as specified in build_map.md. +as specified in the \subpage map_building section. To test localization of a single frame, use the command: @@ -196,12 +197,8 @@ for speed and here we want more accuracy. ### Testing localization using a bag -See: - - astrobee/tools/ekf_bag/readme.md - -for how to see how well a BRISK map with a vocabulary database does -when localizing images from a bag. +See the \subpage ekfbag page for how to study how well a BRISK map +with a vocabulary database does when localizing images from a bag. ### Extract sub-maps @@ -248,8 +245,8 @@ Given a set of SURF maps, they can be merged using the command: -num_image_overlaps_at_endpoints 50 It is very important to note that only maps with SURF features (see -build_map.md) can be merged. If a map has BRISK features, it needs to -be rebuilt with SURF features, as follows: +\subpage map_building) can be merged. If a map has BRISK features, it +needs to be rebuilt with SURF features, as follows: build_map -rebuild -histogram_equalization \ -rebuild_detector SURF -output_map @@ -325,10 +322,10 @@ maps using the command: images/*jpg -output_map examine them individually, merging them as appropriate, then -performing bundle adjustment and registration as per build_map.md. -Only when a good enough map is obtained, a renamed copy of it -should be rebuilt with BRISK features and a vocabulary database -to be used on the robot. +performing bundle adjustment and registration as per the \subpage +map_building section. Only when a good enough map is obtained, a +renamed copy of it should be rebuilt with BRISK features and a +vocabulary database to be used on the robot. ### Map strategy for the space station @@ -480,8 +477,3 @@ Instead of taking images out of the map randomly, one can start with a reduced map with a small list of desired images which can be set with -image_list, and then all images for which localization fails will be added back to it. - -\subpage map_building -\subpage total_station -\subpage granite_lab_registration -\subpage using_faro \ No newline at end of file diff --git a/localization/sparse_mapping/total_station.md b/localization/sparse_mapping/total_station.md index e58f737998..9360a2d617 100644 --- a/localization/sparse_mapping/total_station.md +++ b/localization/sparse_mapping/total_station.md @@ -54,29 +54,29 @@ granite lab. Once you are done you need to export your project to the GSI format so you can read it back in. * Keep hitting ESC until you are back to the home screen. - * Press "4" to convert - * Press "1" to export + * Press "4" to convert. + * Press "1" to export. * Select Job, change to what ever is your job. * Select Format File, change to "gsi16_cartesian.frt". * Select File Name, write value for what you want the output file to be. * Press "F1" to save. - * Press "F4" to close out. + * Press "F4" to exit. Turn the nob on the side of the station where it says "CF Card" and pull the card out. Hook it up to a USB reader and get the measurement file. Here is a sample data file: -*110001+0000000000ORIGIN 81...0+0000000000000000 82...0+0000000000000000 83...0+0000000000000000 -*110002+000000000000YREF 81...0+0000000000000000 82...0+0000000000006417 83...0-0000000000000022 -*110003+0000000000000003 81...0+0000000000000004 82...0+0000000000006535 83...0-0000000000000786 + *110001+0000000000ORIGIN 81...0+0000000000000000 82...0+0000000000000000 83...0+0000000000000000 + *110002+000000000000YREF 81...0+0000000000000000 82...0+0000000000006417 83...0-0000000000000022 + *110003+0000000000000003 81...0+0000000000000004 82...0+0000000000006535 83...0-0000000000000786 What this means is that the points are in Cartesian coordinates with x,y,z measured in millimeters (note that some values are negative). After removing the redundant text and converting to meters, this looks like: -Point 1 (Origin): 0.0 0.0 0.0 -Point 2 (YREF) : 0.0 6.417 -0.022 -Point 3 : 0.004 6.535 -0.786 + Point 1 (Origin): 0.0 0.0 0.0 + Point 2 (YREF) : 0.0 6.417 -0.022 + Point 3 : 0.004 6.535 -0.786 -The program +The program: - localization/sparse_mapping/tools/parse_total_station.py + localization/sparse_mapping/tools/parse_total_station.py can be used to convert from the Total Station format to an xyz format that can be used for sparse map registration. diff --git a/localization/sparse_mapping/using_faro.md b/localization/sparse_mapping/using_faro.md index eec2c98527..989933d6fd 100644 --- a/localization/sparse_mapping/using_faro.md +++ b/localization/sparse_mapping/using_faro.md @@ -1,4 +1,4 @@ -\page using_faro Using Faro +\page faro Faro IRG has an instrument, called FARO, that can create very high-resolution 3D scans with overlaid texture. Such a scan can be From 45e2f1e949060435c6b2b126ccd047b61b80ecf1 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 09:33:50 -0800 Subject: [PATCH 13/29] Update scicam_to_hazcam_timestamp_offset. This is still in flux. --- astrobee/config/robots/bumble.config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/astrobee/config/robots/bumble.config b/astrobee/config/robots/bumble.config index 9aaa23737a..c1f73031e8 100644 --- a/astrobee/config/robots/bumble.config +++ b/astrobee/config/robots/bumble.config @@ -28,7 +28,7 @@ robot_geometry = { hazcam_to_navcam_transform = transform(vec3(0.07142937, 0.00058221217, -0.001373669), quat4(-0.0030431141, 0.0092646368, 0.99993195, -0.0064039206)), scicam_to_hazcam_transform = transform(vec3(-0.0052887445, 0.010298013, -0.043606689), quat4(0.0018545621, 0.012796392, 0.99991616, -0.00069204825)), navcam_to_hazcam_timestamp_offset = -0.02, - scicam_to_hazcam_timestamp_offset = -0.2, + scicam_to_hazcam_timestamp_offset = 0.5, hazcam_depth_to_image_transform = { 0.91602851, -0.00031586647, -0.0028485861, 0.0029767338, 0.00032189197, 0.91603089, 0.0019373744, -0.0020741879, From a5f70d19319a2379e073ee49b25c7e3a83a132f6 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 09:46:10 -0800 Subject: [PATCH 14/29] Add a sample theia_flags.txt file --- localization/sparse_mapping/theia_flags.txt | 172 ++++++++++++++++++++ 1 file changed, 172 insertions(+) create mode 100644 localization/sparse_mapping/theia_flags.txt diff --git a/localization/sparse_mapping/theia_flags.txt b/localization/sparse_mapping/theia_flags.txt new file mode 100644 index 0000000000..6d0216ebf5 --- /dev/null +++ b/localization/sparse_mapping/theia_flags.txt @@ -0,0 +1,172 @@ +############### Input/Output ############### +# Input/output files. +# Set these if a matches file is not present. Images should be a filepath with a +# wildcard e.g., /home/my_username/my_images/*.jpg +# These should be left empty if specified on the command line. +--images= +--output_matches_file= + +# If a matches file has already been created, set the filepath here. This avoids +# having to recompute all features and matches. +# This should be left empty if specified on the command line. +--matches_file= + +# The calibration file indicates possibly known calibration e.g, from EXIF or +# explicit calibration. Theia attempts to extract EXIF focal lengths if calibration +# is not supplied for a given image. +# These should be left empty if specified on the command line. +--calibration_file= +--output_reconstruction= + +############### Multithreading ############### +# Set to the number of threads you want to use. +--num_threads=16 + +############### Feature Extraction ############### +--descriptor=SIFT +--feature_density=NORMAL + +############### Matching Options ############### +# Perform matching out-of-core. If set to true, the matching_working_directory +# must be set to a valid, writable directory (the directory will be created if +# it does not exits) Set to false to perform all-in-memory matching. +--match_out_of_core=true + +# During feature matching, features are saved to disk so that out-of-core +# matching may be performed. This directory specifies which directory those +# features should be saved to. +# This should be left empty if specified on the command line. +--matching_working_directory= + +# During feature matching we utilize an LRU cache for out-of-core matching. The size +# of that cache (in terms of number of images) is controlled by this parameter. The +# higher this number the more memory is required. +--matching_max_num_images_in_cache=128 + +--matching_strategy=CASCADE_HASHING +--lowes_ratio=0.75 +--min_num_inliers_for_valid_match=30 +# NOTE: This threshold is relative to an image with a width of 1024 pixels. It +# will be scaled appropriately based on the image resolutions. This allows a +# single threshold to be used for images with different resolutions. +--max_sampson_error_for_verified_match=6.0 +--bundle_adjust_two_view_geometry=true +--keep_only_symmetric_matches=true + +# Global descriptor extractor settings. The global image descriptors are used to +# speed up matching by selected the K most similar images for each image, and +# only performing feature matching with these images. +--num_nearest_neighbors_for_global_descriptor_matching=100 +--num_gmm_clusters_for_fisher_vector=16 +--max_num_features_for_fisher_vector_training=1000000 + +############### General SfM Options ############### +--reconstruction_estimator=GLOBAL +--min_track_length=2 +--max_track_length=50 +--reconstruct_largest_connected_component=false + +# Set to true if all views were captured with the same camera. If true, then a +# single set of camera intrinsic parameters will be used for all views in the +# reconstruction. +--shared_calibration=true + +# If set to true, only views with known calibration are reconstructed. +--only_calibrated_views=false + +############### Global SfM Options ############### +--global_position_estimator=LEAST_UNSQUARED_DEVIATION +--global_rotation_estimator=ROBUST_L1L2 +# The value below should be bigger to filter less +--post_rotation_filtering_degrees=10 + +# This refinement is very unstable for rotation-only motions so +# it is advised that this is set to false for these motions. +--refine_relative_translations_after_rotation_estimation=false + +# If true, only cameras that are well-conditioned for position estimation will +# be used for global position estimation +--extract_maximal_rigid_subgraph=false + +# Filter the relative translations with the 1DSfM filter to remove potential +# outliers in the relative pose measurements. +--filter_relative_translations_with_1dsfm=true + +# Nonlinear position estimation options +--position_estimation_min_num_tracks_per_view=0 +--position_estimation_robust_loss_width=0.1 + +# If true, perform a single iteration of bundle adjustment only on the camera +# positions and 3D points (rotation and camera intrinsics are held +# constant). This helps often to constrain inaccurate intrinsics. +--refine_camera_positions_and_points_after_position_estimation=true + +# After estimating camera poses, we perform trianguation, then BA, +# then filter out bad points. This controls how many times we repeat +# this process. +--num_retriangulation_iterations=1 + +############### Incremental SfM Options ############### +# NOTE: This threshold is relative to an image with a width of 1024 pixels. It +# will be scaled appropriately based on the image resolutions. This allows a +# single threshold to be used for images with different resolutions. +--absolute_pose_reprojection_error_threshold=4 +--partial_bundle_adjustment_num_views=20 +--full_bundle_adjustment_growth_percent=5 +--min_num_absolute_pose_inliers=30 + +############### Bundle Adjustment Options ############### +# Set this parameter to a value other than NONE if you want to utilize a robust +# cost function during bundle adjustment. This can increase robustness to outliers +# during the optimization. +--bundle_adjustment_robust_loss_function=HUBER + +# Set this value to the determine the reprojection error in pixels at which +# robustness begins (if a robust cost function is being used). +# TODO(oalexan1): This looks too high. In Astrobee we use 2.0. +--bundle_adjustment_robust_loss_width=10.0 + +# Set this parameter to change which camera intrinsics should be +# optimized. Valid options are NONE, ALL, FOCAL_LENGTH, PRINCIPAL_POINTS, +# RADIAL_DISTORTION, ASPECT_RATIO, and SKEW. This parameter can be set using a +# bitmask (with no spaces) e.g., FOCAL_LENGTH|RADIAL_DISTORTION +--intrinsics_to_optimize=NONE + +# After BA, remove any points that have a reprojection error greater +# than this. +--max_reprojection_error_pixels=8.0 + +############### Track Subsampling Options ############### + +# If true, the estimated tracks are subsampled for bundle adjustment to increase +# the efficiency of BA. Tracks are chosen in a way which attempts to constraint +# BA as best as possible. This has been shown to provide a significant speedup +# without reducing the accuracy much (in fact, it increases the accuracy in some +# cases). +--subsample_tracks_for_bundle_adjustment=false + +# Subsampled tracks are chosen with a probability related to their track +# length. We limit the effect of long tracks by capping the track length at this +# value for the purpose of selecting tracks. +--track_subset_selection_long_track_length_threshold=10 + +# Tracks are chosen in a way that ensures each view is spatially +# constrained. Tracks are first binned in an image grid and the top ranked track +# is chosen for optimization from each grid cell to ensure good spatial coverage +# of the image. The grid cells are set to be this size. +--track_selection_image_grid_cell_size_pixels=100 + +# Tracks are chosen such that each image observes at least this many tracks +# which are being optimized. This ensures that each image is well-constrained. +--min_num_optimized_tracks_per_view=100 + +############### Triangulation Options ############### +--min_triangulation_angle_degrees=0.1 +--triangulation_reprojection_error_pixels=15.0 +--bundle_adjust_tracks=true + +############### Logging Options ############### +# Logging verbosity. +--logtostderr +# Increase this number to get more verbose logging. +--v=1 From b424f98f13704ed9bae73f79e9550260578c3fef Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 09:53:21 -0800 Subject: [PATCH 15/29] Fix minor document and script issues --- localization/sparse_mapping/theia_map.md | 11 ++++++----- localization/sparse_mapping/tools/build_theia_map.py | 2 +- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/localization/sparse_mapping/theia_map.md b/localization/sparse_mapping/theia_map.md index 56ce9813d1..67909eeb65 100644 --- a/localization/sparse_mapping/theia_map.md +++ b/localization/sparse_mapping/theia_map.md @@ -18,13 +18,13 @@ Fetch and install ``conda`` from: https://docs.conda.io/projects/conda/en/latest/user-guide/install/linux.html +Restart your shell if it is suggested to do so. + Create and activate the environment: conda create -n theia conda activate theia -(Restart your shell if it is suggested to do so.) - Run the following command to install some packages and GCC 11: conda install -c conda-forge cmake \ @@ -53,7 +53,8 @@ view_reconstruction. That visualizer logic is not easy to compile and is not needed. Run ``which cmake`` to ensure its version in the ``theia`` environemnt -installed earlier is used. Do: +installed earlier is used. Otherwise run again ``conda activate +theia``. Do: mkdir build cd build @@ -71,13 +72,13 @@ especially the robot name: export ASTROBEE_SOURCE_PATH=$HOME/projects/astrobee/src export ASTROBEE_BUILD_PATH=$HOME/projects/astrobee/build - source $ASTROBEE_BUILD_PATH/devel/setup.zsh + source $ASTROBEE_BUILD_PATH/devel/setup.bash export ASTROBEE_RESOURCE_DIR=$ASTROBEE_SOURCE_PATH/astrobee/resources export ASTROBEE_CONFIG_DIR=$ASTROBEE_SOURCE_PATH/astrobee/config export ASTROBEE_WORLD=iss export ASTROBEE_ROBOT=bumble - python ASTROBEE_SOURCE_PATH/localization/sparse_mapping/tools/build_theia_map.py \ + python $ASTROBEE_SOURCE_PATH/localization/sparse_mapping/tools/build_theia_map.py \ --output_map theia.map --work_dir theia_work --image_list image_list.txt This will take care of preparing everything Theia needs, will run it, diff --git a/localization/sparse_mapping/tools/build_theia_map.py b/localization/sparse_mapping/tools/build_theia_map.py index a4fe7daa63..a943ed77bf 100644 --- a/localization/sparse_mapping/tools/build_theia_map.py +++ b/localization/sparse_mapping/tools/build_theia_map.py @@ -275,7 +275,7 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): '-output_map', args.output_map, '-rebuild', '-rebuild_refloat_cameras', '-rebuild_detector', 'SURF', - '--min_valid_angle 1.0' # to avoid features only seen in close-by images + '--min_valid_angle', '1.0' # to avoid features only seen in close-by images ] run_cmd(cmd) From 5cfb01bf407e66196b913bccd52108d1bfe27ca9 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 10:00:03 -0800 Subject: [PATCH 16/29] Try to run the code formatter on python --- localization/sparse_mapping/tools/build_theia_map.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/sparse_mapping/tools/build_theia_map.py b/localization/sparse_mapping/tools/build_theia_map.py index a943ed77bf..e61c41817f 100644 --- a/localization/sparse_mapping/tools/build_theia_map.py +++ b/localization/sparse_mapping/tools/build_theia_map.py @@ -275,7 +275,7 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): '-output_map', args.output_map, '-rebuild', '-rebuild_refloat_cameras', '-rebuild_detector', 'SURF', - '--min_valid_angle', '1.0' # to avoid features only seen in close-by images + '--min_valid_angle', '1.0' # to avoid features only seen in close-by images ] run_cmd(cmd) From f22f10de88336a28c80c1ce89f4071a1b082f609 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 10:16:20 -0800 Subject: [PATCH 17/29] Still arguing with the linter --- localization/sparse_mapping/tools/build_theia_map.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/sparse_mapping/tools/build_theia_map.py b/localization/sparse_mapping/tools/build_theia_map.py index e61c41817f..a943ed77bf 100644 --- a/localization/sparse_mapping/tools/build_theia_map.py +++ b/localization/sparse_mapping/tools/build_theia_map.py @@ -275,7 +275,7 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): '-output_map', args.output_map, '-rebuild', '-rebuild_refloat_cameras', '-rebuild_detector', 'SURF', - '--min_valid_angle', '1.0' # to avoid features only seen in close-by images + '--min_valid_angle', '1.0' # to avoid features only seen in close-by images ] run_cmd(cmd) From 6d9d31644df7b9841cda8af57e82e84fbd209638 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 10:23:59 -0800 Subject: [PATCH 18/29] Fix some issues with pre-commit.linter_python, it was failing when it should succeed --- scripts/git/pre-commit.linter_python | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/scripts/git/pre-commit.linter_python b/scripts/git/pre-commit.linter_python index 80ea0a186b..e0565b8b05 100755 --- a/scripts/git/pre-commit.linter_python +++ b/scripts/git/pre-commit.linter_python @@ -28,16 +28,18 @@ fi failed_lint=false -if ! command -v black &> /dev/null -then - echo "Black was not found, to install check https://github.com/psf/black" - echo "Unfortunately black requires Python 3.6.2+ to run" +command -v black +ans=$? +if [ "$ans" -ne "0" ]; then + echo "'black' was not found. To install, check https://github.com/psf/black." + echo "Note that black requires Python 3.6.2+ to run." exit 0 fi -if ! command -v isort &> /dev/null -then - echo "isort was not found, to install check https://github.com/PyCQA/isort" +command -v isort +ans=$? +if [ "$ans" -ne "0" ]; then + echo "'isort' was not found. To install, check https://github.com/PyCQA/isort." exit 0 fi From 308e64496367c0ac608e873ea668389058849bed Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 10:24:39 -0800 Subject: [PATCH 19/29] Try to fix the python linter issues --- .../sparse_mapping/tools/build_theia_map.py | 283 ++++++++++++------ 1 file changed, 184 insertions(+), 99 deletions(-) diff --git a/localization/sparse_mapping/tools/build_theia_map.py b/localization/sparse_mapping/tools/build_theia_map.py index a943ed77bf..c16b24a433 100644 --- a/localization/sparse_mapping/tools/build_theia_map.py +++ b/localization/sparse_mapping/tools/build_theia_map.py @@ -16,17 +16,25 @@ # License for the specific language governing permissions and limitations # under the License. -''' +""" Undistort the images, create a theia map, then convert it to a sparse map as understood by astrobee software, and rebuild it with distorted images and SURF features, while reoptimizing the cameras. The Theia build_reconstruction tool must be in the PATH. -''' +""" + +import argparse +import glob +import os +import re +import shutil +import subprocess +import sys -import sys, os, argparse, subprocess, re, shutil, glob def which(program): - '''Find if a program is in the PATH''' + """Find if a program is in the PATH""" + def is_exe(fpath): return os.path.isfile(fpath) and os.access(fpath, os.X_OK) @@ -43,30 +51,37 @@ def is_exe(fpath): return None + def run_cmd(cmd): - ''' + """ Run a command. - ''' + """ print(" ".join(cmd) + "\n") process = subprocess.Popen(cmd, stdout=subprocess.PIPE) - process.wait() - if (process.returncode != 0): + process.wait() + if process.returncode != 0: print("Failed execution of: " + " ".join(cmd)) sys.exit(1) + def sanity_checks(undistort_image_path, import_map_path, build_map_path, args): # Check if the environment was set - for var in ['ASTROBEE_RESOURCE_DIR', 'ASTROBEE_CONFIG_DIR', 'ASTROBEE_WORLD', 'ASTROBEE_ROBOT']: + for var in [ + "ASTROBEE_RESOURCE_DIR", + "ASTROBEE_CONFIG_DIR", + "ASTROBEE_WORLD", + "ASTROBEE_ROBOT", + ]: if var not in os.environ: raise Exception("Must set " + var) if not os.path.exists(undistort_image_path): raise Exception("Executable does not exist: " + undistort_image_path) - + if not os.path.exists(import_map_path): raise Exception("Executable does not exist: " + import_map_path) - + if not os.path.exists(build_map_path): raise Exception("Executable does not exist: " + build_map_path) @@ -82,58 +97,85 @@ def sanity_checks(undistort_image_path, import_map_path, build_map_path, args): if not os.path.exists(args.theia_flags): raise Exception("Cannot find the Theia flags file: " + args.theia_flags) - if which('build_reconstruction') is None: + if which("build_reconstruction") is None: raise Exception("Cannot find the 'build_reconstruction' program in PATH.") - - if which('export_to_nvm_file') is None: + + if which("export_to_nvm_file") is None: raise Exception("Cannot find the 'export_to_nvm_file' program in PATH.") - + + def process_args(args): - ''' + """ Set up the parser and parse the args. - ''' - + """ + # Extract some paths before the args are parsed src_path = os.path.dirname(args[0]) - exec_path = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(src_path)))) - undistort_image_path = os.path.join(exec_path, "build/devel/lib/camera/undistort_image") - import_map_path = os.path.join(exec_path, "build/devel/lib/sparse_mapping/import_map") + exec_path = os.path.dirname( + os.path.dirname(os.path.dirname(os.path.dirname(src_path))) + ) + undistort_image_path = os.path.join( + exec_path, "build/devel/lib/camera/undistort_image" + ) + import_map_path = os.path.join( + exec_path, "build/devel/lib/sparse_mapping/import_map" + ) build_map_path = os.path.join(exec_path, "build/devel/lib/sparse_mapping/build_map") parser = argparse.ArgumentParser(description="Parameters for the geometry mapper.") - parser.add_argument("--theia_flags", dest = "theia_flags", default = "", - help = "The flags to pass to Theia. If not specified, use " + - "localization/sparse_mapping/theia_flags.txt in the astrobee repo.") - parser.add_argument("--image_list", dest = "image_list", default = "", - help = "The list of distorted (original) nav cam images to use, " + - "one per line.") - parser.add_argument("--output_map", dest = "output_map", default = "", - help = "The resulting output map.") - parser.add_argument('--skip_rebuilding', dest = 'skip_rebuilding', action = 'store_true', - help = 'Do not rebuild the map after importing it from Theia.') - parser.add_argument("--work_dir", dest = "work_dir", default = "", - help = "A temporary work directory to be deleted by the user later.") + parser.add_argument( + "--theia_flags", + dest="theia_flags", + default="", + help="The flags to pass to Theia. If not specified, use " + + "localization/sparse_mapping/theia_flags.txt in the astrobee repo.", + ) + parser.add_argument( + "--image_list", + dest="image_list", + default="", + help="The list of distorted (original) nav cam images to use, " + + "one per line.", + ) + parser.add_argument( + "--output_map", dest="output_map", default="", help="The resulting output map." + ) + parser.add_argument( + "--skip_rebuilding", + dest="skip_rebuilding", + action="store_true", + help="Do not rebuild the map after importing it from Theia.", + ) + parser.add_argument( + "--work_dir", + dest="work_dir", + default="", + help="A temporary work directory to be deleted by the user later.", + ) args = parser.parse_args() if args.theia_flags == "": args.theia_flags = os.path.dirname(src_path) + "/theia_flags.txt" - + sanity_checks(undistort_image_path, import_map_path, build_map_path, args) - + return (src_path, undistort_image_path, import_map_path, build_map_path, args) + def mkdir_p(path): if path == "": - return # this can happen when path is os.path.dirname("myfile.txt") + return # this can happen when path is os.path.dirname("myfile.txt") try: os.makedirs(path) except OSError: if os.path.isdir(path): pass else: - raise Exception("Could not make directory " + path + \ - " as a file with this name exists.") + raise Exception( + "Could not make directory " + path + " as a file with this name exists." + ) + # Theia likes its images undistorted and in one directory def gen_undist_image_list(work_dir, dist_image_list): @@ -150,8 +192,8 @@ def gen_undist_image_list(work_dir, dist_image_list): undist_image_list = work_dir + "/undistorted_list.txt" print("Writing: " + undist_image_list) - - count = 10000 # to have the created images show up nicely + + count = 10000 # to have the created images show up nicely undist_images = [] with open(dist_image_list, "r") as dfh: dist_image_files = dfh.readlines() @@ -165,9 +207,10 @@ def gen_undist_image_list(work_dir, dist_image_list): return undist_image_list, undist_dir, undist_images + def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): - - calib_file = work_dir + "/" + "theia_calibration.json"; + + calib_file = work_dir + "/" + "theia_calibration.json" # Parse the intrinsics with open(undist_intrinsics_file, "r") as fh: @@ -179,64 +222,83 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): intrinsics = line.split() if len(intrinsics) < 5: - raise Exception("Expecting 5 intrinsics numbers in: " + undist_intrinsics_file) - - width = intrinsics[0] - height = intrinsics[1] + raise Exception( + "Expecting 5 intrinsics numbers in: " + undist_intrinsics_file + ) + + width = intrinsics[0] + height = intrinsics[1] focal_length = intrinsics[2] - opt_ctr_x = intrinsics[3] - opt_ctr_y = intrinsics[4] - + opt_ctr_x = intrinsics[3] + opt_ctr_y = intrinsics[4] + break - + print("Writing: " + calib_file) with open(calib_file, "w") as fh: fh.write("{\n") - fh.write("\"priors\" : [\n") + fh.write('"priors" : [\n') num_images = len(undist_images) for it in range(num_images): image = undist_images[it] - fh.write("{\"CameraIntrinsicsPrior\" : {\n"); - fh.write("\"image_name\" : \"" + image + "\",\n"); - fh.write("\"width\" : " + str(width) + ",\n"); - fh.write("\"height\" : " + str(height) + ",\n"); - fh.write("\"camera_intrinsics_type\" : \"PINHOLE\",\n"); - fh.write("\"focal_length\" : " + str(focal_length) + ",\n"); - fh.write("\"principal_point\" : [" + str(opt_ctr_x) + ", " + str(opt_ctr_y) + "]\n"); - + fh.write('{"CameraIntrinsicsPrior" : {\n') + fh.write('"image_name" : "' + image + '",\n') + fh.write('"width" : ' + str(width) + ",\n") + fh.write('"height" : ' + str(height) + ",\n") + fh.write('"camera_intrinsics_type" : "PINHOLE",\n') + fh.write('"focal_length" : ' + str(focal_length) + ",\n") + fh.write( + '"principal_point" : [' + str(opt_ctr_x) + ", " + str(opt_ctr_y) + "]\n" + ) + if it < num_images - 1: fh.write("}},\n") else: fh.write("}}\n") - + fh.write("]\n") fh.write("}\n") - + return calib_file - + if __name__ == "__main__": - - (src_path, undistort_image_path, import_map_path, build_map_path, args) \ - = process_args(sys.argv) + + ( + src_path, + undistort_image_path, + import_map_path, + build_map_path, + args, + ) = process_args(sys.argv) mkdir_p(args.work_dir) - undist_image_list, undist_dir, undist_images \ - = gen_undist_image_list(args.work_dir, args.image_list) + undist_image_list, undist_dir, undist_images = gen_undist_image_list( + args.work_dir, args.image_list + ) # Undistort the images and crop to a central region undist_intrinsics_file = args.work_dir + "/undistorted_intrinsics.txt" - cmd = [undistort_image_path, "-image_list", args.image_list, - "--undistorted_crop_win", "1100 776", - "-output_list", undist_image_list, - "-undistorted_intrinsics", undist_intrinsics_file] + cmd = [ + undistort_image_path, + "-image_list", + args.image_list, + "--undistorted_crop_win", + "1100 776", + "-output_list", + undist_image_list, + "-undistorted_intrinsics", + undist_intrinsics_file, + ] run_cmd(cmd) - - calib_file = gen_theia_calib_file(args.work_dir, undist_images, undist_intrinsics_file) - recon_file = args.work_dir + "/run" + + calib_file = gen_theia_calib_file( + args.work_dir, undist_images, undist_intrinsics_file + ) + recon_file = args.work_dir + "/run" matching_dir = args.work_dir + "/matches" - + # Wipe old data for old_recon in glob.glob(recon_file + "*"): print("Deleting old reconstruction: " + old_recon) @@ -248,34 +310,57 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): print("Wiping old matches in: " + matching_dir) count += 1 os.remove(old_matches) - - cmd = ["build_reconstruction", "--flagfile", args.theia_flags, - "--images", undist_dir + "/*jpg", - "--calibration_file", calib_file, - "--output_reconstruction", recon_file, - "--matching_working_directory", matching_dir, - "--intrinsics_to_optimize", "NONE"] + + cmd = [ + "build_reconstruction", + "--flagfile", + args.theia_flags, + "--images", + undist_dir + "/*jpg", + "--calibration_file", + calib_file, + "--output_reconstruction", + recon_file, + "--matching_working_directory", + matching_dir, + "--intrinsics_to_optimize", + "NONE", + ] run_cmd(cmd) - + nvm_file = recon_file + ".nvm" - cmd = ["export_to_nvm_file", - "-input_reconstruction_file", recon_file + "-0", - '-output_nvm_file', nvm_file] + cmd = [ + "export_to_nvm_file", + "-input_reconstruction_file", + recon_file + "-0", + "-output_nvm_file", + nvm_file, + ] run_cmd(cmd) - cmd = [import_map_path, - '-input_map', nvm_file, - '-output_map', args.output_map, - '-undistorted_images_list', undist_image_list, - '-distorted_images_list', args.image_list] + cmd = [ + import_map_path, + "-input_map", + nvm_file, + "-output_map", + args.output_map, + "-undistorted_images_list", + undist_image_list, + "-distorted_images_list", + args.image_list, + ] run_cmd(cmd) - + if not args.skip_rebuilding: - cmd = [build_map_path, - '-output_map', args.output_map, - '-rebuild', '-rebuild_refloat_cameras', - '-rebuild_detector', 'SURF', - '--min_valid_angle', '1.0' # to avoid features only seen in close-by images - ] + cmd = [ + build_map_path, + "-output_map", + args.output_map, + "-rebuild", + "-rebuild_refloat_cameras", + "-rebuild_detector", + "SURF", + "--min_valid_angle", + "1.0", # to avoid features only seen in close-by images + ] run_cmd(cmd) - From 262fa13afcbf56ab8bf333ce9ad2502bcdfb2dfc Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 10:33:31 -0800 Subject: [PATCH 20/29] Adjust a bit pre-commit.linter_python for readability --- scripts/git/pre-commit.linter_python | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/scripts/git/pre-commit.linter_python b/scripts/git/pre-commit.linter_python index e0565b8b05..a3d2123778 100755 --- a/scripts/git/pre-commit.linter_python +++ b/scripts/git/pre-commit.linter_python @@ -28,7 +28,7 @@ fi failed_lint=false -command -v black +command -v black > /dev/null 2>&1 ans=$? if [ "$ans" -ne "0" ]; then echo "'black' was not found. To install, check https://github.com/psf/black." @@ -36,7 +36,7 @@ if [ "$ans" -ne "0" ]; then exit 0 fi -command -v isort +command -v isort > /dev/null 2>&1 ans=$? if [ "$ans" -ne "0" ]; then echo "'isort' was not found. To install, check https://github.com/PyCQA/isort." @@ -44,22 +44,22 @@ if [ "$ans" -ne "0" ]; then fi echo "==================================================" -echo " Analysing with black" +echo " Analysing python code style with 'black'." # This check the files but they will not be commited if `black . --include ${files} --check --quiet`; then - echo "Linter black checks passed" + echo "Linter checks using 'black' passed." else - echo "Errors detected, fixing ..." - black . --include ${files} - failed_lint=true + echo "Errors detected with 'black'. Fixing them. Try to add and commit your files again." + black . --include ${files} + failed_lint=true fi echo "==================================================" -echo " Analysing with isort" +echo " Analysing python code style with 'isort'." if $(isort ${files} --extend-skip cmake --profile black --diff --check-only --quiet >/dev/null); then - echo "Linter isort checks passed" + echo "Linter checks using 'isort' passed." else - echo "Errors detected, fixing ..." + echo "Errors detected with 'isort'. Fixing them. Try to add and commit your files again." isort ${files} --extend-skip cmake --profile black >/dev/null failed_lint=true fi From 355d849b83dc34a81b566258daea991b3da50201 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 14:27:08 -0800 Subject: [PATCH 21/29] Refine the Theia flags --- localization/sparse_mapping/theia_flags.txt | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/localization/sparse_mapping/theia_flags.txt b/localization/sparse_mapping/theia_flags.txt index 6d0216ebf5..10689077e1 100644 --- a/localization/sparse_mapping/theia_flags.txt +++ b/localization/sparse_mapping/theia_flags.txt @@ -72,7 +72,7 @@ --shared_calibration=true # If set to true, only views with known calibration are reconstructed. ---only_calibrated_views=false +--only_calibrated_views=true ############### Global SfM Options ############### --global_position_estimator=LEAST_UNSQUARED_DEVIATION @@ -123,8 +123,7 @@ # Set this value to the determine the reprojection error in pixels at which # robustness begins (if a robust cost function is being used). -# TODO(oalexan1): This looks too high. In Astrobee we use 2.0. ---bundle_adjustment_robust_loss_width=10.0 +--bundle_adjustment_robust_loss_width=2.0 # Set this parameter to change which camera intrinsics should be # optimized. Valid options are NONE, ALL, FOCAL_LENGTH, PRINCIPAL_POINTS, @@ -134,7 +133,7 @@ # After BA, remove any points that have a reprojection error greater # than this. ---max_reprojection_error_pixels=8.0 +--max_reprojection_error_pixels=10.0 ############### Track Subsampling Options ############### @@ -161,8 +160,8 @@ --min_num_optimized_tracks_per_view=100 ############### Triangulation Options ############### ---min_triangulation_angle_degrees=0.1 ---triangulation_reprojection_error_pixels=15.0 +--min_triangulation_angle_degrees=1.0 +--triangulation_reprojection_error_pixels=10.0 --bundle_adjust_tracks=true ############### Logging Options ############### From 4f1f5e73d852862e28f6bad09206bd12c89e3f0e Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 15:04:18 -0800 Subject: [PATCH 22/29] Fixes to paths given the default catkin build setup --- localization/sparse_mapping/readme.md | 6 +++--- localization/sparse_mapping/theia_map.md | 18 +++++++++++------- .../sparse_mapping/tools/build_theia_map.py | 6 +++--- 3 files changed, 17 insertions(+), 13 deletions(-) diff --git a/localization/sparse_mapping/readme.md b/localization/sparse_mapping/readme.md index ec21929964..82af55f241 100644 --- a/localization/sparse_mapping/readme.md +++ b/localization/sparse_mapping/readme.md @@ -73,15 +73,15 @@ The bags created on the ISS are likely split into many smaller bags, for easy and reliability of transfer. Those can be merged into one bag as follows: - astrobee/build/devel/lib/localization_node/merge_bags \ + astrobee/devel/lib/localization_node/merge_bags \ -output_bag ### Extracting images To extract images from a bag file: - astrobee/build/devel/lib/localization_node/extract_image_bag \ - -use_timestamp_as_image_name \ + astrobee/devel/lib/localization_node/extract_image_bag \ + -use_timestamp_as_image_name \ -image_topic /hw/cam_nav -output_directory The above assumes that the software was built with ROS on. diff --git a/localization/sparse_mapping/theia_map.md b/localization/sparse_mapping/theia_map.md index 67909eeb65..065e81c93a 100644 --- a/localization/sparse_mapping/theia_map.md +++ b/localization/sparse_mapping/theia_map.md @@ -67,12 +67,16 @@ directory needs to be added to the PATH. # Run the Astrobee wrapper around the Theia tools -First set the environment. The following lines should be adjusted as needed, +The conda environment set up earlier will confuse the lookup the +depencencies for the Astrobee libraries. Hence the lines ``conda`` added +to one's ``.bashrc`` should be removed, the bash shell restarted, and +one should ensure that the ``env`` command has no mentions of conda. + +Set the environment. The following lines should be adjusted as needed, especially the robot name: export ASTROBEE_SOURCE_PATH=$HOME/projects/astrobee/src - export ASTROBEE_BUILD_PATH=$HOME/projects/astrobee/build - source $ASTROBEE_BUILD_PATH/devel/setup.bash + source $ASTROBEE_SOURCE_PATH/../devel/setup.bash export ASTROBEE_RESOURCE_DIR=$ASTROBEE_SOURCE_PATH/astrobee/resources export ASTROBEE_CONFIG_DIR=$ASTROBEE_SOURCE_PATH/astrobee/config export ASTROBEE_WORLD=iss @@ -109,7 +113,7 @@ reference only. An NVM map exported by Theia (or some other SfM tool) can be saved as an Astrobee sparse map with the command:: - astrobee/build/devel/lib/sparse_mapping/import_map \ + astrobee/devel/lib/sparse_mapping/import_map \ -undistorted_camera_params "wid_x wid_y focal_len opt_ctr_x opt_ctr_y" \ \ -input_map map.nvm -output_map map.map @@ -123,9 +127,9 @@ If desired to replace on importing the undistorted images with the original distorted ones, as it is usually expected of a sparse map, the above command should be called instead as:: - astrobee/build/devel/lib/sparse_mapping/import_map \ - -undistorted_images_list undist_list.txt \ - -distorted_images_list dist_list.txt \ + astrobee/devel/lib/sparse_mapping/import_map \ + -undistorted_images_list undist_list.txt \ + -distorted_images_list dist_list.txt \ -input_map map.nvm -output_map map.map Here, the files undist_list.txt and dist_list.txt must have one image diff --git a/localization/sparse_mapping/tools/build_theia_map.py b/localization/sparse_mapping/tools/build_theia_map.py index c16b24a433..fa88a8ac14 100644 --- a/localization/sparse_mapping/tools/build_theia_map.py +++ b/localization/sparse_mapping/tools/build_theia_map.py @@ -115,12 +115,12 @@ def process_args(args): os.path.dirname(os.path.dirname(os.path.dirname(src_path))) ) undistort_image_path = os.path.join( - exec_path, "build/devel/lib/camera/undistort_image" + exec_path, "devel/lib/camera/undistort_image" ) import_map_path = os.path.join( - exec_path, "build/devel/lib/sparse_mapping/import_map" + exec_path, "devel/lib/sparse_mapping/import_map" ) - build_map_path = os.path.join(exec_path, "build/devel/lib/sparse_mapping/build_map") + build_map_path = os.path.join(exec_path, "devel/lib/sparse_mapping/build_map") parser = argparse.ArgumentParser(description="Parameters for the geometry mapper.") parser.add_argument( From 6215e0896e155e8c9d1ab681b62a65cb33a513ed Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 14:47:23 -0800 Subject: [PATCH 23/29] Require OpenCV 3.3.1 --- cmake/FindOpenCV331.cmake | 2 +- localization/handrail_detect/CMakeLists.txt | 2 +- localization/vive_localization/CMakeLists.txt | 2 +- management/image_sampler/CMakeLists.txt | 2 +- tools/calibration/CMakeLists.txt | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cmake/FindOpenCV331.cmake b/cmake/FindOpenCV331.cmake index 143cd9a47d..02b4a6fcbd 100644 --- a/cmake/FindOpenCV331.cmake +++ b/cmake/FindOpenCV331.cmake @@ -16,7 +16,7 @@ # under the License. # Find OpenCV and fix a 3.3.1 bug -find_package(OpenCV 3 REQUIRED) +find_package(OpenCV 3.3.1 REQUIRED) if (USE_CTC) if (${OpenCV_VERSION} MATCHES "3.3.1") foreach(__cvcomponent ${OpenCV_LIB_COMPONENTS}) diff --git a/localization/handrail_detect/CMakeLists.txt b/localization/handrail_detect/CMakeLists.txt index c268d88d81..36f2706e3c 100644 --- a/localization/handrail_detect/CMakeLists.txt +++ b/localization/handrail_detect/CMakeLists.txt @@ -32,7 +32,7 @@ find_package(catkin2 REQUIRED COMPONENTS ) # Find OpenCV3 -find_package(OpenCV 3 REQUIRED) +find_package(OpenCV 3.3.1 REQUIRED) catkin_package( LIBRARIES ${PROJECT_NAME} diff --git a/localization/vive_localization/CMakeLists.txt b/localization/vive_localization/CMakeLists.txt index e8d8bfdf1c..eb85937dd4 100644 --- a/localization/vive_localization/CMakeLists.txt +++ b/localization/vive_localization/CMakeLists.txt @@ -38,7 +38,7 @@ find_package(Eigen3 REQUIRED) # Find OpenCV3 SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") -find_package(OpenCV 3 REQUIRED) +find_package(OpenCV 3.3.1 REQUIRED) # Non-linear optimizer find_package(Ceres REQUIRED) diff --git a/management/image_sampler/CMakeLists.txt b/management/image_sampler/CMakeLists.txt index 546317542b..a2088b55fd 100644 --- a/management/image_sampler/CMakeLists.txt +++ b/management/image_sampler/CMakeLists.txt @@ -32,7 +32,7 @@ find_package(catkin2 REQUIRED COMPONENTS ) # Find OpenCV3 -find_package(OpenCV 3 REQUIRED) +find_package(OpenCV 3.3.1 REQUIRED) catkin_package( LIBRARIES image_sampler diff --git a/tools/calibration/CMakeLists.txt b/tools/calibration/CMakeLists.txt index 5327388230..dd48d3a5e9 100644 --- a/tools/calibration/CMakeLists.txt +++ b/tools/calibration/CMakeLists.txt @@ -31,7 +31,7 @@ find_package(catkin2 REQUIRED COMPONENTS # Find OpenCV3 SET(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") -find_package(OpenCV 3 REQUIRED) +find_package(OpenCV 3.3.1 REQUIRED) find_package(Eigen3 REQUIRED) From c5da920d8a4f50c8163c2b4c2eaa5d7f292ebd31 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 15:13:00 -0800 Subject: [PATCH 24/29] Fix python style errors --- localization/sparse_mapping/tools/build_theia_map.py | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/localization/sparse_mapping/tools/build_theia_map.py b/localization/sparse_mapping/tools/build_theia_map.py index fa88a8ac14..6b73e29dff 100644 --- a/localization/sparse_mapping/tools/build_theia_map.py +++ b/localization/sparse_mapping/tools/build_theia_map.py @@ -114,12 +114,8 @@ def process_args(args): exec_path = os.path.dirname( os.path.dirname(os.path.dirname(os.path.dirname(src_path))) ) - undistort_image_path = os.path.join( - exec_path, "devel/lib/camera/undistort_image" - ) - import_map_path = os.path.join( - exec_path, "devel/lib/sparse_mapping/import_map" - ) + undistort_image_path = os.path.join(exec_path, "devel/lib/camera/undistort_image") + import_map_path = os.path.join(exec_path, "devel/lib/sparse_mapping/import_map") build_map_path = os.path.join(exec_path, "devel/lib/sparse_mapping/build_map") parser = argparse.ArgumentParser(description="Parameters for the geometry mapper.") From 8ca0e6c70618bd6d90783d9abfa76e35cdd97137 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 16:09:34 -0800 Subject: [PATCH 25/29] granite control points need not be in an executable file --- localization/sparse_mapping/granite_xyz_controlPoints.txt | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100755 => 100644 localization/sparse_mapping/granite_xyz_controlPoints.txt diff --git a/localization/sparse_mapping/granite_xyz_controlPoints.txt b/localization/sparse_mapping/granite_xyz_controlPoints.txt old mode 100755 new mode 100644 From 2fbf7894dfec5991db2798a2eda69de6c5807e3e Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 16:11:11 -0800 Subject: [PATCH 26/29] theia_map: Allow importing an undistorted map --- localization/sparse_mapping/theia_map.md | 5 ++- .../sparse_mapping/tools/build_theia_map.py | 45 ++++++++++++++----- 2 files changed, 39 insertions(+), 11 deletions(-) diff --git a/localization/sparse_mapping/theia_map.md b/localization/sparse_mapping/theia_map.md index 065e81c93a..0817d4df25 100644 --- a/localization/sparse_mapping/theia_map.md +++ b/localization/sparse_mapping/theia_map.md @@ -68,7 +68,7 @@ directory needs to be added to the PATH. # Run the Astrobee wrapper around the Theia tools The conda environment set up earlier will confuse the lookup the -depencencies for the Astrobee libraries. Hence the lines ``conda`` added +dependencies for the Astrobee libraries. Hence the lines ``conda`` added to one's ``.bashrc`` should be removed, the bash shell restarted, and one should ensure that the ``env`` command has no mentions of conda. @@ -101,6 +101,9 @@ This tool has the following command-line options: Theia. --work_dir: A temporary work directory to be deleted by the user later. + --keep_undistorted_images: Do not replace the undistorted images + Theia used with the original distorted ones in the sparse map + imported from Theia. This is for testing purposes. --help: Show this help message and exit. # Auxiliary import_map tool diff --git a/localization/sparse_mapping/tools/build_theia_map.py b/localization/sparse_mapping/tools/build_theia_map.py index 6b73e29dff..388e33cc45 100644 --- a/localization/sparse_mapping/tools/build_theia_map.py +++ b/localization/sparse_mapping/tools/build_theia_map.py @@ -103,12 +103,17 @@ def sanity_checks(undistort_image_path, import_map_path, build_map_path, args): if which("export_to_nvm_file") is None: raise Exception("Cannot find the 'export_to_nvm_file' program in PATH.") - + if args.keep_undistorted_images and (not args.skip_rebuilding): + raise Exception("Cannot rebuild the map if it has undistorted images.") + def process_args(args): """ Set up the parser and parse the args. """ + # Number of arguments before starting to parse them + num_input_args = len(sys.argv) + # Extract some paths before the args are parsed src_path = os.path.dirname(args[0]) exec_path = os.path.dirname( @@ -118,7 +123,7 @@ def process_args(args): import_map_path = os.path.join(exec_path, "devel/lib/sparse_mapping/import_map") build_map_path = os.path.join(exec_path, "devel/lib/sparse_mapping/build_map") - parser = argparse.ArgumentParser(description="Parameters for the geometry mapper.") + parser = argparse.ArgumentParser(description="") parser.add_argument( "--theia_flags", dest="theia_flags", @@ -149,8 +154,21 @@ def process_args(args): help="A temporary work directory to be deleted by the user later.", ) + parser.add_argument( + "--keep_undistorted_images", + dest="keep_undistorted_images", + action="store_true", + help="Do not replace the undistorted images Theia used with the original " + + "distorted ones in the sparse map imported from Theia. This is for testing " + + "purposes.", + ) args = parser.parse_args() + # Print the help message if called with no arguments + if num_input_args <= 1: + parser.print_help() + sys.exit(1) + if args.theia_flags == "": args.theia_flags = os.path.dirname(src_path) + "/theia_flags.txt" @@ -207,7 +225,7 @@ def gen_undist_image_list(work_dir, dist_image_list): def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): calib_file = work_dir + "/" + "theia_calibration.json" - + intrinsics_str = "" # Parse the intrinsics with open(undist_intrinsics_file, "r") as fh: for line in fh: @@ -216,6 +234,7 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): if line[0] == "#": continue + intrinsics_str = line.rstrip() # will need this later intrinsics = line.split() if len(intrinsics) < 5: raise Exception( @@ -256,8 +275,7 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): fh.write("]\n") fh.write("}\n") - return calib_file - + return calib_file, intrinsics_str if __name__ == "__main__": @@ -289,7 +307,7 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): ] run_cmd(cmd) - calib_file = gen_theia_calib_file( + (calib_file, intrinsics_str) = gen_theia_calib_file( args.work_dir, undist_images, undist_intrinsics_file ) recon_file = args.work_dir + "/run" @@ -341,10 +359,17 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): "-output_map", args.output_map, "-undistorted_images_list", - undist_image_list, - "-distorted_images_list", - args.image_list, - ] + undist_image_list] + + if not args.keep_undistorted_images: + cmd += ["-distorted_images_list", + args.image_list, + ] + else: + cmd += ["-undistorted_camera_params", + intrinsics_str, + ] + run_cmd(cmd) if not args.skip_rebuilding: From 5000bb5fb97255f0c934232b71448cd3441330b7 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 16:12:20 -0800 Subject: [PATCH 27/29] Fixes for python style --- .../sparse_mapping/tools/build_theia_map.py | 33 +++++++++++-------- 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/localization/sparse_mapping/tools/build_theia_map.py b/localization/sparse_mapping/tools/build_theia_map.py index 388e33cc45..d5c1c81bb4 100644 --- a/localization/sparse_mapping/tools/build_theia_map.py +++ b/localization/sparse_mapping/tools/build_theia_map.py @@ -105,7 +105,8 @@ def sanity_checks(undistort_image_path, import_map_path, build_map_path, args): if args.keep_undistorted_images and (not args.skip_rebuilding): raise Exception("Cannot rebuild the map if it has undistorted images.") - + + def process_args(args): """ Set up the parser and parse the args. @@ -113,7 +114,7 @@ def process_args(args): # Number of arguments before starting to parse them num_input_args = len(sys.argv) - + # Extract some paths before the args are parsed src_path = os.path.dirname(args[0]) exec_path = os.path.dirname( @@ -158,9 +159,9 @@ def process_args(args): "--keep_undistorted_images", dest="keep_undistorted_images", action="store_true", - help="Do not replace the undistorted images Theia used with the original " + - "distorted ones in the sparse map imported from Theia. This is for testing " + - "purposes.", + help="Do not replace the undistorted images Theia used with the original " + + "distorted ones in the sparse map imported from Theia. This is for testing " + + "purposes.", ) args = parser.parse_args() @@ -234,7 +235,7 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): if line[0] == "#": continue - intrinsics_str = line.rstrip() # will need this later + intrinsics_str = line.rstrip() # will need this later intrinsics = line.split() if len(intrinsics) < 5: raise Exception( @@ -277,6 +278,7 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): return calib_file, intrinsics_str + if __name__ == "__main__": ( @@ -359,17 +361,20 @@ def gen_theia_calib_file(work_dir, undist_images, undist_intrinsics_file): "-output_map", args.output_map, "-undistorted_images_list", - undist_image_list] - + undist_image_list, + ] + if not args.keep_undistorted_images: - cmd += ["-distorted_images_list", + cmd += [ + "-distorted_images_list", args.image_list, - ] + ] else: - cmd += ["-undistorted_camera_params", - intrinsics_str, - ] - + cmd += [ + "-undistorted_camera_params", + intrinsics_str, + ] + run_cmd(cmd) if not args.skip_rebuilding: From af17571aaf8c16eebe6d05a4330b29494c025326 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Tue, 30 Nov 2021 16:16:56 -0800 Subject: [PATCH 28/29] One more fix for the catkin devel/setup.bash location --- localization/sparse_mapping/build_map.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/localization/sparse_mapping/build_map.md b/localization/sparse_mapping/build_map.md index 77610a4b17..8f85d0ad5c 100644 --- a/localization/sparse_mapping/build_map.md +++ b/localization/sparse_mapping/build_map.md @@ -362,9 +362,13 @@ be visualized in the ISS as follows: Open two terminals, and in each one type: - export BUILD_PATH=$HOME/astrobee/build + export BUILD_PATH=$HOME/astrobee source $BUILD_PATH/devel/setup.bash +The Astrobee directory above must have ``src`` and ``devel`` +subdirectories, and needs to be adjusted given its location on your +disk. + In the first terminal start the simulator: roslaunch astrobee sim.launch speed:=0.75 rviz:=true From 2de0d3d18c3eef6328eb351839bb07d289b03f23 Mon Sep 17 00:00:00 2001 From: Oleg Alexandrov Date: Fri, 3 Dec 2021 10:58:25 -0800 Subject: [PATCH 29/29] Set up all needed paths before running the mapping tools --- README.md | 2 +- localization/sparse_mapping/build_map.md | 113 +++++++++++++---------- localization/sparse_mapping/readme.md | 27 ++++-- localization/sparse_mapping/theia_map.md | 5 +- 4 files changed, 90 insertions(+), 57 deletions(-) diff --git a/README.md b/README.md index 1fab0582c5..747c431e9b 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,7 @@ signaling, and sound. The flight software is hosted on each Astrobee's three int uses the open-source [Robot Operating System (ROS)](https://www.ros.org/) framework as message-passing middleware. It provides a high-level [Astrobee Command API](https://nasa.github.io/astrobee/html/command_dictionary.html) for controlling the robot and has multiple operating modes. It can execute a plan (command sequence), individual operator commands (teleoperation), or commands from guest science code running onboard Astrobee. -The Astrobee Robot Software simulator, built using [ROS](https://www.ros.org/) and [Gazebo](http://gazebosim.org/), enables the flight software to be evaluated without the need for robot hardware. The supporting tools include a tool that processes ISS imagery to [build maps for Astrobee localization](https://github.com/nasa/astrobee/tree/master/localization/sparse_mapping), along with many others. +The Astrobee Robot Software simulator, built using [ROS](https://www.ros.org/) and [Gazebo](http://gazebosim.org/), enables the flight software to be evaluated without the need for robot hardware. The supporting tools include a tool that processes ISS imagery to [build maps for Astrobee localization](https://nasa.github.io/astrobee/html/sparsemapping.html), along with many others. Released separately, the Astrobee ground data system (GDS) includes Astrobee control station software that communicates with Astrobee flight software via the Data Distribution Service (DDS) network protocol over the ISS Ku-IP space-to-ground link. The control station is written primarily in Java using the Eclipse RCP framework. Source code is in the [`astrobee_gds`](https://github.com/nasa/astrobee_gds) repository, or you can download the [binary release](https://software.nasa.gov/software/ARC-17994-1B). diff --git a/localization/sparse_mapping/build_map.md b/localization/sparse_mapping/build_map.md index 8f85d0ad5c..8a774e8fa3 100644 --- a/localization/sparse_mapping/build_map.md +++ b/localization/sparse_mapping/build_map.md @@ -4,9 +4,9 @@ Here we describe how to build a map. ## Summary -1. Reduce the number of images. +1. Set up the environment. -2. Set up the environment. +2. Reduce the number of images. 3. Build the map. @@ -16,6 +16,48 @@ Here we describe how to build a map. # Detailed explanation +## Setup the environment + +In the first step, one needs to set some environmental variables, as +follows: + + export ASTROBEE_SOURCE_PATH=$HOME/astrobee/src + export ASTROBEE_BUILD_PATH=$HOME/astrobee + export ASTROBEE_RESOURCE_DIR=$ASTROBEE_SOURCE_PATH/astrobee/resources + export ASTROBEE_CONFIG_DIR=$ASTROBEE_SOURCE_PATH/astrobee/config + export ASTROBEE_ROBOT=p4d + export ASTROBEE_WORLD=granite + +The source and build paths need to be adjusted for your particular +setup. + +Also consider setting: + + export PATH=$ASTROBEE_BUILD_PATH/devel/lib/sparse_mapping:$PATH + +to have the ``build_map`` and other related tools in your path. + +Above, ``p4d`` is the robot being used to take pictures, and the world +is the granite table. These may need to change, depending on your +goals. + +Under the hood, the following configuration files will be read: + + $ASTROBEE_CONFIG_DIR/cameras.config + +which contains the image width and height (the camera we use is +the nav cam) and + + $ASTROBEE_CONFIG_DIR/robots/$ASTROBEE_ROBOT.config + +having nav cam's intrinsics. If your camera is not the nav cam on p4d, +and none of the other available config files apply, you can just +temporarily modify the above files to reflect your camera's parameters +(without checking in your changes). + +More details on these and other environmental variables can be found +in the \subpage astrobee configuration documentation. + ## Reduce the number of images Here, we delete the images that overlap highly. (This tool is, like all @@ -55,34 +97,6 @@ images, as then the map could be of poor quality. Hence, the robot should have some translation motion (in addition to any rotation) when the data is acquired. -## Setup the environment - -In the first step, one needs to set some environmental variables, as -follows: - - export ASTROBEE_RESOURCE_DIR=$SOURCE_PATH/astrobee/resources - export ASTROBEE_CONFIG_DIR=$SOURCE_PATH/astrobee/config - export ASTROBEE_ROBOT=p4d - export ASTROBEE_WORLD=granite - -Here, `p4d` is the robot being used to take pictures, and the world is -the granite table. These may need to change, depending on your -goals. Under the hood, the following configuration files will be read: - - $ASTROBEE_CONFIG_DIR/cameras.config - -which contains the image width and height (the camera we use is -the nav cam) and - - $ASTROBEE_CONFIG_DIR/robots/$ASTROBEE_ROBOT.config - -having nav cam's intrinsics. If your camera is not the nav cam on p4d, -and none of the other available config files apply, you can just -temporarily modify the above files to reflect your camera's parameters -(without checking in your changes). - -More details on these and other environmental variables can be found -in the \subpage astrobee configuration documentation. ## Building a map @@ -156,11 +170,12 @@ consistently across multiple frames. build_map -bundle_adjustment -histogram_equalization - Adjust the initial transformations to minimize error with bundle adjustment. +Adjust the initial transformations to minimize error with bundle +adjustment. -If the options: +If the options: - -first_ba_index and -last_ba_index + -first_ba_index and -last_ba_index are specified, only cameras with indices between these (including both endpoints) will be optimized during bundle adjustment. @@ -362,8 +377,8 @@ be visualized in the ISS as follows: Open two terminals, and in each one type: - export BUILD_PATH=$HOME/astrobee - source $BUILD_PATH/devel/setup.bash + export ASTROBEE_BUILD_PATH=$HOME/astrobee + source $ASTROBEE_BUILD_PATH/devel/setup.bash The Astrobee directory above must have ``src`` and ``devel`` subdirectories, and needs to be adjusted given its location on your @@ -375,8 +390,8 @@ In the first terminal start the simulator: In the second, run: - python $SOURCE_PATH/localization/sparse_mapping/tools/view_control_points.py \ - $SOURCE_PATH/localization/sparse_mapping/iss_registration.txt + python $ASTROBEE_SOURCE_PATH/localization/sparse_mapping/tools/view_control_points.py \ + $ASTROBEE_SOURCE_PATH/localization/sparse_mapping/iss_registration.txt Go back to the simulated ISS and examine the registration points. If the Rviz display looks too cluttered, most topics can be turned off. @@ -412,9 +427,9 @@ Python command will refresh them. ### Registration in the MGTF A set of 10 registration points were measured in the MGTF with the -Total Station. They are in the file +\subpage total_station. They are in the file: - $SOURCE_PATH/localization/sparse_mapping/mgtf_registration.txt + $ASTROBEE_SOURCE_PATH/localization/sparse_mapping/mgtf_registration.txt Two of these are on the back wall, and the rest are on the metal columns on the side walls, with four on each wall. Half of the points @@ -430,7 +445,8 @@ be needed to identify them. A registered and bundle-adjusted map can be used to study how well it predicts the computed 3D locations for an independently acquired set of control points and 3D measurements. These are in the same format as -for registration. The map is not modified in any way during this step. +for registration. The map is not modified in any way during this step, +The command is: build_map -verification @@ -462,7 +478,7 @@ To test how the map may perform on the robot, do the following: ### Stage the feature counter utility (should be added to the install at one point): - scp $SOURCE_PATH/localization/marker_tracking/ros/tools/features_counter.py mlp: + scp $ASTROBEE_SOURCE_PATH/localization/marker_tracking/ros/tools/features_counter.py mlp: ### Launch the localization node on LLP @@ -477,7 +493,8 @@ this robot will give wrong results for other users. Then launch localization: ssh llp - roslaunch astrobee astrobee.launch llp:=disabled mlp:=mlp nodes:=framestore,dds_ros_bridge,localization_node + roslaunch astrobee astrobee.launch llp:=disabled mlp:=mlp \ + nodes:=framestore,dds_ros_bridge,localization_node ### Enable localization and the mapped landmark production (on MLP) @@ -532,9 +549,9 @@ computations may differ. Set up the environment in every terminal that is used. Ensure that you use the correct robot name below. - source $BUILD_PATH/devel/setup.bash - export ASTROBEE_RESOURCE_DIR=$SOURCE_PATH/astrobee/resources - export ASTROBEE_CONFIG_DIR=$SOURCE_PATH/astrobee/config + source $ASTROBEE_BUILD_PATH/devel/setup.bash + export ASTROBEE_RESOURCE_DIR=$ASTROBEE_SOURCE_PATH/astrobee/resources + export ASTROBEE_CONFIG_DIR=$ASTROBEE_SOURCE_PATH/astrobee/config export ASTROBEE_WORLD=iss export ASTROBEE_ROBOT=bumble # your robot's name may be different export ROS_MASTER_URI=http://127.0.0.1:11311/ @@ -545,9 +562,9 @@ Examine the localization configuration file: Sym link the map to test: - mkdir -p $SOURCE_PATH/astrobee/resources/maps - rm -fv $SOURCE_PATH/astrobee/resources/maps/iss.map - ln -s $(pwd)/mymap.map $SOURCE_PATH/astrobee/resources/maps/iss.map + mkdir -p $ASTROBEE_SOURCE_PATH/astrobee/resources/maps + rm -fv $ASTROBEE_SOURCE_PATH/astrobee/resources/maps/iss.map + ln -s $(pwd)/mymap.map $ASTROBEE_SOURCE_PATH/astrobee/resources/maps/iss.map Start the localization node: diff --git a/localization/sparse_mapping/readme.md b/localization/sparse_mapping/readme.md index 82af55f241..10164f5c98 100644 --- a/localization/sparse_mapping/readme.md +++ b/localization/sparse_mapping/readme.md @@ -14,8 +14,8 @@ Maps are stored as protobuf files. ## ROS node -The ROS node takes images and a map as input, and outputs visual features -detected in the image and their 3D coordinates. +The ROS node takes images and a map as input, and outputs visual +features detected in the image and their 3D coordinates. ### Inputs @@ -28,6 +28,21 @@ detected in the image and their 3D coordinates. * `/localization/mapped_landmarks/features` * `/localization/mapped_landmarks/registration` +## The environment + +It will be convenient in this document to set these environmental +variables, pointing to the source and build directories. They may need +to be adjusted given your setup. + + export ASTROBEE_SOURCE_PATH=$HOME/astrobee/src + export ASTROBEE_BUILD_PATH=$HOME/astrobee + +To access many of the localization tools, such as ``build_map``, +``merge_maps``, etc., without specifying a full path, also consider +setting: + + export PATH=$ASTROBEE_BUILD_PATH/devel/lib/sparse_mapping:$PATH + ## Tools and procedures ### Record a bag @@ -73,15 +88,15 @@ The bags created on the ISS are likely split into many smaller bags, for easy and reliability of transfer. Those can be merged into one bag as follows: - astrobee/devel/lib/localization_node/merge_bags \ + $ASTROBEE_BUILD_PATH/devel/lib/localization_node/merge_bags \ -output_bag ### Extracting images To extract images from a bag file: - astrobee/devel/lib/localization_node/extract_image_bag \ - -use_timestamp_as_image_name \ + $ASTROBEE_BUILD_PATH/devel/lib/localization_node/extract_image_bag \ + -use_timestamp_as_image_name \ -image_topic /hw/cam_nav -output_directory The above assumes that the software was built with ROS on. @@ -190,7 +205,7 @@ This functionality is implemented in the localize_cams tool. Usage: Here we use values that are different from - astrobee/config/localization.config + $ASTROBEE_SOURCE_PATH/astrobee/config/localization.config which are used for localization on the robot, since those are optimized for speed and here we want more accuracy. diff --git a/localization/sparse_mapping/theia_map.md b/localization/sparse_mapping/theia_map.md index 0817d4df25..181b16f308 100644 --- a/localization/sparse_mapping/theia_map.md +++ b/localization/sparse_mapping/theia_map.md @@ -110,7 +110,7 @@ This tool has the following command-line options: This tool is used to import a map from the NVM format, which Theia exports to. These operations are done automatically by the -``build_theia_map.py'' tool. This documentation is provided for +``build_theia_map.py`` tool. This documentation is provided for reference only. An NVM map exported by Theia (or some other SfM tool) can be saved as @@ -124,7 +124,8 @@ an Astrobee sparse map with the command:: This assumes that the images were acquired with the nav camera of the robot given by $ASTROBEE_ROBOT and undistorted with the Astrobee program ``undistort_image``. The undistorted camera parameters to use -should be as printed on the screen by ``undistort_image``. +should be as printed on the screen (and saved to disk) by +``undistort_image``. If desired to replace on importing the undistorted images with the original distorted ones, as it is usually expected of a sparse map,