diff --git a/astrobee/config/localization.config b/astrobee/config/localization.config index 2f02d6bf12..ab46b78f90 100644 --- a/astrobee/config/localization.config +++ b/astrobee/config/localization.config @@ -15,17 +15,102 @@ -- License for the specific language governing permissions and limitations -- under the License. -num_similar = 20 -ransac_inlier_tolerance = 5 -ransac_iterations = 100 -early_break_landmarks = 100 -histogram_equalization = 1 -min_features = 200 -max_features = 800 -detection_retries = 1 -num_threads = 1 -min_brisk_threshold = 20.0 -default_brisk_threshold = 90.0 -max_brisk_threshold = 110.0 matched_features_on = false all_features_on = false +num_threads = 1 +verbose_localization = false +visualize_localization_matches = false + +-- BRISK +brisk_num_similar = 20 +brisk_min_query_score_ratio = 0 +brisk_ransac_inlier_tolerance = 5 +brisk_num_ransac_iterations = 100 +brisk_early_break_landmarks = 100 +brisk_histogram_equalization = 1 +brisk_check_essential_matrix = false +brisk_essential_ransac_iterations = 2000 +brisk_add_similar_images = false +brisk_add_best_previous_image = false +brisk_hamming_distance = 90 +brisk_goodness_ratio = 0.8 +brisk_use_clahe = false +brisk_num_extra_localization_db_images = 0 +-- Detection Params +brisk_min_threshold = 20.0 +brisk_default_threshold = 90.0 +brisk_max_threshold = 110.0 +brisk_min_features = 200 +brisk_max_features = 800 +brisk_detection_retries = 1 +-- Localizer Threshold Params +brisk_success_history_size = 10 +brisk_min_success_rate = 0 +brisk_max_success_rate = 1 +brisk_adjust_hamming_distance = false +brisk_min_hamming_distance = 90 +brisk_max_hamming_distance = 90 + +-- TEBLID512 +teblid512_num_similar = 20 +teblid512_min_query_score_ratio = 0.45 +teblid512_ransac_inlier_tolerance = 3 +teblid512_num_ransac_iterations = 1000 +teblid512_early_break_landmarks = 100 +teblid512_histogram_equalization = 3 +teblid512_check_essential_matrix = true +teblid512_essential_ransac_iterations = 2000 +teblid512_add_similar_images = true +teblid512_add_best_previous_image = true +teblid512_hamming_distance = 85 +teblid512_goodness_ratio = 0.8 +teblid512_use_clahe = true +teblid512_num_extra_localization_db_images = 0 + +-- Detection Params +teblid512_min_threshold = 20.0 +teblid512_default_threshold = 60.0 +teblid512_max_threshold = 110.0 +teblid512_min_features = 1000 +teblid512_max_features = 3000 +teblid512_detection_retries = 1 + +-- Localizer Threshold Params +teblid512_success_history_size = 10 +teblid512_min_success_rate = 0.3 +teblid512_max_success_rate = 0.9 +teblid512_adjust_hamming_distance = false +teblid512_min_hamming_distance = 85 +teblid512_max_hamming_distance = 85 + +-- TEBLID256 +teblid256_num_similar = 20 +teblid256_min_query_score_ratio = 0.45 +teblid256_ransac_inlier_tolerance = 3 +teblid256_num_ransac_iterations = 1000 +teblid256_early_break_landmarks = 100 +teblid256_histogram_equalization = 3 +teblid256_check_essential_matrix = true +teblid256_essential_ransac_iterations = 2000 +teblid256_add_similar_images = true +teblid256_add_best_previous_image = true +teblid256_hamming_distance = 85 +teblid256_goodness_ratio = 0.8 +teblid256_use_clahe = true +teblid256_num_extra_localization_db_images = 0 + +-- Detection Params +teblid256_min_threshold = 20.0 +teblid256_default_threshold = 60.0 +teblid256_max_threshold = 110.0 +teblid256_min_features = 1000 +teblid256_max_features = 3000 +teblid256_detection_retries = 1 + +-- Localizer Threshold Params +teblid256_success_history_size = 10 +teblid256_min_success_rate = 0.3 +teblid256_max_success_rate = 0.9 +teblid256_adjust_hamming_distance = false +teblid256_min_hamming_distance = 85 +teblid256_max_hamming_distance = 85 diff --git a/localization/interest_point/CMakeLists.txt b/localization/interest_point/CMakeLists.txt index 8a3cbd43bd..acdf0dd57f 100644 --- a/localization/interest_point/CMakeLists.txt +++ b/localization/interest_point/CMakeLists.txt @@ -61,6 +61,7 @@ add_library(interest_point src/brisk.cc src/essential.cc src/matching.cc + src/BAD.cpp ) add_dependencies(interest_point ${catkin_EXPORTED_TARGETS}) target_link_libraries(interest_point ${OPENMVG_LIBRARIES} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) diff --git a/localization/interest_point/include/interest_point/BAD.h b/localization/interest_point/include/interest_point/BAD.h new file mode 100644 index 0000000000..8cf3ae96aa --- /dev/null +++ b/localization/interest_point/include/interest_point/BAD.h @@ -0,0 +1,104 @@ +/** + * @copyright 2021 Xoan Iago Suarez Canosa. All rights reserved. + * Constact: iago.suarez@thegraffter.com + * Software developed in the PhD: Low-level vision for resource-limited devices + * Repo link: https://github.com/iago-suarez/efficient-descriptors + * License: Apache 2 + * TODO(rsoussan): Remove this and BAD.cpp and switch to opencv TEBLID if opencv + * version upgraded. + */ + +#ifndef INTEREST_POINT_BAD_H_ +#define INTEREST_POINT_BAD_H_ + +#include + +#include +#include + +namespace upm { + +/** + * Implementation of the Box Average Difference (BAD) descriptor. The method uses features + * computed from the difference of the average gray values of two boxes in the patch. + * + * Each pair of boxes is represented with a BoxPairParams struct. After obtaining the feature + * from them, the i'th feature is thresholded with thresholds_[i], producing the binary + * weak-descriptor. + */ +class BAD : public cv::Feature2D { + public: + /** + * @brief Descriptor number of bits, each bit is a weak-descriptor. + * The user can choose between 512 or 256 bits. + */ + enum BadSize { + SIZE_512_BITS = 100, SIZE_256_BITS = 101, + }; + + /** + * @param scale_factor Adjust the sampling window around detected keypoints: + - 1.00f should be the scale for ORB keypoints + - 6.75f should be the scale for SIFT detected keypoints + - 6.25f is default and fits for KAZE, SURF detected keypoints + - 5.00f should be the scale for AKAZE, MSD, AGAST, FAST, BRISK keypoints + * @param n_bits Determine the number of bits in the descriptor. Should be either + BAD::SIZE_512_BITS or BAD::SIZE_256_BITS. + */ + explicit BAD(float scale_factor = 1.0f, BadSize n_bits = SIZE_512_BITS); + + /** + * @brief Creates the BAD descriptor. + * @param scale_factor Adjust the sampling window around detected keypoints: + - 1.00f should be the scale for ORB keypoints + - 6.75f should be the scale for SIFT detected keypoints + - 6.25f is default and fits for KAZE, SURF detected keypoints + - 5.00f should be the scale for AKAZE, MSD, AGAST, FAST, BRISK keypoints + * @param n_bits + * @return + */ + static cv::Ptr create(float scale_factor = 1.0f, BadSize n_bits = SIZE_512_BITS) { + return cv::makePtr(scale_factor, n_bits); + } + + /** @brief Computes the descriptors for a set of keypoints detected in an image (first variant) or image set + (second variant). + + @param image Image. + @param keypoints Input collection of keypoints. Keypoints for which a descriptor cannot be + computed are removed. Sometimes new keypoints can be added, for example: SIFT duplicates keypoint + with several dominant orientations (for each orientation). + @param descriptors Computed descriptors. In the second variant of the method descriptors[i] are + descriptors computed for a keypoints[i]. Row j is the keypoints (or keypoints[i]) is the + descriptor for keypoint j-th keypoint. + */ + void compute(cv::InputArray image, + CV_OUT CV_IN_OUT std::vector &keypoints, + cv::OutputArray descriptors) override; + + int descriptorSize() const override { return box_params_.size() / 8; } + int descriptorType() const override { return CV_8UC1; } + int defaultNorm() const override { return cv::NORM_HAMMING; } + bool empty() const override { return false; } + + cv::String getDefaultName() const override { return std::string("BAD") + std::to_string(box_params_.size()); } + + // Struct representing a pair of boxes in the patch + struct BoxPairParams { + int x1, x2, y1, y2, boxRadius; + }; + + protected: + // Computes the BADdescriptor + void computeBAD(const cv::Mat &integral_img, + const std::vector &keypoints, + cv::Mat &descriptors); + + std::vector thresholds_; + std::vector box_params_; + float scale_factor_ = 1; + cv::Size patch_size_ = {32, 32}; +}; + +} // namespace upm +#endif // INTEREST_POINT_BAD_H_ diff --git a/localization/interest_point/include/interest_point/essential.h b/localization/interest_point/include/interest_point/essential.h index 0d86d4990c..89d37ae71a 100644 --- a/localization/interest_point/include/interest_point/essential.h +++ b/localization/interest_point/include/interest_point/essential.h @@ -27,22 +27,17 @@ namespace interest_point { // Performs a robust, ransac, solving for the essential matrix // between interest point measurements in x1 and x2. - bool RobustEssential(Eigen::Matrix3d const& k1, Eigen::Matrix3d const& k2, - Eigen::Matrix2Xd const& x1, Eigen::Matrix2Xd const& x2, - Eigen::Matrix3d * e, - std::vector * vec_inliers, - std::pair const& size1, - std::pair const& size2, - double * error_max, - double precision); +bool RobustEssential(Eigen::Matrix3d const& k1, Eigen::Matrix3d const& k2, Eigen::Matrix2Xd const& x1, + Eigen::Matrix2Xd const& x2, Eigen::Matrix3d* e, std::vector* vec_inliers, + std::pair const& size1, std::pair const& size2, double* error_max, + double precision, const int ransac_iterations = 4096); - // Solves for the RT (Rotation and Translation) from the essential - // matrix and x1 and x2. There are 4 possible, and this returns the - // best of the 4 solutions. - bool EstimateRTFromE(Eigen::Matrix3d const& k1, Eigen::Matrix3d const& k2, - Eigen::Matrix2Xd const& x1, Eigen::Matrix2Xd const& x2, - Eigen::Matrix3d const& e, std::vector const& vec_inliers, - Eigen::Matrix3d * r, Eigen::Vector3d * t); +// Solves for the RT (Rotation and Translation) from the essential +// matrix and x1 and x2. There are 4 possible, and this returns the +// best of the 4 solutions. +bool EstimateRTFromE(Eigen::Matrix3d const& k1, Eigen::Matrix3d const& k2, Eigen::Matrix2Xd const& x1, + Eigen::Matrix2Xd const& x2, Eigen::Matrix3d const& e, std::vector const& vec_inliers, + Eigen::Matrix3d* r, Eigen::Vector3d* t); } // end namespace interest_point diff --git a/localization/interest_point/include/interest_point/matching.h b/localization/interest_point/include/interest_point/matching.h index e2c2040e9f..21cb10328a 100644 --- a/localization/interest_point/include/interest_point/matching.h +++ b/localization/interest_point/include/interest_point/matching.h @@ -18,6 +18,7 @@ #ifndef INTEREST_POINT_MATCHING_H_ #define INTEREST_POINT_MATCHING_H_ +#include #include #include @@ -45,9 +46,12 @@ namespace interest_point { void GetDetectorParams(int & min_features, int & max_features, int & max_retries, double & min_thresh, double & default_thresh, double & max_thresh); + int last_keypoint_count(void) { return last_keypoint_count_; } + protected: unsigned int min_features_, max_features_, max_retries_; double min_thresh_, default_thresh_, max_thresh_, dynamic_thresh_; + int last_keypoint_count_; }; class FeatureDetector { @@ -81,16 +85,20 @@ namespace interest_point { friend bool operator== (FeatureDetector const& A, FeatureDetector const& B) { return (A.detector_name_ == B.detector_name_); } + + DynamicDetector& dynamic_detector() { return *detector_; } }; /** * descriptor is what opencv descriptor was used to make the descriptors * the descriptor maps are the features in the two images * matches is output to contain the matching features between the two images + * Optionally pass hamming distance and goodness ratio, otherwise flag + * values are used. **/ - void FindMatches(const cv::Mat & img1_descriptor_map, - const cv::Mat & img2_descriptor_map, - std::vector * matches); + void FindMatches(const cv::Mat& img1_descriptor_map, const cv::Mat& img2_descriptor_map, + std::vector* matches, boost::optional hamming_distance = boost::none, + boost::optional goodness_ratio = boost::none); } // namespace interest_point #endif // INTEREST_POINT_MATCHING_H_ diff --git a/localization/interest_point/src/BAD.cpp b/localization/interest_point/src/BAD.cpp new file mode 100644 index 0000000000..47835952a9 --- /dev/null +++ b/localization/interest_point/src/BAD.cpp @@ -0,0 +1,638 @@ +/** + * @copyright 2021 Xoan Iago Suarez Canosa. All rights reserved. + * Constact: iago.suarez@thegraffter.com + * Software developed in the PhD: Low-level vision for resource-limited devices + */ +#include +#include +#include + +#define UPM_DEGREES_TO_RADS 0.017453292519943295 +#define UPM_ROUNDNUM(x) (static_cast(x + 0.5f)) +#define UPM_BAD_EXTRA_RATIO_MARGIN 1.75 + +namespace upm { +/** + * @brief Rectifies the coordinates of the box pairs measurement functions with the keypoint + * location parameters. + * @param boxes_params The input parameters defining the location of each pair of boxes inside + * the normalized (32x32) patch. + * @param out_params The output parameters, now the boxes are located in the full image. + * @param kp The keypoint defining the offset, rotation and scale to be applied + * @param scale_factor A scale factor that magnifies the measurement functions w.r.t. the keypoint. + * @param patch_size The size of the normalized patch where the measurement functions were learnt. + */ +static inline void rectifyBoxes(const std::vector &boxes_params, + std::vector &out_params, + const cv::KeyPoint &kp, + float scale_factor = 1, + const cv::Size &patch_size = cv::Size(32, 32)) { + float m00, m01, m02, m10, m11, m12; + float s, cosine, sine; + + s = scale_factor * kp.size / (0.5f * (patch_size.width + patch_size.height)); + out_params.resize(boxes_params.size()); + + if (kp.angle == -1) { + m00 = s; + m01 = 0.0f; + m02 = -0.5f * s * patch_size.width + kp.pt.x; + m10 = 0.0f; + m11 = s; + m12 = -s * 0.5f * patch_size.height + kp.pt.y; + } else { + cosine = (kp.angle >= 0) ? static_cast(cos(kp.angle * UPM_DEGREES_TO_RADS)) : 1.f; + sine = (kp.angle >= 0) ? static_cast(sin(kp.angle * UPM_DEGREES_TO_RADS)) : 0.f; + + m00 = s * cosine; + m01 = -s * sine; + m02 = (-s * cosine + s * sine) * patch_size.width * 0.5f + kp.pt.x; + m10 = s * sine; + m11 = s * cosine; + m12 = (-s * sine - s * cosine) * patch_size.height * 0.5f + kp.pt.y; + } + + for (size_t i = 0; i < boxes_params.size(); i++) { + out_params[i].x1 = UPM_ROUNDNUM(m00 * boxes_params[i].x1 + m01 * boxes_params[i].y1 + m02); + out_params[i].y1 = UPM_ROUNDNUM(m10 * boxes_params[i].x1 + m11 * boxes_params[i].y1 + m12); + out_params[i].x2 = UPM_ROUNDNUM(m00 * boxes_params[i].x2 + m01 * boxes_params[i].y2 + m02); + out_params[i].y2 = UPM_ROUNDNUM(m10 * boxes_params[i].x2 + m11 * boxes_params[i].y2 + m12); + out_params[i].boxRadius = UPM_ROUNDNUM(s * boxes_params[i].boxRadius); + } +} + +/** + * @brief Computes the Box Average Difference, measuring the difference of gray level in the two + * square regions. + * @param box_params The weak-learner parameter defining the size and locations of each box. + * @param integral_img The integral image used to compute the average gray value in the square regions. + * @return The difference of gray level in the two squares defined by box_params + */ +static inline float computeBadResponse(const BAD::BoxPairParams &box_params, + const cv::Mat &integral_img) { + assert(!integral_img.empty()); + assert(integral_img.type() == CV_32SC1); + + int frame_w, frame_h, box1x1, box1y1, box1x2, box1y2, box2x1, box2y1, box2x2, box2y2; + int A, B, C, D; + int box_area1, box_area2; + float sum1, sum2, average1, average2; + // Since the integral image has one extra row and col, calculate the patch dimensions + frame_w = integral_img.cols; + frame_h = integral_img.rows; + + // For the first box, we calculate its margin coordinates + box1x1 = box_params.x1 - box_params.boxRadius; + if (box1x1 < 0) + box1x1 = 0; + else if (box1x1 >= frame_w - 1) + box1x1 = frame_w - 2; + box1y1 = box_params.y1 - box_params.boxRadius; + if (box1y1 < 0) + box1y1 = 0; + else if (box1y1 >= frame_h - 1) + box1y1 = frame_h - 2; + box1x2 = box_params.x1 + box_params.boxRadius + 1; + if (box1x2 <= 0) + box1x2 = 1; + else if (box1x2 >= frame_w) + box1x2 = frame_w - 1; + box1y2 = box_params.y1 + box_params.boxRadius + 1; + if (box1y2 <= 0) + box1y2 = 1; + else if (box1y2 >= frame_h) + box1y2 = frame_h - 1; + assert((box1x1 < box1x2 && box1y1 < box1y2) && "Box 1 has size 0"); + + // For the second box, we calculate its margin coordinates + box2x1 = box_params.x2 - box_params.boxRadius; + if (box2x1 < 0) + box2x1 = 0; + else if (box2x1 >= frame_w - 1) + box2x1 = frame_w - 2; + box2y1 = box_params.y2 - box_params.boxRadius; + if (box2y1 < 0) + box2y1 = 0; + else if (box2y1 >= frame_h - 1) + box2y1 = frame_h - 2; + box2x2 = box_params.x2 + box_params.boxRadius + 1; + if (box2x2 <= 0) + box2x2 = 1; + else if (box2x2 >= frame_w) + box2x2 = frame_w - 1; + box2y2 = box_params.y2 + box_params.boxRadius + 1; + if (box2y2 <= 0) + box2y2 = 1; + else if (box2y2 >= frame_h) + box2y2 = frame_h - 1; + assert((box2x1 < box2x2 && box2y1 < box2y2) && "Box 2 has size 0"); + + // Read the integral image values for the first box + A = integral_img.at(box1y1, box1x1); + B = integral_img.at(box1y1, box1x2); + C = integral_img.at(box1y2, box1x1); + D = integral_img.at(box1y2, box1x2); + + // Calculate the mean intensity value of the pixels in the box + sum1 = static_cast(A + D - B - C); + box_area1 = (box1y2 - box1y1) * (box1x2 - box1x1); + assert(box_area1 > 0); + average1 = sum1 / box_area1; + + // Calculate the indices on the integral image where the box falls + A = integral_img.at(box2y1, box2x1); + B = integral_img.at(box2y1, box2x2); + C = integral_img.at(box2y2, box2x1); + D = integral_img.at(box2y2, box2x2); + + // Calculate the mean intensity value of the pixels in the box + sum2 = static_cast(A + D - B - C); + box_area2 = (box2y2 - box2y1) * (box2x2 - box2x1); + assert(box_area2 > 0); + average2 = sum2 / box_area2; + + return average1 - average2; +} + +/** + * @brief Function that determines if a keypoint is close to the image border. + * @param kp The detected keypoint + * @param img_size The size of the image + * @param patch_size The size of the normalized patch where the measurement functions were learnt. + * @param scale_factor A scale factor that magnifies the measurement functions w.r.t. the keypoint. + * @return true if the keypoint is in the border, false otherwise + */ +static inline bool isKeypointInTheBorder(const cv::KeyPoint &kp, + const cv::Size &img_size, + const cv::Size &patch_size = {32, 32}, + float scale_factor = 1) { + // This would be the correct measure but since we will compare with half of the size, use this as border size + float s = scale_factor * kp.size / (patch_size.width + patch_size.height); + cv::Size2f border(patch_size.width * s * UPM_BAD_EXTRA_RATIO_MARGIN, + patch_size.height * s * UPM_BAD_EXTRA_RATIO_MARGIN); + + if (kp.pt.x < border.width || kp.pt.x + border.width >= img_size.width) + return true; + + if (kp.pt.y < border.height || kp.pt.y + border.height >= img_size.height) + return true; + + return false; +} + +/////////////////////////////////////////////////////////////////////////////// + +BAD::BAD(float scale_factor, BadSize n_bits) { + scale_factor_ = scale_factor; + + if (n_bits == SIZE_512_BITS) { + // With data augmentation + // box_params_ = {{17,18,12,15,2}, {13,14,5,7,5}, {21,16,16,14,1}, {27,11,18,20,3}, {17,13,16,19,2}, + // {18,24,18,16,5}, {12,11,10,25,6}, {14,17,14,13,1}, {7,4,4,15,4}, {27,27,23,8,4}, {19,13,19,6,6}, {14,15,10,16,1}, + // {13,15,12,22,1}, {8,22,3,27,3}, {13,19,8,13,1}, {18,16,17,12,1}, {27,7,25,11,4}, {24,20,20,15,2}, {16,24,14,3,3}, + // {23,18,7,18,7}, {8,7,2,1,1}, {17,28,17,26,3}, {17,13,17,10,2}, {10,18,10,11,1}, {11,28,7,22,2}, {18,13,15,15,1}, + // {7,14,3,20,3}, {17,19,14,15,1}, {14,12,14,8,2}, {14,12,13,11,1}, {21,9,19,19,2}, {4,28,3,10,3}, {27,27,26,26,4}, + // {19,22,19,19,2}, {12,25,12,20,1}, {19,12,15,12,1}, {28,21,23,21,2}, {10,15,7,18,2}, {12,7,10,3,3}, + // {21,16,19,15,1}, {19,20,18,17,1}, {26,2,19,7,2}, {18,2,15,22,2}, {24,26,24,22,5}, {15,26,15,19,1}, + // {13,19,11,20,1}, {5,14,4,10,4}, {15,7,15,4,2}, {13,16,11,7,1}, {15,22,15,18,1}, {24,8,23,4,4}, {13,11,11,14,1}, + // {4,19,3,19,3}, {22,12,19,10,1}, {24,27,15,22,2}, {12,13,10,10,1}, {11,25,9,29,2}, {15,21,15,10,1}, + // {19,16,18,19,1}, {29,13,24,8,2}, {17,16,16,20,1}, {12,17,12,15,1}, {28,4,2,11,2}, {7,25,5,19,3}, {22,13,20,16,1}, + // {14,16,13,17,1}, {10,3,8,11,3}, {18,7,17,11,2}, {27,11,25,22,2}, {5,26,3,28,3}, {28,13,27,13,3}, {22,20,20,28,3}, + // {12,6,5,2,2}, {14,18,13,16,1}, {17,29,3,25,2}, {20,20,19,19,1}, {15,12,14,15,1}, {12,14,12,13,1}, + // {17,14,10,26,3}, {11,15,6,12,6}, {9,22,9,19,1}, {19,18,19,14,1}, {23,15,12,18,2}, {12,15,11,14,1}, {28,2,27,9,2}, + // {11,19,11,11,7}, {13,29,13,23,2}, {27,19,22,17,3}, {17,3,17,2,2}, {4,6,3,3,3}, {19,15,16,16,1}, {22,5,20,9,2}, + // {14,6,13,9,2}, {17,16,13,16,2}, {24,18,12,6,6}, {20,14,18,15,2}, {20,9,18,13,1}, {18,20,17,8,2}, {10,15,9,15,2}, + // {13,7,12,26,2}, {13,12,11,19,2}, {15,2,2,29,2}, {15,12,14,13,1}, {20,30,19,26,1}, {28,26,28,4,3}, + // {16,13,15,12,1}, {18,11,17,25,2}, {3,17,1,24,1}, {21,18,19,22,1}, {9,13,9,8,2}, {19,18,16,16,1}, {21,22,17,20,1}, + // {13,4,13,3,3}, {24,15,21,9,1}, {24,25,19,17,6}, {4,14,3,14,2}, {17,13,14,19,1}, {7,19,4,16,3}, {4,20,1,5,1}, + // {15,13,12,14,3}, {19,26,19,21,2}, {11,26,10,18,5}, {17,16,17,13,1}, {19,16,19,11,1}, {4,26,4,23,4}, + // {14,19,14,13,5}, {10,13,8,13,2}, {14,12,14,10,1}, {29,24,26,19,2}, {26,9,19,19,5}, {16,23,16,17,1}, {4,13,3,4,3}, + // {13,16,7,21,2}, {17,16,16,17,1}, {29,15,5,18,2}, {29,2,23,5,2}, {9,17,9,14,2}, {25,26,25,22,5}, {13,21,13,20,1}, + // {23,12,7,20,6}, {6,8,6,3,3}, {13,19,13,17,1}, {25,21,22,20,1}, {24,17,23,15,2}, {20,8,17,4,1}, {11,19,10,17,1}, + // {9,11,6,9,1}, {25,9,24,14,1}, {18,20,13,14,3}, {26,23,25,23,5}, {14,20,11,4,4}, {28,7,25,13,3}, {13,13,12,12,1}, + // {7,29,2,2,2}, {16,17,16,8,5}, {20,6,19,12,3}, {19,7,19,6,6}, {20,13,19,14,1}, {19,24,16,29,2}, {8,15,4,13,1}, + // {7,9,2,10,2}, {15,14,14,13,1}, {18,13,18,11,1}, {8,19,5,23,2}, {3,13,1,14,1}, {23,20,16,14,1}, {17,15,13,18,2}, + // {16,16,9,14,5}, {15,28,15,27,3}, {18,20,16,19,1}, {16,17,16,11,2}, {30,1,10,19,1}, {12,19,9,23,2}, + // {25,13,21,13,1}, {9,23,5,24,5}, {13,20,13,18,1}, {13,13,12,13,3}, {29,18,25,2,2}, {30,30,25,26,1}, + // {16,20,15,11,1}, {18,16,18,14,1}, {15,18,5,7,4}, {16,13,15,19,1}, {26,24,16,9,5}, {1,28,1,5,1}, {20,17,20,16,1}, + // {15,19,10,17,4}, {12,9,10,5,1}, {30,29,28,29,1}, {29,17,27,18,2}, {17,29,15,27,2}, {9,29,9,28,2}, + // {23,24,21,22,1}, {22,2,1,1,1}, {20,4,20,1,1}, {5,30,4,25,1}, {20,8,17,12,7}, {10,7,3,17,3}, {21,17,14,15,5}, + // {14,10,13,8,1}, {4,21,4,13,3}, {30,1,24,10,1}, {15,17,14,16,3}, {21,23,20,15,3}, {17,20,17,18,3}, {12,11,12,6,5}, + // {15,15,12,17,1}, {25,9,16,25,6}, {22,28,22,27,3}, {5,8,3,3,3}, {9,5,9,1,1}, {30,12,29,23,1}, {20,21,5,9,5}, + // {15,21,15,20,1}, {11,17,10,23,2}, {16,11,15,13,1}, {16,12,16,10,1}, {15,6,14,3,3}, {2,4,1,1,1}, {15,16,11,15,1}, + // {24,6,24,2,2}, {8,15,6,12,1}, {21,27,1,30,1}, {17,10,14,16,3}, {13,9,7,7,7}, {22,17,19,17,1}, {16,14,14,13,2}, + // {14,21,13,23,1}, {18,2,15,7,2}, {3,25,1,24,1}, {24,20,7,14,7}, {26,25,24,19,2}, {6,25,6,23,6}, {15,24,15,17,7}, + // {22,14,16,15,1}, {17,25,17,23,1}, {12,18,2,26,2}, {30,30,26,11,1}, {22,8,16,14,5}, {9,16,8,20,1}, {4,14,2,13,2}, + // {28,7,27,8,1}, {10,22,9,24,1}, {14,16,13,18,3}, {28,26,3,15,2}, {12,15,10,15,1}, {18,17,17,15,1}, + // {30,10,28,14,1}, {30,14,28,30,1}, {30,18,7,13,1}, {3,19,2,20,1}, {16,19,14,13,2}, {11,9,5,27,4}, {16,19,15,15,2}, + // {24,22,18,19,7}, {12,17,12,12,1}, {28,5,28,1,1}, {4,29,2,30,1}, {27,11,27,8,1}, {8,3,8,1,1}, {15,10,15,8,3}, + // {12,27,11,18,4}, {25,6,22,8,6}, {15,3,15,2,2}, {19,22,17,19,1}, {24,21,24,16,2}, {9,7,6,6,6}, {13,26,11,27,2}, + // {24,10,19,12,4}, {22,17,22,9,2}, {17,14,14,11,1}, {13,4,13,3,1}, {15,18,15,17,1}, {29,30,29,24,1}, + // {29,29,20,17,2}, {6,12,2,27,2}, {18,17,14,13,2}, {11,27,11,26,4}, {22,12,3,18,3}, {15,13,13,9,1}, {12,20,7,18,1}, + // {16,6,15,9,1}, {3,6,1,7,1}, {12,17,11,19,1}, {15,8,8,18,7}, {11,19,11,5,3}, {17,20,16,23,3}, {12,6,9,13,1}, + // {2,1,1,2,1}, {14,26,13,21,3}, {25,16,16,14,3}, {30,14,29,14,1}, {27,25,15,22,4}, {13,10,8,7,2}, {18,19,13,14,1}, + // {28,28,28,22,3}, {8,14,8,11,1}, {23,28,22,24,2}, {8,2,3,18,2}, {22,24,22,23,7}, {20,17,15,16,1}, {8,11,6,4,4}, + // {25,13,23,13,2}, {18,18,16,15,1}, {20,16,16,15,1}, {18,20,14,26,3}, {17,12,17,8,1}, {1,5,1,3,1}, {22,13,13,20,2}, + // {17,16,17,14,3}, {27,17,25,17,2}, {8,23,6,29,2}, {15,4,14,18,1}, {10,24,10,17,4}, {25,30,25,28,1}, {3,22,1,29,1}, + // {24,8,23,17,1}, {26,3,26,1,1}, {18,22,18,17,2}, {9,17,8,10,2}, {29,22,29,2,2}, {19,4,5,10,3}, {3,28,3,27,1}, + // {12,15,11,18,1}, {30,3,28,4,1}, {7,9,7,8,1}, {24,15,8,14,7}, {30,6,20,16,1}, {18,18,1,10,1}, {30,20,28,21,1}, + // {15,15,13,14,1}, {6,3,5,1,1}, {3,8,1,17,1}, {3,2,2,2,2}, {19,28,18,20,1}, {20,20,20,17,2}, {21,30,19,29,1}, + // {12,19,12,13,1}, {29,10,29,4,2}, {20,16,20,14,1}, {15,9,11,16,2}, {8,13,6,26,4}, {13,11,12,8,2}, {17,27,17,26,4}, + // {29,29,14,12,1}, {29,2,28,3,2}, {9,15,7,9,4}, {27,28,12,30,1}, {14,30,2,28,1}, {19,12,18,14,1}, {26,5,24,15,5}, + // {2,24,2,2,2}, {6,21,5,21,1}, {22,16,9,17,2}, {16,19,15,17,1}, {2,29,2,28,2}, {25,11,24,1,1}, {16,30,16,29,1}, + // {14,20,14,17,3}, {15,14,11,17,3}, {18,17,16,21,1}, {17,8,17,4,2}, {11,4,11,3,3}, {25,16,9,17,6}, {18,8,18,6,6}, + // {17,22,17,19,1}, {8,20,3,11,3}, {20,17,4,17,1}, {29,12,12,19,2}, {14,29,14,28,2}, {12,18,10,18,1}, + // {13,15,13,11,2}, {18,15,14,15,2}, {19,17,17,19,1}, {22,17,12,16,6}, {30,22,29,18,1}, {30,2,29,20,1}, + // {12,3,1,1,1}, {4,7,1,7,1}, {27,10,21,13,4}, {18,21,18,13,3}, {12,4,3,6,2}, {12,10,9,3,2}, {3,28,2,29,2}, + // {22,2,20,5,2}, {27,18,20,3,3}, {6,24,6,23,1}, {27,26,9,16,4}, {5,18,5,11,5}, {20,14,15,12,3}, {19,16,19,15,1}, + // {27,4,21,9,4}, {3,19,2,29,1}, {20,24,18,22,1}, {18,7,18,2,1}, {28,30,28,28,1}, {11,24,10,9,1}, {21,18,21,14,3}, + // {27,19,26,18,2}, {16,18,10,6,6}, {11,18,5,19,1}, {24,16,22,16,1}, {17,15,17,9,5}, {27,29,20,11,2}, + // {29,25,28,22,1}, {21,11,21,5,1}, {12,15,8,16,2}, {2,29,1,30,1}, {18,12,4,21,3}, {18,9,11,13,3}, {18,3,10,21,3}, + // {17,11,16,16,1}, {15,17,13,14,1}, {7,7,7,5,5}, {9,29,5,18,2}, {10,11,10,6,6}, {28,26,25,26,1}, {19,30,8,20,1}, + // {8,15,7,29,2}, {21,18,19,17,1}, {2,22,1,22,1}, {12,20,4,17,1}, {27,8,4,14,2}, {26,10,25,13,1}, {19,13,19,8,3}, + // {12,16,7,18,7}, {20,26,12,3,3}, {6,10,3,10,2}, {25,25,25,21,2}, {12,3,7,16,2}, {8,4,4,17,4}, {12,20,5,8,5}, + // {22,15,8,13,7}, {12,13,12,8,2}, {20,15,19,13,1}, {30,5,29,8,1}, {14,29,13,23,2}, {18,19,9,10,7}, {2,11,1,10,1}, + // {12,13,12,11,1}, {27,15,9,5,4}, {13,12,7,17,2}, {8,17,1,26,1}, {20,24,11,12,4}, {12,24,10,22,6}, {19,29,14,20,1}, + // {20,27,20,25,2}, {9,25,8,27,1}, {7,11,5,11,1}, {20,11,11,8,1}, {9,8,9,5,1}, {27,9,25,10,1}, {30,20,22,20,1}, + // {26,21,26,20,1}, {30,14,27,16,1}, {12,16,11,19,3}, {7,28,6,29,1}, {17,23,17,22,2}, {12,17,2,2,1}, + // {17,14,17,13,1}, {18,12,16,16,1}, {7,23,7,17,1}, {25,12,9,15,4}, {16,6,16,5,5}, {8,16,7,16,7}, {6,7,5,7,5}, + // {15,13,15,12,2}, {13,15,13,13,3}, {16,12,16,11,1}, {18,15,15,14,3}, {17,8,14,5,4}, {9,26,6,22,5}, + // {17,16,14,17,3}, {25,1,24,2,1}, {14,16,14,15,1}, {24,22,4,23,4}, {30,29,27,29,1}, {17,18,17,17,1}, + // {19,30,19,28,1}, {21,27,21,23,3}, {16,18,15,20,1}, {27,27,13,12,4}, {30,25,27,26,1}, {4,21,3,7,1}, {10,5,10,4,4}, + // {14,14,5,3,1}, {23,6,21,3,3}, {9,20,2,15,2}, {23,9,20,13,1}, {15,14,12,3,3}, {19,25,19,18,4}, {27,25,24,22,4}, + // {15,15,15,11,1}, {17,16,14,13,1}, {12,18,12,17,1}, {30,3,30,2,1}, {21,20,18,28,3}, {25,25,7,14,5}, {3,11,2,3,2}, + // {25,5,9,21,4}, {6,15,4,28,3}, {9,9,3,3,3}, {16,19,14,16,2}, {10,25,10,20,1}, {2,17,2,15,1}, {17,15,15,16,1}, + // {20,15,19,15,1}, {22,2,22,1,1}, {15,19,15,18,1}, {15,16,10,12,1}, {28,2,23,14,2}, {11,3,9,2,1}}; thresholds_ = + // {14.45,4.15,7.75,9.65,2.25,0.15,0.45,-0.95,3.65,-1.75,1.05,5.45,-0.05,-2.65,3.35,1.65,-1.55,2.85,3.05,22.05, + // -3.65,-0.15,-0.55,-0.05,3.25,-2.85,-1.25,10.45,-1.05,1.25,3.15,2.05,-0.55,-1.25,3.45,4.35,2.45,2.55,1.35,1.25, + // 2.75,-0.15,8.35,0.35,-1.25,0.75,-0.45,-0.05,0.85,3.65,1.55,1.75,-0.35,1.35,4.85,-2.25,0.25,-2.15,3.35,1.95,6.25, + // 0.35,24.45,-1.15,0.85,-1.95,5.15,1.35,0.85,0.35,-0.45,4.95,-0.25,2.45,11.75,0.85,-1.65,0.05,4.05,-0.35,-0.95, + // -0.25,49.35,0.85,1.95,0.25,1.15,-2.65,-0.25,0.85,-5.65,1.15,3.05,4.05,7.35,9.05,0.35,1.65,1.65,2.55,6.95,1.75, + // 0.85,-1.15,1.35,5.45,1.35,-2.35,-0.15,0.85,-3.05,0.05,-0.35,-0.65,11.65,-0.85,1.15,2.35,-9.25,16.05,-1.45,1.95, + // 0.35,-0.35,0.15,0.25,-1.35,0.65,-4.05,-2.25,1.05,-0.05,-1.55,0.25,69.45,0.15,-1.25,-1.85,-0.65,8.75,-0.95,1.95, + // 1.05,-1.45,2.15,-1.85,-1.75,-2.95,2.65,0.65,-1.05,4.35,0.25,19.65,0.35,1.65,0.65,2.75,2.85,-10.95,3.65,-4.15, + // 0.25,-0.65,-2.25,3.75,35.75,3.15,-0.55,1.95,-6.55,88.35,7.25,1.75,-4.15,0.25,0.25,0.65,3.75,1.65,2.85,42.15, + // 11.75, -1.25,-8.25,0.05,2.15,0.65,-1.55,-2.75,1.15,-0.15,-0.75,16.85,1.15,1.45,15.35,19.45,8.65,-1.25,0.25, + // 2.15,0.15,2.85, -2.25,1.75,-15.15,4.15,-0.45,-8.25,1.25,-0.85,3.55,0.45,-1.25,9.65,1.25,3.15,-2.05,-6.85,-0.05, + // 3.25,7.05,2.35, -1.45,-1.05,20.25,1.15,1.35,1.05,5.15,2.55,0.55,0.55,-0.15,0.65,-28.35,3.85,2.05,-1.15,3.05, + // -0.95,-1.55,-0.35, 101.95,0.35,-2.05,-2.15,4.25,105.15,1.15,27.45,30.15,18.45,-2.45,1.85,2.95,-3.75,-0.65,0.05, + // 0.25,34.45,-2.05, 0.25,3.25,2.05,2.85,3.45,20.95,1.45,-1.95,-0.35,-6.85,1.15,-31.95,11.85,2.75,0.15,8.35,1.15, + // 2.15,-1.15,-1.05, 2.45,1.45,0.15,-1.65,41.85,0.35,0.65,-0.35,0.25,56.75,23.55,88.85,-7.25,1.05,0.75,-1.05,-1.65, + // -9.75,-16.95,-0.05, -10.05,34.15,14.75,-0.95,1.45,4.85,-0.05,-0.35,0.75,64.75,24.25,-0.35,-13.65,2.85,1.75, + // -0.35,0.25,-12.15,108.05, -0.35,-3.35,1.95,-0.35,21.25,-20.05,95.85,-1.05,-17.75,-0.55,2.75,0.65,0.75,-1.85, + // 2.65,-2.15,1.05,-0.05,4.25,3.75, -13.55,-0.05,105.95,0.45,3.05,9.35,79.25,1.75,7.65,-0.45,0.95,79.15,28.05,0.05, + // 0.65,-0.35,0.95,2.05,1.15,2.75, -1.65,8.65,3.55,-0.25,-28.95,-7.35,122.25,0.55,4.75,1.75,11.15,0.15,2.55,0.05, + // -7.95,51.05,6.25,0.05,-1.15,0.75, 26.65,-1.25,1.15,-0.55,0.05,5.85,1.55,2.25,-1.65,-16.85,-62.65,-1.25,-1.05, + // -1.55,1.35,0.25,-0.65,0.85,17.45, 0.25, -9.35,34.35,0.95,-0.95,-10.05,1.45,-6.05,93.25,3.15,-10.85,-1.65,-0.15, + // -2.45,-0.35,0.15,118.15,-81.85,4.05,1.05, -0.85,152.35,0.65,1.35,-12.35,95.35,-23.25,-0.75,75.65,50.85,47.85, + // 2.55,0.55,0.15,0.25,38.15,-2.65,-1.95,-1.05, 110.25,35.25,2.65,3.65,18.75,89.45,-0.55,0.35,0.25,105.05,0.25, + // 1.25,-34.65,-0.55,-0.15,0.75,-0.15,-0.15,-94.25, -12.05,-15.35,-1.75,31.35,0.15,-2.15,-0.15,-12.05,-0.35,-0.65, + // -0.55,28.95,39.05,7.05,0.65,-1.65,2.85,4.85,-1.85, 1.75,-31.65,0.05,13.75,-0.35,0.35,-5.75,83.85,0.95,27.95, + // -0.25,-19.05,2.45,-13.25,44.05,51.85,-0.05,0.05,79.35,111.25,-61.65,3.05,-69.95,35.65,62.05,-0.25,-1.05,-2.95, + // 0.95,-0.15,-18.15,-18.65,74.55,2.65}; + + // Without data augmentation + box_params_ = { + {17, 12, 18, 15, 2}, {13, 5, 14, 7, 5}, {21, 16, 16, 14, 1}, {27, 18, 11, 20, 3}, {17, 16, 13, 19, 2}, + {18, 18, 24, 16, 5}, {12, 10, 11, 25, 6}, {14, 14, 17, 13, 1}, {7, 4, 4, 15, 4}, {27, 23, 27, 8, 4}, + {19, 19, 13, 6, 6}, {14, 10, 15, 16, 1}, {13, 12, 15, 22, 1}, {8, 3, 22, 27, 3}, {13, 8, 19, 13, 1}, + {18, 17, 16, 12, 1}, {27, 25, 7, 11, 4}, {24, 20, 20, 15, 2}, {16, 14, 24, 3, 3}, {23, 7, 18, 18, 7}, + {8, 2, 7, 1, 1}, {17, 17, 28, 26, 3}, {17, 17, 13, 10, 2}, {10, 10, 18, 11, 1}, {11, 7, 28, 22, 2}, + {18, 15, 13, 15, 1}, {7, 3, 14, 20, 3}, {17, 14, 19, 15, 1}, {14, 14, 12, 8, 2}, {14, 13, 12, 11, 1}, + {21, 19, 9, 19, 2}, {4, 3, 28, 10, 3}, {27, 26, 27, 26, 4}, {19, 19, 22, 19, 2}, {12, 12, 25, 20, 1}, + {19, 15, 12, 12, 1}, {28, 23, 21, 21, 2}, {10, 7, 15, 18, 2}, {12, 10, 7, 3, 3}, {21, 19, 16, 15, 1}, + {19, 18, 20, 17, 1}, {26, 19, 2, 7, 2}, {18, 15, 2, 22, 2}, {24, 24, 26, 22, 5}, {15, 15, 26, 19, 1}, + {13, 11, 19, 20, 1}, {5, 4, 14, 10, 4}, {15, 15, 7, 4, 2}, {13, 11, 16, 7, 1}, {15, 15, 22, 18, 1}, + {24, 23, 8, 4, 4}, {13, 11, 11, 14, 1}, {4, 3, 19, 19, 3}, {22, 19, 12, 10, 1}, {24, 15, 27, 22, 2}, + {12, 10, 13, 10, 1}, {11, 9, 25, 29, 2}, {15, 15, 21, 10, 1}, {19, 18, 16, 19, 1}, {29, 24, 13, 8, 2}, + {17, 16, 16, 20, 1}, {12, 12, 17, 15, 1}, {28, 2, 4, 11, 2}, {7, 5, 25, 19, 3}, {22, 20, 13, 16, 1}, + {14, 13, 16, 17, 1}, {10, 8, 3, 11, 3}, {18, 17, 7, 11, 2}, {27, 25, 11, 22, 2}, {5, 3, 26, 28, 3}, + {28, 27, 13, 13, 3}, {22, 20, 20, 28, 3}, {12, 5, 6, 2, 2}, {14, 13, 18, 16, 1}, {17, 3, 29, 25, 2}, + {20, 19, 20, 19, 1}, {15, 14, 12, 15, 1}, {12, 12, 14, 13, 1}, {17, 10, 14, 26, 3}, {11, 6, 15, 12, 6}, + {9, 9, 22, 19, 1}, {19, 19, 18, 14, 1}, {23, 12, 15, 18, 2}, {12, 11, 15, 14, 1}, {28, 27, 2, 9, 2}, + {11, 11, 19, 11, 7}, {13, 13, 29, 23, 2}, {27, 22, 19, 17, 3}, {17, 17, 3, 2, 2}, {4, 3, 6, 3, 3}, + {19, 16, 15, 16, 1}, {22, 20, 5, 9, 2}, {14, 13, 6, 9, 2}, {17, 13, 16, 16, 2}, {24, 12, 18, 6, 6}, + {20, 18, 14, 15, 2}, {20, 18, 9, 13, 1}, {18, 17, 20, 8, 2}, {10, 9, 15, 15, 2}, {13, 12, 7, 26, 2}, + {13, 11, 12, 19, 2}, {15, 2, 2, 29, 2}, {15, 14, 12, 13, 1}, {20, 19, 30, 26, 1}, {28, 28, 26, 4, 3}, + {16, 15, 13, 12, 1}, {18, 17, 11, 25, 2}, {3, 1, 17, 24, 1}, {21, 19, 18, 22, 1}, {9, 9, 13, 8, 2}, + {19, 16, 18, 16, 1}, {21, 17, 22, 20, 1}, {13, 13, 4, 3, 3}, {24, 21, 15, 9, 1}, {24, 19, 25, 17, 6}, + {4, 3, 14, 14, 2}, {17, 14, 13, 19, 1}, {7, 4, 19, 16, 3}, {4, 1, 20, 5, 1}, {15, 12, 13, 14, 3}, + {19, 19, 26, 21, 2}, {11, 10, 26, 18, 5}, {17, 17, 16, 13, 1}, {19, 19, 16, 11, 1}, {4, 4, 26, 23, 4}, + {14, 14, 19, 13, 5}, {10, 8, 13, 13, 2}, {14, 14, 12, 10, 1}, {29, 26, 24, 19, 2}, {26, 19, 9, 19, 5}, + {16, 16, 23, 17, 1}, {4, 3, 13, 4, 3}, {13, 7, 16, 21, 2}, {17, 16, 16, 17, 1}, {29, 5, 15, 18, 2}, + {29, 23, 2, 5, 2}, {9, 9, 17, 14, 2}, {25, 25, 26, 22, 5}, {13, 13, 21, 20, 1}, {23, 7, 12, 20, 6}, + {6, 6, 8, 3, 3}, {13, 13, 19, 17, 1}, {25, 22, 21, 20, 1}, {24, 23, 17, 15, 2}, {20, 17, 8, 4, 1}, + {11, 10, 19, 17, 1}, {9, 6, 11, 9, 1}, {25, 24, 9, 14, 1}, {18, 13, 20, 14, 3}, {26, 25, 23, 23, 5}, + {14, 11, 20, 4, 4}, {28, 25, 7, 13, 3}, {13, 12, 13, 12, 1}, {7, 2, 29, 2, 2}, {16, 16, 17, 8, 5}, + {20, 19, 6, 12, 3}, {19, 19, 7, 6, 6}, {20, 19, 13, 14, 1}, {19, 16, 24, 29, 2}, {8, 4, 15, 13, 1}, + {7, 2, 9, 10, 2}, {15, 14, 14, 13, 1}, {18, 18, 13, 11, 1}, {8, 5, 19, 23, 2}, {3, 1, 13, 14, 1}, + {23, 16, 20, 14, 1}, {17, 13, 15, 18, 2}, {16, 9, 16, 14, 5}, {15, 15, 28, 27, 3}, {18, 16, 20, 19, 1}, + {16, 16, 17, 11, 2}, {30, 10, 1, 19, 1}, {12, 9, 19, 23, 2}, {25, 21, 13, 13, 1}, {9, 5, 23, 24, 5}, + {13, 13, 20, 18, 1}, {13, 12, 13, 13, 3}, {29, 25, 18, 2, 2}, {30, 25, 30, 26, 1}, {16, 15, 20, 11, 1}, + {18, 18, 16, 14, 1}, {15, 5, 18, 7, 4}, {16, 15, 13, 19, 1}, {26, 16, 24, 9, 5}, {1, 1, 28, 5, 1}, + {20, 20, 17, 16, 1}, {15, 10, 19, 17, 4}, {12, 10, 9, 5, 1}, {30, 28, 29, 29, 1}, {29, 27, 17, 18, 2}, + {17, 15, 29, 27, 2}, {9, 9, 29, 28, 2}, {23, 21, 24, 22, 1}, {22, 1, 2, 1, 1}, {20, 20, 4, 1, 1}, + {5, 4, 30, 25, 1}, {20, 17, 8, 12, 7}, {10, 3, 7, 17, 3}, {21, 14, 17, 15, 5}, {14, 13, 10, 8, 1}, + {4, 4, 21, 13, 3}, {30, 24, 1, 10, 1}, {15, 14, 17, 16, 3}, {21, 20, 23, 15, 3}, {17, 17, 20, 18, 3}, + {12, 12, 11, 6, 5}, {15, 12, 15, 17, 1}, {25, 16, 9, 25, 6}, {22, 22, 28, 27, 3}, {5, 3, 8, 3, 3}, + {9, 9, 5, 1, 1}, {30, 29, 12, 23, 1}, {20, 5, 21, 9, 5}, {15, 15, 21, 20, 1}, {11, 10, 17, 23, 2}, + {16, 15, 11, 13, 1}, {16, 16, 12, 10, 1}, {15, 14, 6, 3, 3}, {2, 1, 4, 1, 1}, {15, 11, 16, 15, 1}, + {24, 24, 6, 2, 2}, {8, 6, 15, 12, 1}, {21, 1, 27, 30, 1}, {17, 14, 10, 16, 3}, {13, 7, 9, 7, 7}, + {22, 19, 17, 17, 1}, {16, 14, 14, 13, 2}, {14, 13, 21, 23, 1}, {18, 15, 2, 7, 2}, {3, 1, 25, 24, 1}, + {24, 7, 20, 14, 7}, {26, 24, 25, 19, 2}, {6, 6, 25, 23, 6}, {15, 15, 24, 17, 7}, {22, 16, 14, 15, 1}, + {17, 17, 25, 23, 1}, {12, 2, 18, 26, 2}, {30, 26, 30, 11, 1}, {22, 16, 8, 14, 5}, {9, 8, 16, 20, 1}, + {4, 2, 14, 13, 2}, {28, 27, 7, 8, 1}, {10, 9, 22, 24, 1}, {14, 13, 16, 18, 3}, {28, 3, 26, 15, 2}, + {12, 10, 15, 15, 1}, {18, 17, 17, 15, 1}, {30, 28, 10, 14, 1}, {30, 28, 14, 30, 1}, {30, 7, 18, 13, 1}, + {3, 2, 19, 20, 1}, {16, 14, 19, 13, 2}, {11, 5, 9, 27, 4}, {16, 15, 19, 15, 2}, {24, 18, 22, 19, 7}, + {12, 12, 17, 12, 1}, {28, 28, 5, 1, 1}, {4, 2, 29, 30, 1}, {27, 27, 11, 8, 1}, {8, 8, 3, 1, 1}, + {15, 15, 10, 8, 3}, {12, 11, 27, 18, 4}, {25, 22, 6, 8, 6}, {15, 15, 3, 2, 2}, {19, 17, 22, 19, 1}, + {24, 24, 21, 16, 2}, {9, 6, 7, 6, 6}, {13, 11, 26, 27, 2}, {24, 19, 10, 12, 4}, {22, 22, 17, 9, 2}, + {17, 14, 14, 11, 1}, {13, 13, 4, 3, 1}, {15, 15, 18, 17, 1}, {29, 29, 30, 24, 1}, {29, 20, 29, 17, 2}, + {6, 2, 12, 27, 2}, {18, 14, 17, 13, 2}, {11, 11, 27, 26, 4}, {22, 3, 12, 18, 3}, {15, 13, 13, 9, 1}, + {12, 7, 20, 18, 1}, {16, 15, 6, 9, 1}, {3, 1, 6, 7, 1}, {12, 11, 17, 19, 1}, {15, 8, 8, 18, 7}, + {11, 11, 19, 5, 3}, {17, 16, 20, 23, 3}, {12, 9, 6, 13, 1}, {2, 1, 1, 2, 1}, {14, 13, 26, 21, 3}, + {25, 16, 16, 14, 3}, {30, 29, 14, 14, 1}, {27, 15, 25, 22, 4}, {13, 8, 10, 7, 2}, {18, 13, 19, 14, 1}, + {28, 28, 28, 22, 3}, {8, 8, 14, 11, 1}, {23, 22, 28, 24, 2}, {8, 3, 2, 18, 2}, {22, 22, 24, 23, 7}, + {20, 15, 17, 16, 1}, {8, 6, 11, 4, 4}, {25, 23, 13, 13, 2}, {18, 16, 18, 15, 1}, {20, 16, 16, 15, 1}, + {18, 14, 20, 26, 3}, {17, 17, 12, 8, 1}, {1, 1, 5, 3, 1}, {22, 13, 13, 20, 2}, {17, 17, 16, 14, 3}, + {27, 25, 17, 17, 2}, {8, 6, 23, 29, 2}, {15, 14, 4, 18, 1}, {10, 10, 24, 17, 4}, {25, 25, 30, 28, 1}, + {3, 1, 22, 29, 1}, {24, 23, 8, 17, 1}, {26, 26, 3, 1, 1}, {18, 18, 22, 17, 2}, {9, 8, 17, 10, 2}, + {29, 29, 22, 2, 2}, {19, 5, 4, 10, 3}, {3, 3, 28, 27, 1}, {12, 11, 15, 18, 1}, {30, 28, 3, 4, 1}, + {7, 7, 9, 8, 1}, {24, 8, 15, 14, 7}, {30, 20, 6, 16, 1}, {18, 1, 18, 10, 1}, {30, 28, 20, 21, 1}, + {15, 13, 15, 14, 1}, {6, 5, 3, 1, 1}, {3, 1, 8, 17, 1}, {3, 2, 2, 2, 2}, {19, 18, 28, 20, 1}, + {20, 20, 20, 17, 2}, {21, 19, 30, 29, 1}, {12, 12, 19, 13, 1}, {29, 29, 10, 4, 2}, {20, 20, 16, 14, 1}, + {15, 11, 9, 16, 2}, {8, 6, 13, 26, 4}, {13, 12, 11, 8, 2}, {17, 17, 27, 26, 4}, {29, 14, 29, 12, 1}, + {29, 28, 2, 3, 2}, {9, 7, 15, 9, 4}, {27, 12, 28, 30, 1}, {14, 2, 30, 28, 1}, {19, 18, 12, 14, 1}, + {26, 24, 5, 15, 5}, {2, 2, 24, 2, 2}, {6, 5, 21, 21, 1}, {22, 9, 16, 17, 2}, {16, 15, 19, 17, 1}, + {2, 2, 29, 28, 2}, {25, 24, 11, 1, 1}, {16, 16, 30, 29, 1}, {14, 14, 20, 17, 3}, {15, 11, 14, 17, 3}, + {18, 16, 17, 21, 1}, {17, 17, 8, 4, 2}, {11, 11, 4, 3, 3}, {25, 9, 16, 17, 6}, {18, 18, 8, 6, 6}, + {17, 17, 22, 19, 1}, {8, 3, 20, 11, 3}, {20, 4, 17, 17, 1}, {29, 12, 12, 19, 2}, {14, 14, 29, 28, 2}, + {12, 10, 18, 18, 1}, {13, 13, 15, 11, 2}, {18, 14, 15, 15, 2}, {19, 17, 17, 19, 1}, {22, 12, 17, 16, 6}, + {30, 29, 22, 18, 1}, {30, 29, 2, 20, 1}, {12, 1, 3, 1, 1}, {4, 1, 7, 7, 1}, {27, 21, 10, 13, 4}, + {18, 18, 21, 13, 3}, {12, 3, 4, 6, 2}, {12, 9, 10, 3, 2}, {3, 2, 28, 29, 2}, {22, 20, 2, 5, 2}, + {27, 20, 18, 3, 3}, {6, 6, 24, 23, 1}, {27, 9, 26, 16, 4}, {5, 5, 18, 11, 5}, {20, 15, 14, 12, 3}, + {19, 19, 16, 15, 1}, {27, 21, 4, 9, 4}, {3, 2, 19, 29, 1}, {20, 18, 24, 22, 1}, {18, 18, 7, 2, 1}, + {28, 28, 30, 28, 1}, {11, 10, 24, 9, 1}, {21, 21, 18, 14, 3}, {27, 26, 19, 18, 2}, {16, 10, 18, 6, 6}, + {11, 5, 18, 19, 1}, {24, 22, 16, 16, 1}, {17, 17, 15, 9, 5}, {27, 20, 29, 11, 2}, {29, 28, 25, 22, 1}, + {21, 21, 11, 5, 1}, {12, 8, 15, 16, 2}, {2, 1, 29, 30, 1}, {18, 4, 12, 21, 3}, {18, 11, 9, 13, 3}, + {18, 10, 3, 21, 3}, {17, 16, 11, 16, 1}, {15, 13, 17, 14, 1}, {7, 7, 7, 5, 5}, {9, 5, 29, 18, 2}, + {10, 10, 11, 6, 6}, {28, 25, 26, 26, 1}, {19, 8, 30, 20, 1}, {8, 7, 15, 29, 2}, {21, 19, 18, 17, 1}, + {2, 1, 22, 22, 1}, {12, 4, 20, 17, 1}, {27, 4, 8, 14, 2}, {26, 25, 10, 13, 1}, {19, 19, 13, 8, 3}, + {12, 7, 16, 18, 7}, {20, 12, 26, 3, 3}, {6, 3, 10, 10, 2}, {25, 25, 25, 21, 2}, {12, 7, 3, 16, 2}, + {8, 4, 4, 17, 4}, {12, 5, 20, 8, 5}, {22, 8, 15, 13, 7}, {12, 12, 13, 8, 2}, {20, 19, 15, 13, 1}, + {30, 29, 5, 8, 1}, {14, 13, 29, 23, 2}, {18, 9, 19, 10, 7}, {2, 1, 11, 10, 1}, {12, 12, 13, 11, 1}, + {27, 9, 15, 5, 4}, {13, 7, 12, 17, 2}, {8, 1, 17, 26, 1}, {20, 11, 24, 12, 4}, {12, 10, 24, 22, 6}, + {19, 14, 29, 20, 1}, {20, 20, 27, 25, 2}, {9, 8, 25, 27, 1}, {7, 5, 11, 11, 1}, {20, 11, 11, 8, 1}, + {9, 9, 8, 5, 1}, {27, 25, 9, 10, 1}, {30, 22, 20, 20, 1}, {26, 26, 21, 20, 1}, {30, 27, 14, 16, 1}, + {12, 11, 16, 19, 3}, {7, 6, 28, 29, 1}, {17, 17, 23, 22, 2}, {12, 2, 17, 2, 1}, {17, 17, 14, 13, 1}, + {18, 16, 12, 16, 1}, {7, 7, 23, 17, 1}, {25, 9, 12, 15, 4}, {16, 16, 6, 5, 5}, {8, 7, 16, 16, 7}, + {6, 5, 7, 7, 5}, {15, 15, 13, 12, 2}, {13, 13, 15, 13, 3}, {16, 16, 12, 11, 1}, {18, 15, 15, 14, 3}, + {17, 14, 8, 5, 4}, {9, 6, 26, 22, 5}, {17, 14, 16, 17, 3}, {25, 24, 1, 2, 1}, {14, 14, 16, 15, 1}, + {24, 4, 22, 23, 4}, {30, 27, 29, 29, 1}, {17, 17, 18, 17, 1}, {19, 19, 30, 28, 1}, {21, 21, 27, 23, 3}, + {16, 15, 18, 20, 1}, {27, 13, 27, 12, 4}, {30, 27, 25, 26, 1}, {4, 3, 21, 7, 1}, {10, 10, 5, 4, 4}, + {14, 5, 14, 3, 1}, {23, 21, 6, 3, 3}, {9, 2, 20, 15, 2}, {23, 20, 9, 13, 1}, {15, 12, 14, 3, 3}, + {19, 19, 25, 18, 4}, {27, 24, 25, 22, 4}, {15, 15, 15, 11, 1}, {17, 14, 16, 13, 1}, {12, 12, 18, 17, 1}, + {30, 30, 3, 2, 1}, {21, 18, 20, 28, 3}, {25, 7, 25, 14, 5}, {3, 2, 11, 3, 2}, {25, 9, 5, 21, 4}, + {6, 4, 15, 28, 3}, {9, 3, 9, 3, 3}, {16, 14, 19, 16, 2}, {10, 10, 25, 20, 1}, {2, 2, 17, 15, 1}, + {17, 15, 15, 16, 1}, {20, 19, 15, 15, 1}, {22, 22, 2, 1, 1}, {15, 15, 19, 18, 1}, {15, 10, 16, 12, 1}, + {28, 23, 2, 14, 2}, {11, 9, 3, 2, 1}, + }; + thresholds_ = { + 14.45, 4.15, 7.75, 9.65, 2.25, 0.15, 0.45, -0.95, 3.65, -1.75, 1.05, 5.45, -0.05, -2.65, + 3.35, 1.65, -1.55, 2.85, 3.05, 22.05, -3.65, -0.15, -0.55, -0.05, 3.25, -2.85, -1.25, 10.45, + -1.05, 1.25, 3.15, 2.05, -0.55, -1.25, 3.45, 4.35, 2.45, 2.55, 1.35, 1.25, 2.75, -0.15, + 8.35, 0.35, -1.25, 0.75, -0.45, -0.05, 0.85, 3.65, 1.55, 1.75, -0.35, 1.35, 4.85, -2.25, + 0.25, -2.15, 3.35, 1.95, 6.25, 0.35, 24.45, -1.15, 0.85, -1.95, 5.15, 1.35, 0.85, 0.35, + -0.45, 4.95, -0.25, 2.45, 11.75, 0.85, -1.65, 0.05, 4.05, -0.35, -0.95, -0.25, 49.35, 0.85, + 1.95, 0.25, 1.15, -2.65, -0.25, 0.85, -5.65, 1.15, 3.05, 4.05, 7.35, 9.05, 0.35, 1.65, + 1.65, 2.55, 6.95, 1.75, 0.85, -1.15, 1.35, 5.45, 1.35, -2.35, -0.15, 0.85, -3.05, 0.05, + -0.35, -0.65, 11.65, -0.85, 1.15, 2.35, -9.25, 16.05, -1.45, 1.95, 0.35, -0.35, 0.15, 0.25, + -1.35, 0.65, -4.05, -2.25, 1.05, -0.05, -1.55, 0.25, 69.45, 0.15, -1.25, -1.85, -0.65, 8.75, + -0.95, 1.95, 1.05, -1.45, 2.15, -1.85, -1.75, -2.95, 2.65, 0.65, -1.05, 4.35, 0.25, 19.65, + 0.35, 1.65, 0.65, 2.75, 2.85, -10.95, 3.65, -4.15, 0.25, -0.65, -2.25, 3.75, 35.75, 3.15, + -0.55, 1.95, -6.55, 88.35, 7.25, 1.75, -4.15, 0.25, 0.25, 0.65, 3.75, 1.65, 2.85, 42.15, + 11.75, -1.25, -8.25, 0.05, 2.15, 0.65, -1.55, -2.75, 1.15, -0.15, -0.75, 16.85, 1.15, 1.45, + 15.35, 19.45, 8.65, -1.25, 0.25, 2.15, 0.15, 2.85, -2.25, 1.75, -15.15, 4.15, -0.45, -8.25, + 1.25, -0.85, 3.55, 0.45, -1.25, 9.65, 1.25, 3.15, -2.05, -6.85, -0.05, 3.25, 7.05, 2.35, + -1.45, -1.05, 20.25, 1.15, 1.35, 1.05, 5.15, 2.55, 0.55, 0.55, -0.15, 0.65, -28.35, 3.85, + 2.05, -1.15, 3.05, -0.95, -1.55, -0.35, 101.95, 0.35, -2.05, -2.15, 4.25, 105.15, 1.15, 27.45, + 30.15, 18.45, -2.45, 1.85, 2.95, -3.75, -0.65, 0.05, 0.25, 34.45, -2.05, 0.25, 3.25, 2.05, + 2.85, 3.45, 20.95, 1.45, -1.95, -0.35, -6.85, 1.15, -31.95, 11.85, 2.75, 0.15, 8.35, 1.15, + 2.15, -1.15, -1.05, 2.45, 1.45, 0.15, -1.65, 41.85, 0.35, 0.65, -0.35, 0.25, 56.75, 23.55, + 88.85, -7.25, 1.05, 0.75, -1.05, -1.65, -9.75, -16.95, -0.05, -10.05, 34.15, 14.75, -0.95, 1.45, + 4.85, -0.05, -0.35, 0.75, 64.75, 24.25, -0.35, -13.65, 2.85, 1.75, -0.35, 0.25, -12.15, 108.05, + -0.35, -3.35, 1.95, -0.35, 21.25, -20.05, 95.85, -1.05, -17.75, -0.55, 2.75, 0.65, 0.75, -1.85, + 2.65, -2.15, 1.05, -0.05, 4.25, 3.75, -13.55, -0.05, 105.95, 0.45, 3.05, 9.35, 79.25, 1.75, + 7.65, -0.45, 0.95, 79.15, 28.05, 0.05, 0.65, -0.35, 0.95, 2.05, 1.15, 2.75, -1.65, 8.65, + 3.55, -0.25, -28.95, -7.35, 122.25, 0.55, 4.75, 1.75, 11.15, 0.15, 2.55, 0.05, -7.95, 51.05, + 6.25, 0.05, -1.15, 0.75, 26.65, -1.25, 1.15, -0.55, 0.05, 5.85, 1.55, 2.25, -1.65, -16.85, + -62.65, -1.25, -1.05, -1.55, 1.35, 0.25, -0.65, 0.85, 17.45, 0.25, -9.35, 34.35, 0.95, -0.95, + -10.05, 1.45, -6.05, 93.25, 3.15, -10.85, -1.65, -0.15, -2.45, -0.35, 0.15, 118.15, -81.85, 4.05, + 1.05, -0.85, 152.35, 0.65, 1.35, -12.35, 95.35, -23.25, -0.75, 75.65, 50.85, 47.85, 2.55, 0.55, + 0.15, 0.25, 38.15, -2.65, -1.95, -1.05, 110.25, 35.25, 2.65, 3.65, 18.75, 89.45, -0.55, 0.35, + 0.25, 105.05, 0.25, 1.25, -34.65, -0.55, -0.15, 0.75, -0.15, -0.15, -94.25, -12.05, -15.35, -1.75, + 31.35, 0.15, -2.15, -0.15, -12.05, -0.35, -0.65, -0.55, 28.95, 39.05, 7.05, 0.65, -1.65, 2.85, + 4.85, -1.85, 1.75, -31.65, 0.05, 13.75, -0.35, 0.35, -5.75, 83.85, 0.95, 27.95, -0.25, -19.05, + 2.45, -13.25, 44.05, 51.85, -0.05, 0.05, 79.35, 111.25, -61.65, 3.05, -69.95, 35.65, 62.05, -0.25, + -1.05, -2.95, 0.95, -0.15, -18.15, -18.65, 74.55, 2.65}; + } else { + // With data augmentation + // box_params_ = {}; + // thresholds_ = {}; + + // Without data augmentation + box_params_ = { + {25, 13, 14, 15, 6}, {16, 14, 15, 11, 1}, {14, 7, 14, 8, 6}, {10, 6, 9, 20, 6}, {13, 13, 26, 19, 5}, + {19, 19, 14, 5, 4}, {16, 15, 19, 13, 2}, {26, 21, 26, 12, 5}, {18, 15, 23, 20, 2}, {12, 10, 15, 20, 1}, + {26, 18, 4, 8, 3}, {8, 2, 21, 29, 2}, {19, 17, 16, 19, 1}, {10, 5, 3, 13, 3}, {16, 10, 10, 14, 1}, + {19, 18, 12, 17, 1}, {21, 21, 26, 19, 5}, {6, 5, 7, 5, 5}, {22, 20, 12, 14, 2}, {14, 13, 12, 17, 1}, + {11, 10, 16, 13, 2}, {7, 7, 23, 17, 3}, {27, 25, 13, 8, 4}, {20, 16, 19, 14, 1}, {27, 24, 10, 16, 2}, + {13, 13, 12, 6, 2}, {14, 13, 18, 23, 1}, {14, 11, 8, 1, 1}, {14, 12, 23, 9, 2}, {6, 2, 19, 13, 2}, + {8, 6, 19, 19, 3}, {18, 17, 28, 25, 3}, {29, 25, 28, 22, 2}, {15, 15, 19, 17, 3}, {23, 19, 21, 19, 1}, + {20, 20, 20, 16, 3}, {29, 25, 4, 8, 2}, {17, 16, 6, 25, 2}, {12, 8, 21, 29, 1}, {14, 9, 15, 17, 2}, + {18, 17, 5, 3, 3}, {21, 18, 12, 10, 1}, {17, 14, 14, 14, 2}, {5, 3, 26, 6, 3}, {16, 15, 13, 14, 1}, + {28, 24, 21, 22, 3}, {13, 13, 12, 10, 1}, {22, 21, 3, 11, 3}, {27, 4, 27, 16, 4}, {12, 7, 13, 10, 1}, + {15, 15, 25, 22, 2}, {19, 18, 10, 12, 1}, {17, 17, 16, 9, 2}, {21, 21, 17, 14, 2}, {13, 12, 19, 16, 1}, + {11, 9, 11, 15, 1}, {15, 14, 26, 28, 3}, {17, 17, 22, 20, 1}, {10, 2, 26, 27, 2}, {28, 26, 12, 23, 3}, + {4, 3, 5, 14, 3}, {17, 17, 7, 4, 3}, {19, 17, 15, 15, 1}, {7, 2, 8, 5, 2}, {22, 19, 15, 14, 2}, + {15, 12, 16, 20, 1}, {13, 12, 19, 20, 1}, {17, 17, 10, 8, 2}, {26, 19, 16, 15, 4}, {9, 8, 14, 20, 2}, + {27, 27, 14, 4, 4}, {17, 15, 14, 9, 1}, {5, 5, 4, 3, 3}, {15, 9, 30, 5, 1}, {7, 7, 25, 23, 6}, + {12, 11, 24, 16, 1}, {20, 20, 29, 20, 2}, {19, 15, 18, 19, 1}, {9, 7, 11, 11, 7}, {27, 26, 26, 15, 4}, + {10, 10, 28, 27, 3}, {8, 8, 12, 6, 3}, {21, 16, 23, 22, 1}, {22, 4, 7, 25, 4}, {17, 16, 19, 15, 1}, + {28, 11, 21, 15, 3}, {15, 15, 3, 2, 2}, {16, 14, 16, 17, 3}, {10, 7, 17, 18, 3}, {12, 12, 18, 15, 1}, + {18, 16, 16, 13, 1}, {20, 19, 16, 15, 1}, {16, 11, 15, 11, 1}, {4, 2, 14, 13, 2}, {29, 27, 18, 17, 2}, + {16, 14, 18, 16, 1}, {23, 22, 29, 27, 2}, {18, 18, 13, 11, 1}, {26, 21, 23, 27, 4}, {18, 17, 22, 18, 1}, + {3, 2, 11, 21, 2}, {13, 13, 18, 9, 3}, {15, 14, 14, 5, 2}, {1, 1, 14, 1, 1}, {29, 5, 2, 9, 2}, + {12, 11, 17, 17, 1}, {13, 12, 10, 25, 4}, {5, 1, 13, 25, 1}, {13, 13, 16, 12, 1}, {16, 16, 23, 12, 1}, + {27, 22, 14, 14, 2}, {29, 27, 29, 27, 2}, {23, 22, 6, 4, 4}, {22, 22, 16, 8, 3}, {14, 11, 1, 9, 1}, + {12, 10, 11, 8, 2}, {24, 7, 19, 16, 7}, {5, 2, 29, 20, 2}, {19, 19, 15, 13, 1}, {15, 8, 18, 24, 2}, + {4, 1, 24, 30, 1}, {17, 17, 30, 26, 1}, {9, 7, 8, 5, 2}, {15, 15, 20, 18, 1}, {27, 14, 5, 26, 4}, + {18, 18, 19, 15, 1}, {24, 9, 14, 12, 1}, {20, 18, 6, 10, 1}, {21, 21, 23, 21, 1}, {19, 6, 17, 6, 6}, + {10, 6, 13, 12, 3}, {30, 27, 10, 14, 1}, {9, 6, 5, 3, 3}, {26, 18, 21, 19, 2}, {23, 23, 5, 4, 4}, + {14, 11, 11, 12, 1}, {18, 16, 13, 13, 1}, {7, 3, 8, 16, 3}, {16, 16, 15, 12, 2}, {25, 24, 20, 25, 3}, + {20, 19, 14, 14, 1}, {12, 12, 29, 5, 1}, {23, 13, 17, 13, 5}, {27, 23, 27, 22, 4}, {11, 11, 4, 3, 3}, + {9, 7, 18, 15, 1}, {18, 18, 17, 14, 1}, {28, 6, 2, 17, 2}, {5, 3, 20, 22, 3}, {30, 30, 30, 2, 1}, + {16, 15, 8, 13, 1}, {15, 14, 16, 13, 1}, {28, 27, 5, 5, 3}, {13, 12, 13, 12, 1}, {7, 6, 8, 7, 6}, + {10, 10, 21, 17, 1}, {11, 3, 17, 30, 1}, {16, 9, 17, 14, 7}, {17, 9, 16, 14, 1}, {14, 13, 29, 27, 2}, + {19, 19, 5, 3, 2}, {18, 14, 16, 14, 1}, {10, 8, 23, 25, 2}, {17, 15, 17, 18, 1}, {16, 16, 22, 16, 6}, + {29, 27, 11, 11, 2}, {13, 7, 9, 11, 1}, {18, 17, 23, 19, 4}, {12, 11, 14, 17, 1}, {13, 11, 23, 18, 2}, + {27, 23, 8, 20, 4}, {18, 18, 18, 11, 4}, {8, 5, 21, 8, 5}, {23, 21, 5, 10, 1}, {16, 16, 16, 12, 1}, + {18, 14, 17, 19, 1}, {16, 16, 27, 24, 2}, {21, 15, 17, 15, 1}, {16, 15, 5, 9, 2}, {24, 1, 16, 30, 1}, + {15, 14, 14, 19, 1}, {19, 12, 12, 14, 2}, {5, 3, 5, 4, 3}, {16, 16, 11, 9, 1}, {16, 6, 9, 18, 6}, + {25, 23, 24, 14, 1}, {5, 5, 26, 17, 5}, {9, 6, 16, 18, 1}, {29, 9, 25, 24, 2}, {25, 24, 22, 30, 1}, + {22, 20, 2, 5, 2}, {27, 25, 1, 11, 1}, {15, 14, 12, 10, 1}, {17, 16, 6, 8, 1}, {28, 23, 8, 7, 3}, + {24, 23, 24, 22, 7}, {7, 5, 18, 20, 3}, {22, 20, 15, 20, 1}, {30, 28, 21, 20, 1}, {3, 2, 18, 18, 2}, + {6, 5, 14, 15, 1}, {15, 15, 18, 16, 1}, {7, 5, 11, 2, 1}, {17, 13, 17, 15, 3}, {12, 7, 15, 15, 5}, + {16, 15, 12, 18, 1}, {14, 14, 26, 25, 5}, {11, 8, 17, 18, 1}, {23, 15, 13, 21, 7}, {10, 10, 9, 2, 2}, + {17, 12, 13, 19, 1}, {20, 19, 25, 22, 1}, {9, 8, 26, 21, 1}, {19, 19, 22, 18, 1}, {8, 3, 15, 12, 1}, + {26, 16, 13, 19, 5}, {24, 21, 12, 13, 1}, {12, 12, 14, 9, 1}, {3, 1, 7, 1, 1}, {16, 15, 9, 3, 3}, + {23, 23, 20, 8, 7}, {24, 22, 16, 15, 1}, {20, 20, 19, 14, 1}, {30, 29, 27, 22, 1}, {27, 4, 17, 16, 4}, + {8, 5, 13, 13, 5}, {19, 10, 8, 16, 3}, {30, 30, 11, 4, 1}, {14, 14, 21, 20, 1}, {14, 13, 11, 13, 1}, + {30, 28, 2, 5, 1}, {17, 12, 29, 24, 2}, {15, 6, 25, 30, 1}, {4, 1, 1, 1, 1}, {12, 5, 16, 20, 5}, + {16, 14, 20, 15, 1}, {6, 6, 17, 9, 3}, {20, 12, 17, 20, 4}, {15, 12, 15, 4, 4}, {28, 22, 20, 21, 3}, + {14, 9, 18, 18, 5}, {26, 23, 1, 5, 1}, {21, 11, 24, 10, 7}, {15, 14, 19, 12, 1}, {27, 11, 29, 16, 1}, + {23, 22, 19, 29, 1}, {2, 2, 30, 29, 1}, {14, 6, 16, 5, 3}, {17, 14, 13, 16, 1}, {19, 15, 14, 16, 1}, + {20, 13, 25, 15, 6}, {19, 11, 18, 12, 5}, {30, 30, 30, 13, 1}, {3, 1, 14, 9, 1}, {20, 1, 17, 18, 1}, + {16, 12, 20, 19, 1}}; + thresholds_ = { + 21.65, 5.65, 4.95, 2.45, 2.25, 0.85, 3.35, 1.75, 4.55, -1.55, 4.55, -5.05, 3.15, 4.85, 9.95, + 1.35, -2.05, -0.15, 1.55, 3.35, 0.25, 0.35, 2.45, 2.75, -1.65, -0.05, -0.75, 0.85, 2.95, -1.65, + -0.05, -0.25, -3.85, -0.05, 3.35, 0.05, -3.55, 2.65, 1.95, 6.35, 0.85, 2.65, 12.45, 0.05, 3.35, + 1.75, -1.05, -1.05, 28.25, 0.35, -0.15, 2.05, 2.55, 0.85, 1.35, 1.15, 1.25, 1.35, 1.85, 3.95, + 0.75, 1.65, -3.15, -6.35, 2.05, -5.15, 1.75, -0.65, -0.65, 1.05, -0.85, 0.85, -0.35, 9.05, 0.75, + -1.75, 0.75, 16.05, 0.35, 0.75, 0.05, 0.05, 3.75, 14.15, -8.95, 67.25, -0.45, 1.65, -1.95, 1.15, + 1.85, 3.95, -1.75, 0.45, -1.55, 1.05, -0.25, -1.05, 3.05, -1.05, 1.95, -0.05, 0.85, 3.05, 34.85, + -0.15, 4.35, -10.65, 2.35, -1.35, 0.05, 1.05, 1.05, -0.15, 0.45, -0.55, 10.45, 1.35, -0.95, 0.45, + -0.85, 1.45, -1.85, 1.65, 2.75, 1.05, 81.45, 3.35, 0.85, 2.65, 9.35, 1.15, 1.35, -1.55, 0.85, + 20.65, 2.05, 12.85, 7.95, 2.25, 0.05, 0.85, 8.75, -8.25, -0.35, 1.65, -3.95, 92.55, 0.55, 0.35, + -0.75, -12.25, 0.55, 1.05, 0.95, 1.15, -43.25, 3.05, 4.35, 7.15, 0.15, 57.95, 4.35, 0.75, 0.05, + 0.05, 5.45, 0.55, 0.95, 20.55, -4.45, 0.75, 4.55, -0.15, 8.65, 42.65, -0.45, -1.25, -1.75, 11.25, + -8.15, 2.85, -2.85, -5.05, 44.65, 1.45, -0.75, 11.85, 2.05, 1.25, 4.45, -1.35, 5.95, 1.35, -2.55, + 5.05, -2.85, 7.35, -1.35, -0.45, 0.45, -11.85, -39.65, 1.65, -0.05, 3.65, -0.35, 0.05, 1.85, -0.45, + -1.75, 3.95, 5.25, -1.05, -11.95, 37.05, -1.15, 1.25, 0.75, -6.05, -1.55, -1.65, 0.85, 0.35, 101.55, + -5.05, 3.65, -2.35, -0.35, -1.65, 0.65, 6.35, 2.85, 5.25, 24.05, 38.15, -1.05, 3.05, 0.35, -16.05, + -1.25, 0.25, 1.95, -0.85, 107.65, -1.55, -0.25, 26.95, 35.95, -4.85, 1.55, 10.85, -7.15, -4.25, -25.15, + 2.75}; + } +} + +void BAD::compute(cv::InputArray _image, + std::vector &keypoints, + cv::OutputArray &_descriptors) { + cv::Mat image = _image.getMat(); + + if (image.empty()) + return; + + if (keypoints.empty()) { + // clean output buffer (it may be reused with "allocated" data) + _descriptors.release(); + return; + } + + assert(image.type() == CV_8UC1); + cv::Mat integral_img; + + // compute the integral image + cv::integral(image, integral_img); + + // Create the output array of descriptors + _descriptors.create(static_cast(keypoints.size()), descriptorSize(), descriptorType()); + + // descriptor storage + cv::Mat descriptors = _descriptors.getMat(); + assert(descriptors.type() == CV_8UC1); + + computeBAD(integral_img, keypoints, descriptors); +} + +void BAD::computeBAD(const cv::Mat &integral_img, + const std::vector &keypoints, + cv::Mat &descriptors) { + assert(!integral_img.empty()); + assert(descriptors.rows == keypoints.size()); + + const int *integral_ptr = integral_img.ptr(); + cv::Size frame_size(integral_img.cols - 1, integral_img.rows - 1); + + /////////////////// Parallel Loop to process descriptors //////////////////// + +#ifndef UPM_BAD_PARALLEL + const cv::Range range(0, keypoints.size()); +#else + cv::parallel_for_(cv::Range(0, static_cast(keypoints.size())), [&](const cv::Range &range){ // NOLINT +#endif + // Get a pointer to the first element in the range + // Get a pointer to the first element in the range + BoxPairParams *box_pair; + float response_fun; + int area_response_fun, kpIdx; + size_t box_pair_idx; + int box1x1, box1y1, box1x2, box1y2, box2x1, box2y1, box2x2, box2y2, bit_idx, side; + uchar byte = 0; + std::vector img_boxes(box_params_.size()); + uchar *d = &descriptors.at(range.start, 0); + + for (kpIdx = range.start; kpIdx < range.end; kpIdx++) { + // Rectify the weak learners coordinates using the keypoint information + // Rectify the weak learners coordinates using the keypoint information + rectifyBoxes(box_params_, img_boxes, keypoints[kpIdx], scale_factor_, patch_size_); + if (isKeypointInTheBorder(keypoints[kpIdx], frame_size, patch_size_, scale_factor_)) { + // Code to process the keypoints in the image margins + for (box_pair_idx = 0; box_pair_idx < box_params_.size(); box_pair_idx++) { + bit_idx = 7 - static_cast(box_pair_idx % 8); + response_fun = computeBadResponse(img_boxes[box_pair_idx], integral_img); + // Set the bit to 1 if the response function is less or equal to the threshod + byte |= (response_fun <= thresholds_[box_pair_idx]) << bit_idx; + // If we filled the byte, save it + if (bit_idx == 0) { + *d = byte; + byte = 0; + d++; + } + } + } else { + // Code to process the keypoints in the image center + box_pair = img_boxes.data(); + for (box_pair_idx = 0; box_pair_idx < box_params_.size(); box_pair_idx++) { + bit_idx = 7 - static_cast(box_pair_idx % 8); + + // For the first box, we calculate its margin coordinates + box1x1 = box_pair->x1 - box_pair->boxRadius; + box1y1 = (box_pair->y1 - box_pair->boxRadius) * integral_img.cols; + box1x2 = box_pair->x1 + box_pair->boxRadius + 1; + box1y2 = (box_pair->y1 + box_pair->boxRadius + 1) * integral_img.cols; + // For the second box, we calculate its margin coordinates + box2x1 = box_pair->x2 - box_pair->boxRadius; + box2y1 = (box_pair->y2 - box_pair->boxRadius) * integral_img.cols; + box2x2 = box_pair->x2 + box_pair->boxRadius + 1; + box2y2 = (box_pair->y2 + box_pair->boxRadius + 1) * integral_img.cols; + side = 1 + (box_pair->boxRadius << 1); + + // Get the difference between the average level of the two boxes + area_response_fun = (integral_ptr[box1y1 + box1x1] // A of Box1 + + integral_ptr[box1y2 + box1x2] // D of Box1 + - integral_ptr[box1y1 + box1x2] // B of Box1 + - integral_ptr[box1y2 + box1x1] // C of Box1 + - integral_ptr[box2y1 + box2x1] // A of Box2 + - integral_ptr[box2y2 + box2x2] // D of Box2 + + integral_ptr[box2y1 + box2x2] // B of Box2 + + integral_ptr[box2y2 + box2x1]); // C of Box2 + + // Set the bit to 1 if the response function is less or equal to the threshod + byte |= (area_response_fun <= (thresholds_[box_pair_idx] * (side * side))) << bit_idx; + box_pair++; + // If we filled the byte, save it + if (bit_idx == 0) { + *d = byte; + byte = 0; + d++; + } + } // End of for each dimension + } // End of else (of pixels in the image center) + } // End of for each keypoint +#ifdef UPM_BAD_PARALLEL + }); +#endif + } // namespace upm ## NOLINT +} diff --git a/localization/interest_point/src/essential.cc b/localization/interest_point/src/essential.cc index 7fae3debf0..9382ec9bb9 100644 --- a/localization/interest_point/src/essential.cc +++ b/localization/interest_point/src/essential.cc @@ -38,7 +38,7 @@ bool interest_point::RobustEssential(Eigen::Matrix3d const& k1, Eigen::Matrix3d std::pair const& size1, std::pair const& size2, double * error_max, - double precision) { + double precision, const int ransac_iterations) { CHECK(e) << "Missing e argument"; CHECK(vec_inliers) << "Missing vec inliers argument"; @@ -53,7 +53,7 @@ bool interest_point::RobustEssential(Eigen::Matrix3d const& k1, Eigen::Matrix3d x2, size2.first, size2.second, k1, k2); std::pair ransac_output = - openMVG::robust::ACRANSAC(kernel, *vec_inliers, 4096 /* iterations */, + openMVG::robust::ACRANSAC(kernel, *vec_inliers, ransac_iterations /* iterations */, e, precision, false); *error_max = ransac_output.first; diff --git a/localization/interest_point/src/matching.cc b/localization/interest_point/src/matching.cc index 37ea1ab808..6d9d351cf4 100644 --- a/localization/interest_point/src/matching.cc +++ b/localization/interest_point/src/matching.cc @@ -16,8 +16,9 @@ * under the License. */ -#include +#include #include +#include #include #include @@ -31,10 +32,10 @@ // same settings! // TODO(oalexan1): Ideally the settings used here must be saved in the // map file, for the localize executable to read them from there. -DEFINE_int32(hamming_distance, 90, +DEFINE_int32(hamming_distance, 85, "A smaller value keeps fewer but more reliable binary descriptor matches."); DEFINE_double(goodness_ratio, 0.8, - "A smaller value keeps fewer but more reliable float descriptor matches."); + "A smaller value keeps fewer but more reliable descriptor matches."); DEFINE_int32(orgbrisk_octaves, 4, "Number of octaves, or scale spaces, that BRISK will evaluate."); DEFINE_double(orgbrisk_pattern_scale, 1.0, @@ -54,14 +55,14 @@ DEFINE_double(default_surf_threshold, 10, "Default threshold for feature detection using SURF."); DEFINE_double(max_surf_threshold, 1000, "Maximum threshold for feature detection using SURF."); -// ORGBRISK detector -DEFINE_int32(min_brisk_features, 400, +// Binary detector +DEFINE_int32(min_brisk_features, 1000, "Minimum number of features to be computed using ORGBRISK."); -DEFINE_int32(max_brisk_features, 800, +DEFINE_int32(max_brisk_features, 5000, "Maximum number of features to be computed using ORGBRISK."); -DEFINE_double(min_brisk_threshold, 20, +DEFINE_double(min_brisk_threshold, 10, "Minimum threshold for feature detection using ORGBRISK."); -DEFINE_double(default_brisk_threshold, 90, +DEFINE_double(default_brisk_threshold, 20, "Default threshold for feature detection using ORGBRISK."); DEFINE_double(max_brisk_threshold, 110, "Maximum threshold for feature detection using ORGBRISK."); @@ -72,7 +73,7 @@ namespace interest_point { double min_thresh, double default_thresh, double max_thresh): min_features_(min_features), max_features_(max_features), max_retries_(max_retries), min_thresh_(min_thresh), default_thresh_(default_thresh), max_thresh_(max_thresh), - dynamic_thresh_(default_thresh) {} + dynamic_thresh_(default_thresh), last_keypoint_count_(0) {} void DynamicDetector::GetDetectorParams(int & min_features, int & max_features, int & max_retries, double & min_thresh, double & default_thresh, @@ -92,6 +93,7 @@ namespace interest_point { for (unsigned int i = 0; i < max_retries_; i++) { keypoints->clear(); DetectImpl(image, keypoints); + last_keypoint_count_ = keypoints->size(); if (keypoints->size() < min_features_) TooFew(); else if (keypoints->size() > max_features_) @@ -175,6 +177,57 @@ namespace interest_point { cv::Ptr surf_; }; + class TeblidDynamicDetector : public DynamicDetector { + public: + TeblidDynamicDetector(int min_features, int max_features, int max_retries, + double min_thresh, double default_thresh, double max_thresh, bool use_512 = true) + : DynamicDetector(min_features, max_features, max_retries, + min_thresh, default_thresh, max_thresh) { + if (use_512) { + teblid_ = upm::BAD::create(5.0, upm::BAD::SIZE_512_BITS); + } else { + teblid_ = upm::BAD::create(5.0, upm::BAD::SIZE_256_BITS); + } + Reset(); + } + + void Reset(void) { + brisk_ = interest_point::BRISK::create(dynamic_thresh_, FLAGS_orgbrisk_octaves, + FLAGS_orgbrisk_pattern_scale); + } + + virtual void DetectImpl(const cv::Mat& image, std::vector* keypoints) { + // std::cout << "detect max thresh: " << max_thresh_ << ", min: " << min_thresh_ << ", def: " << default_thresh_ + // << ", min feat: " << min_features_ << ", max_feat: " << max_features_ << ", dyn thresh: " << dynamic_thresh_ << + // std::endl; + brisk_->detect(image, *keypoints); + } + virtual void ComputeImpl(const cv::Mat& image, std::vector* keypoints, + cv::Mat* keypoints_description) { + teblid_->compute(image, *keypoints, *keypoints_description); + } + virtual void TooMany(void) { + dynamic_thresh_ *= 1.1; + dynamic_thresh_ = static_cast(dynamic_thresh_); // for backwards compatibility + if (dynamic_thresh_ > max_thresh_) + dynamic_thresh_ = max_thresh_; + brisk_->setThreshold(dynamic_thresh_); + } + virtual void TooFew(void) { + dynamic_thresh_ *= 0.9; + dynamic_thresh_ = static_cast(dynamic_thresh_); // for backwards compatibility + if (dynamic_thresh_ < min_thresh_) + dynamic_thresh_ = min_thresh_; + brisk_->setThreshold(dynamic_thresh_); + } + + private: + cv::Ptr teblid_; + cv::Ptr brisk_; + }; + + + FeatureDetector::FeatureDetector(std::string const& detector_name, int min_features, int max_features, int retries, double min_thresh, double default_thresh, double max_thresh) { @@ -218,7 +271,8 @@ namespace interest_point { min_thresh = FLAGS_min_surf_threshold; default_thresh = FLAGS_default_surf_threshold; max_thresh = FLAGS_max_surf_threshold; - } else if (detector_name == "ORGBRISK") { + } else if (detector_name == "ORGBRISK" || detector_name == "TEBLID512" || + detector_name == "TEBLID256") { min_features = FLAGS_min_brisk_features; max_features = FLAGS_max_brisk_features; retries = FLAGS_detection_retries; @@ -237,8 +291,16 @@ namespace interest_point { else if (detector_name == "SURF") detector_ = new SurfDynamicDetector(min_features, max_features, retries, min_thresh, default_thresh, max_thresh); + else if (detector_name == "TEBLID512") + detector_ = new TeblidDynamicDetector(min_features, max_features, retries, + min_thresh, default_thresh, max_thresh, true); + else if (detector_name == "TEBLID256") + detector_ = new TeblidDynamicDetector(min_features, max_features, retries, + min_thresh, default_thresh, max_thresh, false); else LOG(FATAL) << "Unimplemented feature detector: " << detector_name; + + LOG(INFO) << "Using descriptor: " << detector_name; } void FeatureDetector::Detect(const cv::Mat& image, @@ -256,12 +318,16 @@ namespace interest_point { } } - void FindMatches(const cv::Mat & img1_descriptor_map, - const cv::Mat & img2_descriptor_map, std::vector * matches) { + void FindMatches(const cv::Mat& img1_descriptor_map, const cv::Mat& img2_descriptor_map, + std::vector* matches, boost::optional hamming_distance, + boost::optional goodness_ratio) { CHECK(img1_descriptor_map.depth() == img2_descriptor_map.depth()) << "Mixed descriptor types. Did you mash BRISK with SIFT/SURF?"; + if (!hamming_distance) hamming_distance = FLAGS_hamming_distance; + if (!goodness_ratio) goodness_ratio = FLAGS_goodness_ratio; + // Check for early exit conditions matches->clear(); if (img1_descriptor_map.rows == 0 || @@ -270,22 +336,23 @@ namespace interest_point { if (img1_descriptor_map.depth() == CV_8U) { // Binary descriptor - - // cv::BFMatcher matcher(cv::NORM_HAMMING, true /* Forward & Backward matching */); cv::FlannBasedMatcher matcher(cv::makePtr(3, 18, 2)); - matcher.match(img1_descriptor_map, img2_descriptor_map, *matches); - - // Select only inlier matches that meet a BRISK threshold of - // of FLAGS_hamming_distance. - // TODO(oalexan1) This needs further study. - std::vector inlier_matches; - inlier_matches.reserve(matches->size()); // This saves time in allocation - for (cv::DMatch const& dmatch : *matches) { - if (dmatch.distance < FLAGS_hamming_distance) { - inlier_matches.push_back(dmatch); + std::vector > possible_matches; + matcher.knnMatch(img1_descriptor_map, img2_descriptor_map, possible_matches, 2); + matches->clear(); + matches->reserve(possible_matches.size()); + for (std::vector const& best_pair : possible_matches) { + if (best_pair.size() == 0 || best_pair.at(0).distance > *hamming_distance) continue; + if (best_pair.size() == 1) { + // This was the only best match, push it. + matches->push_back(best_pair.at(0)); + } else { + // Push back a match only if it is a certain percent better than the next best. + if (best_pair.at(0).distance < *goodness_ratio * best_pair.at(1).distance) { + matches->push_back(best_pair[0]); + } } } - matches->swap(inlier_matches); // Doesn't invoke a copy of all elements. } else { // Traditional floating point descriptor cv::FlannBasedMatcher matcher; @@ -299,7 +366,7 @@ namespace interest_point { matches->push_back(best_pair.at(0)); } else { // Push back a match only if it is 25% better than the next best. - if (best_pair.at(0).distance < FLAGS_goodness_ratio * best_pair.at(1).distance) { + if (best_pair.at(0).distance < *goodness_ratio * best_pair.at(1).distance) { matches->push_back(best_pair[0]); } } diff --git a/localization/localization_node/CMakeLists.txt b/localization/localization_node/CMakeLists.txt index fd16021ae7..94ad104b2b 100644 --- a/localization/localization_node/CMakeLists.txt +++ b/localization/localization_node/CMakeLists.txt @@ -32,6 +32,7 @@ find_package(catkin2 REQUIRED COMPONENTS ff_util sparse_mapping msg_conversions + localization_common ) # System dependencies are found with CMake's conventions @@ -59,6 +60,7 @@ catkin_package( msg_conversions ff_util nodelet + localization_common ) diff --git a/localization/localization_node/include/localization_node/localization.h b/localization/localization_node/include/localization_node/localization.h index ccfd67b7f0..d4a0837f45 100644 --- a/localization/localization_node/include/localization_node/localization.h +++ b/localization/localization_node/include/localization_node/localization.h @@ -26,17 +26,34 @@ #include #include +#include + namespace localization_node { +struct ThresholdParams { + int success_history_size; + double min_success_rate; + double max_success_rate; + int min_features; + int max_features; + bool adjust_hamming_distance; + int min_hamming_distance; + int max_hamming_distance; +}; + class Localizer { public: - explicit Localizer(sparse_mapping::SparseMap* comp_map_ptr); - ~Localizer(); - void ReadParams(config_reader::ConfigReader* config); + explicit Localizer(sparse_mapping::SparseMap* map); + void ReadParams(config_reader::ConfigReader& config); bool Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLandmarks* vl, Eigen::Matrix2Xd* image_keypoints = NULL); private: + void AdjustThresholds(); + sparse_mapping::SparseMap* map_; + // Success params for adjusting keypoint thresholds + std::deque successes_; + ThresholdParams params_; }; }; // namespace localization_node diff --git a/localization/localization_node/package.xml b/localization/localization_node/package.xml index 8842c24650..72c1597176 100644 --- a/localization/localization_node/package.xml +++ b/localization/localization_node/package.xml @@ -27,6 +27,7 @@ sparse_mapping msg_conversions ff_util + localization_common roscpp message_runtime std_msgs @@ -40,6 +41,7 @@ sparse_mapping msg_conversions ff_util + localization_common diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc index 1f8a0f7f7b..574fc5c614 100644 --- a/localization/localization_node/src/localization.cc +++ b/localization/localization_node/src/localization.cc @@ -27,55 +27,74 @@ namespace localization_node { -Localizer::Localizer(sparse_mapping::SparseMap* comp_map_ptr) : - map_(comp_map_ptr) { -} - -Localizer::~Localizer(void) { -} +Localizer::Localizer(sparse_mapping::SparseMap* map): map_(map) {} + +void Localizer::ReadParams(config_reader::ConfigReader& config) { + camera::CameraParameters cam_params(&config, "nav_cam"); + std::string prefix; + const auto detector_name = map_->GetDetectorName(); + if (detector_name == "ORGBRISK") { + prefix = "brisk_"; + } else if (detector_name == "TEBLID512") { + prefix = "teblid512_"; + } else if (detector_name == "TEBLID256") { + prefix = "teblid256_"; + } else { + ROS_FATAL_STREAM("Invalid detector: " << detector_name); + } -void Localizer::ReadParams(config_reader::ConfigReader* config) { - int num_similar, ransac_inlier_tolerance, ransac_iterations, early_break_landmarks, histogram_equalization; + // Loc params + sparse_mapping::LocalizationParameters loc_params; + LOAD_PARAM(loc_params.num_similar, config, prefix); + LOAD_PARAM(loc_params.min_query_score_ratio, config, prefix); + LOAD_PARAM(loc_params.ransac_inlier_tolerance, config, prefix); + LOAD_PARAM(loc_params.num_ransac_iterations, config, prefix); + LOAD_PARAM(loc_params.early_break_landmarks, config, prefix); + LOAD_PARAM(loc_params.histogram_equalization, config, prefix); + LOAD_PARAM(loc_params.check_essential_matrix, config, prefix); + LOAD_PARAM(loc_params.essential_ransac_iterations, config, prefix); + LOAD_PARAM(loc_params.add_similar_images, config, prefix); + LOAD_PARAM(loc_params.add_best_previous_image, config, prefix); + LOAD_PARAM(loc_params.hamming_distance, config, prefix); + LOAD_PARAM(loc_params.goodness_ratio, config, prefix); + LOAD_PARAM(loc_params.use_clahe, config, prefix); + LOAD_PARAM(loc_params.num_extra_localization_db_images, config, prefix); + LOAD_PARAM(loc_params.verbose_localization, config, ""); + LOAD_PARAM(loc_params.visualize_localization_matches, config, ""); + + // Detector Params + double min_threshold, default_threshold, max_threshold, goodness_ratio; int min_features, max_features, detection_retries; - double min_brisk_threshold, default_brisk_threshold, max_brisk_threshold; - camera::CameraParameters cam_params(config, "nav_cam"); - if (!config->GetInt("num_similar", &num_similar)) - ROS_FATAL("num_similar not specified in localization."); - if (!config->GetInt("ransac_inlier_tolerance", &ransac_inlier_tolerance)) - ROS_FATAL("ransac_inlier_tolerance not specified in localization."); - if (!config->GetInt("ransac_iterations", &ransac_iterations)) - ROS_FATAL("ransac_iterations not specified in localization."); - if (!config->GetInt("min_features", &min_features)) - ROS_FATAL("min_features not specified in localization."); - if (!config->GetInt("max_features", &max_features)) - ROS_FATAL("max_features not specified in localization."); - if (!config->GetInt("detection_retries", &detection_retries)) - ROS_FATAL("detection_retries not specified in localization."); - if (!config->GetInt("histogram_equalization", &histogram_equalization)) - ROS_FATAL("histogram_equalization not specified in localization."); - - // For the brisk thresholds and other values, quietly assume some defaults - if (!config->GetReal("min_brisk_threshold", &min_brisk_threshold)) - min_brisk_threshold = 20.0; - if (!config->GetReal("default_brisk_threshold", &default_brisk_threshold)) - default_brisk_threshold = 90.0; - if (!config->GetReal("max_brisk_threshold", &max_brisk_threshold)) - max_brisk_threshold = 110.0; - if (!config->GetInt("early_break_landmarks", &early_break_landmarks)) - early_break_landmarks = 100; + LOAD_PARAM(min_threshold, config, prefix); + LOAD_PARAM(default_threshold, config, prefix); + LOAD_PARAM(max_threshold, config, prefix); + LOAD_PARAM(detection_retries, config, prefix); + LOAD_PARAM(min_features, config, prefix); + LOAD_PARAM(max_features, config, prefix); + + // Localizer threshold params + LOAD_PARAM(params_.success_history_size, config, prefix); + LOAD_PARAM(params_.min_success_rate, config, prefix); + LOAD_PARAM(params_.max_success_rate, config, prefix); + LOAD_PARAM(params_.min_features, config, prefix); + LOAD_PARAM(params_.max_features, config, prefix); + LOAD_PARAM(params_.adjust_hamming_distance, config, prefix); + LOAD_PARAM(params_.min_hamming_distance, config, prefix); + LOAD_PARAM(params_.max_hamming_distance, config, prefix); // This check must happen before the histogram_equalization flag is set into the map // to compare with what is there already. sparse_mapping::HistogramEqualizationCheck(map_->GetHistogramEqualization(), - histogram_equalization); + loc_params.histogram_equalization); + // Check consistency between clahe params + if (loc_params.use_clahe && (loc_params.histogram_equalization != 3 || map_->GetHistogramEqualization() != 3)) { + ROS_FATAL("Invalid clahe and histogram equalization settings."); + } + map_->SetCameraParameters(cam_params); - map_->SetNumSimilar(num_similar); - map_->SetRansacInlierTolerance(ransac_inlier_tolerance); - map_->SetRansacIterations(ransac_iterations); - map_->SetEarlyBreakLandmarks(early_break_landmarks); - map_->SetHistogramEqualization(histogram_equalization); + map_->SetLocParams(loc_params); map_->SetDetectorParams(min_features, max_features, detection_retries, - min_brisk_threshold, default_brisk_threshold, max_brisk_threshold); + min_threshold, default_threshold, max_threshold); } bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLandmarks* vl, @@ -99,10 +118,14 @@ bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLa std::vector landmarks; std::vector observations; if (!map_->Localize(image_descriptors, *image_keypoints, - &camera, &landmarks, &observations)) { + &camera, &landmarks, &observations, nullptr, image_ptr->image)) { + successes_.emplace_back(0); + AdjustThresholds(); // LOG(INFO) << "Failed to localize image."; return false; } + successes_.emplace_back(1); + AdjustThresholds(); Eigen::Affine3d global_pose = camera.GetTransform().inverse(); Eigen::Quaterniond quat(global_pose.rotation()); @@ -125,4 +148,35 @@ bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLa return true; } -}; // namespace localization_node +void Localizer::AdjustThresholds() { + if (successes_.size() < params_.success_history_size) return; + while (successes_.size() > params_.success_history_size) { + successes_.pop_front(); + } + const double average = + std::accumulate(successes_.cbegin(), successes_.cend(), 0) / static_cast(successes_.size()); + const int last_keypoint_count = map_->detector().dynamic_detector().last_keypoint_count(); + if (average < params_.min_success_rate) { + if (last_keypoint_count < params_.max_features) { + map_->detector().dynamic_detector().TooFew(); + } else if (params_.adjust_hamming_distance) { + const int current_hamming_distance = map_->loc_params().hamming_distance; + const int new_hamming_distance = current_hamming_distance + 3; + if (new_hamming_distance <= params_.max_hamming_distance) { + map_->loc_params().hamming_distance = new_hamming_distance; + } + } + } + if (average > params_.max_success_rate) { + if (last_keypoint_count > params_.min_features) { + map_->detector().dynamic_detector().TooMany(); + } else if (params_.adjust_hamming_distance) { + const int current_hamming_distance = map_->loc_params().hamming_distance; + const int new_hamming_distance = current_hamming_distance - 3; + if (new_hamming_distance >= params_.min_hamming_distance) { + map_->loc_params().hamming_distance = new_hamming_distance; + } + } + } +} +} // namespace localization_node diff --git a/localization/localization_node/src/localization_nodelet.cc b/localization/localization_node/src/localization_nodelet.cc index f1dfcf4495..bb1a9f9f25 100644 --- a/localization/localization_node/src/localization_nodelet.cc +++ b/localization/localization_node/src/localization_nodelet.cc @@ -72,7 +72,9 @@ void LocalizationNodelet::Initialize(ros::NodeHandle* nh) { config_.AddFile("cameras.config"); config_.AddFile("localization.config"); - config_.ReadFiles(); + if (!config_.ReadFiles()) { + ROS_FATAL("Failed to read config files."); + } // Resolve the full path to the AR tag file specified for the current world std::string map_file; @@ -125,11 +127,7 @@ void LocalizationNodelet::Initialize(ros::NodeHandle* nh) { } void LocalizationNodelet::ReadParams(void) { - if (!config_.ReadFiles()) { - ROS_ERROR("Failed to read config files."); - return; - } - if (inst_) inst_->ReadParams(&config_); + if (inst_) inst_->ReadParams(config_); } bool LocalizationNodelet::EnableService(ff_msgs::SetBool::Request & req, ff_msgs::SetBool::Response & res) { diff --git a/localization/sparse_mapping/CMakeLists.txt b/localization/sparse_mapping/CMakeLists.txt index 92874609e2..677d2871a9 100644 --- a/localization/sparse_mapping/CMakeLists.txt +++ b/localization/sparse_mapping/CMakeLists.txt @@ -79,6 +79,7 @@ add_library(sparse_mapping src/sparse_map.cc src/sparse_mapping.cc src/tensor.cc + src/visualization_utilities.cc src/vocab_tree.cc ${PROTO_SRCS} ) diff --git a/localization/sparse_mapping/build_map_from_multiple_bags.md b/localization/sparse_mapping/build_map_from_multiple_bags.md index 8352389a3c..7659a23c1e 100644 --- a/localization/sparse_mapping/build_map_from_multiple_bags.md +++ b/localization/sparse_mapping/build_map_from_multiple_bags.md @@ -51,13 +51,13 @@ First copy the map that should be registered then run: The creation of registration points is detailed in `build_map.md`, images from the map are used to manually select feature locations and 3D points for these features are selected using a 3D model of the mapped environment provided externally. -### 6. Build BRISK map for localization +### 6. Build TEBLID512 map for localization Use: -`rosrun sparse_mapping make_brisk_map.py surf_map_name -r robot_name -w world_name -d map_directory` -to rebuild the SURF map with BRISK features for use with localization. +`rosrun sparse_mapping make_teblid512_map.py surf_map_name -r robot_name -w world_name -d map_directory` +to rebuild the SURF map with TEBLID512 features for use with localization. -### 7. Verify BRISK map using localization +### 7. Verify TEBLID512 map using localization Use: `rosrun sparse_mapping run_graph_bag_and_plot_results bag_name brisk_map_name config_path --generate-image-features -r robot_config -w world_name` to test the map using localization. diff --git a/localization/sparse_mapping/include/sparse_mapping/localization_parameters.h b/localization/sparse_mapping/include/sparse_mapping/localization_parameters.h new file mode 100644 index 0000000000..b261f81c27 --- /dev/null +++ b/localization/sparse_mapping/include/sparse_mapping/localization_parameters.h @@ -0,0 +1,43 @@ +/* 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. + */ + +#ifndef SPARSE_MAPPING_LOCALIZATION_PARAMETERS_H_ +#define SPARSE_MAPPING_LOCALIZATION_PARAMETERS_H_ + +namespace sparse_mapping { +struct LocalizationParameters { + int num_similar; + double min_query_score_ratio; + int ransac_inlier_tolerance; + int num_ransac_iterations; + int early_break_landmarks; + int histogram_equalization; + bool check_essential_matrix; + int essential_ransac_iterations; + bool add_similar_images; + bool add_best_previous_image; + int hamming_distance; + double goodness_ratio; + bool use_clahe; + int num_extra_localization_db_images; + bool verbose_localization; + bool visualize_localization_matches; +}; +} // namespace sparse_mapping + +#endif // SPARSE_MAPPING_LOCALIZATION_PARAMETERS_H_ diff --git a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h index 1fa91d79f7..c33d588d9e 100644 --- a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h +++ b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h @@ -23,11 +23,14 @@ #include #include #include +#include #include #include +#include #include #include +#include #include #include @@ -51,30 +54,6 @@ void InitializeCidFidToPid(int num_cid, std::vector > const& pid_to_cid_fid, std::vector > * cid_fid_to_pid); -/** - * Estimate the camera pose for a set of image descriptors and keypoints. - * Non-member function. We will invoke it both from within - * the SparseMap class and from outside of it. - **/ -bool Localize(cv::Mat const& test_descriptors, - Eigen::Matrix2Xd const& test_keypoints, - camera::CameraParameters const& camera_params, - camera::CameraModel* pose, - std::vector* inlier_landmarks, - std::vector* inlier_observations, - int num_cid, - std::string const& detector_name, - sparse_mapping::VocabDB * vocab_db, - int num_similar, - std::vector const& cid_to_filename, - std::vector const& cid_to_descriptor_map, - std::vector const& cid_to_keypoint_map, - std::vector > const& cid_fid_to_pid, - std::vector const& pid_to_xyz, - int num_ransac_iterations, int ransac_inlier_tolerance, - int early_break_landmarks, int histogram_equalization, - std::vector * cid_list); - /** * A class representing a sparse map, which consists of a collection * of keyframes and detected features. To localize, an image's features @@ -112,6 +91,14 @@ struct SparseMap { SparseMap(bool bundler_format, std::string const& filename, std::vector const& files); + // Set default loc params using FLAG values + void SetDefaultLocParams(); + + // Set loc params + void SetLocParams(const LocalizationParameters& loc_params); + const LocalizationParameters& loc_params() const { return loc_params_; } + LocalizationParameters& loc_params() { return loc_params_; } + void SetDetectorParams(int min_features, int max_features, int retries, double min_thresh, double default_thresh, double max_thresh); @@ -136,11 +123,10 @@ struct SparseMap { camera::CameraModel* pose, std::vector* inlier_landmarks = NULL, std::vector* inlier_observations = NULL, std::vector * cid_list = NULL); - bool Localize(const cv::Mat & test_descriptors, const Eigen::Matrix2Xd & test_keypoints, - camera::CameraModel* pose, - std::vector* inlier_landmarks, - std::vector* inlier_observations, - std::vector * cid_list = NULL); + bool Localize(cv::Mat const& test_descriptors, Eigen::Matrix2Xd const& test_keypoints, camera::CameraModel* pose, + std::vector* inlier_landmarks, std::vector* inlier_observations, + std::vector* cid_list = NULL, const cv::Mat& image = cv::Mat()); + // access map frames /** * Get the number of keyframes in the map. @@ -188,30 +174,6 @@ struct SparseMap { const std::map & GetLandmarkCidToFidMap(int landmark) const {return pid_to_cid_fid_[landmark];} // access and modify parameters - /** - * Set the number of RANSAC iterations. - **/ - void SetRansacIterations(int iterations) {num_ransac_iterations_ = iterations;} - /** - * Return the number of RANSAC iterations. - **/ - int GetRansacIterations(void) const {return num_ransac_iterations_;} - /** - * Set the RANSAC inlier tolerance, the number of pixels an inlier - * feature is allowed to be off by. - **/ - void SetRansacInlierTolerance(int tolerance) {ransac_inlier_tolerance_ = tolerance;} - /** - * Get the RANSAC inlier tolerance, the number of pixels an inlier - * feature is allowed to be off by. - **/ - int GetRansacInlierTolerance(void) const {return ransac_inlier_tolerance_;} - /** - * Set the number of early break landmarks, when to stop in adding landmarks when localizing. - **/ - void SetEarlyBreakLandmarks(int early_break_landmarks) {early_break_landmarks_ = early_break_landmarks;} - void SetHistogramEqualization(int histogram_equalization) {histogram_equalization_ = histogram_equalization;} - int GetHistogramEqualization() {return histogram_equalization_;} /** * Return the parameters of the camera used to construct the map. **/ @@ -240,6 +202,11 @@ struct SparseMap { // needed for localization. void Load(const std::string & protobuf_file, bool localization = false); + // Optionally load keypoints. Call this after loading the map, if for example + // the user needs to use the keypoints for visualization or essential matrix + // estimation and those params differ from the default FLAG values. + void LoadKeypoints(const std::string & protobuf_file); + // construct from pid_to_cid_fid void InitializeCidFidToPid(); @@ -255,12 +222,9 @@ struct SparseMap { // delete feature descriptors with no matching landmark void PruneMap(void); - /** - * Set the number of similar images queried by the VocabDB. - **/ - void SetNumSimilar(int num_similar) {num_similar_ = num_similar;} - std::string GetDetectorName() { return detector_.GetDetectorName(); } + interest_point::FeatureDetector& detector() { return detector_; } + int GetHistogramEqualization() {return loc_params_.histogram_equalization;} // stored in map file std::vector cid_to_filename_; @@ -272,15 +236,13 @@ struct SparseMap { std::vector cid_to_descriptor_map_; // generated on load std::vector > cid_fid_to_pid_; + std::vector> cid_to_matching_cid_counts_; interest_point::FeatureDetector detector_; camera::CameraParameters camera_params_; mutable sparse_mapping::VocabDB vocab_db_; // TODO(oalexan1): Mutable means someone is doing something wrong. - int num_similar_; - int num_ransac_iterations_; - int ransac_inlier_tolerance_; - int early_break_landmarks_; - int histogram_equalization_; + LocalizationParameters loc_params_; + std::string protobuf_file_; // e.g, 10th db image is 3rd image in cid_to_filename_ std::map db_to_cid_map_; @@ -298,6 +260,9 @@ struct SparseMap { std::vector user_cid_to_keypoint_map_; std::vector > user_pid_to_cid_fid_; std::vector user_pid_to_xyz_; + + cv::Ptr clahe_; + boost::optional best_previous_cid_; std::mutex mutex_detector_; private: diff --git a/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h b/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h index f4c5f80fd6..f573acc273 100644 --- a/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h +++ b/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h @@ -69,172 +69,135 @@ namespace sparse_mapping { // cid_to_camera_transform - Index on CID. Contains affine transform // representing camera_t_global. - Eigen::Quaternion slerp_n(std::vector const& W, - std::vector > const& Q); - - bool IsBinaryDescriptor(std::string const& descriptor); - - // Logic for implementing if two histogram equalization flags are compatible - void HistogramEqualizationCheck(int histogram_equalization1, int histogram_equalization2); - - // Writes the NVM control network format. - void WriteNVM(std::vector const& cid_to_keypoint_map, - std::vector const& cid_to_filename, - std::vector > const& pid_to_cid_fid, - std::vector const& pid_to_xyz, - std::vector const& cid_to_cam_t_global, - double focal_length, - std::string const& output_filename); - // Reads the NVM control network format. - void ReadNVM(std::string const& input_filename, - std::vector * cid_to_keypoint_map, - std::vector * cid_to_filename, - std::vector > * pid_to_cid_fid, - std::vector * pid_to_xyz, - std::vector * cid_to_cam_t_global); - - // Adds yaml.gz or .txt extension, depending on descriptor - std::string ImageToFeatureFile(std::string const& image_file, - std::string const& detector_name); - - // The name of the file storing the list of images - std::string DBImagesFile(std::string const& db_name); - - // The name of the matches file - std::string MatchesFile(std::string const& map_file); - - // The name of the essential file - std::string EssentialFile(std::string const& map_file); - - // Write features yaml file - void WriteFeatures(std::string const& detector_name, - std::vector const& keypoints, - cv::Mat const& descriptors, - std::string const& output_filename); - - // Read features yaml file - bool ReadFeatures(std::string const& input_filename, - std::string const& detector_name, - std::vector * keypoints, - cv::Mat * descriptors); - - // Read SIFT features in Lowe's format - int ReadFeaturesSIFT(std::string const& filename, - cv::Mat * descriptors, - std::vector * keypoints); - - // Triangulate metric camera point - // unnormalized point means that the point is: - // [px (image loc) - cx (optical center), py - cy, f (f length in px)] - Eigen::Vector3d - TriangulatePoint(Eigen::Vector3d const& unnormalized_pt1, - Eigen::Vector3d const& unnormalized_pt2, - Eigen::Matrix3d const& cam2_r_cam1, - Eigen::Vector3d const& cam2_t_cam1, - double* error); - - // Decompose Fundamental Matrix into Essential Matrix given known - // Intrinsics Matrix. - void DecomposeFMatIntoEMat(Eigen::Matrix3d const& fundamental, - Eigen::Matrix3d const& intrinsics, - Eigen::Matrix3d * essential); - - // Decompose Essential Matrix into R and T - void DecomposeEMatIntoRT(Eigen::Matrix3d const& essential, - Eigen::Matrix2Xd const& unnormalized_pts1, - Eigen::Matrix2Xd const& unnormalized_pts2, - std::vector const& matches, - double focal_length1, // Camera 1 - double focal_length2, // Camera 2 - Eigen::Matrix3d * cam2_r_cam1, - Eigen::Vector3d * cam2_t_cam1); - - // Utility to return the type of entries in a given matrix - std::string CvMatTypeStr(cv::Mat const& Mat); - - void ListToListMap(std::vector const& big_list, - std::vector const& small_list, - std::map * map); - - void MergePids(int repeat_index, int num_unique, - std::vector > * pid_to_cid_fid); - - void PrintPidStats(std::vector > const& pid_to_cid_fid); - - // Extract control points and the images they correspond to from - // a hugin project file - void ParseHuginControlPoints(std::string const& hugin_file, - std::vector * images, - Eigen::MatrixXd * points); - - // Parse a file having on each line xyz coordinates - void ParseXYZ(std::string const& xyz_file, Eigen::MatrixXd * xyz); - - // Parse a CSV file, with the first line having column names. Return - // the results as columns in an std::map, with the column name being - // the key. We assume all values are numbers (non-numbers are set to - // 0). - void ParseCSV(std::string const& csv_file, - std::map< std::string, std::vector > *cols); - - // save size before protbuf, to save multiple protobufs in one file - bool WriteProtobufTo(const google::protobuf::MessageLite& message, - google::protobuf::io::ZeroCopyOutputStream* rawOutput); +enum HistogramEqualizationType { kNone = 0, kEqualizeHist = 1, kUnknown = 2, kCLAHE = 3 }; + +Eigen::Quaternion slerp_n(std::vector const& W, std::vector > const& Q); + +bool IsBinaryDescriptor(std::string const& descriptor); + +// Logic for implementing if two histogram equalization flags are compatible +void HistogramEqualizationCheck(int histogram_equalization1, int histogram_equalization2); + +// Writes the NVM control network format. +void WriteNVM(std::vector const& cid_to_keypoint_map, std::vector const& cid_to_filename, + std::vector > const& pid_to_cid_fid, std::vector const& pid_to_xyz, + std::vector const& cid_to_cam_t_global, double focal_length, + std::string const& output_filename); +// Reads the NVM control network format. +void ReadNVM(std::string const& input_filename, std::vector* cid_to_keypoint_map, + std::vector* cid_to_filename, std::vector >* pid_to_cid_fid, + std::vector* pid_to_xyz, std::vector* cid_to_cam_t_global); + +// Adds yaml.gz or .txt extension, depending on descriptor +std::string ImageToFeatureFile(std::string const& image_file, std::string const& detector_name); + +// The name of the file storing the list of images +std::string DBImagesFile(std::string const& db_name); + +// The name of the matches file +std::string MatchesFile(std::string const& map_file); + +// The name of the essential file +std::string EssentialFile(std::string const& map_file); + +// Write features yaml file +void WriteFeatures(std::string const& detector_name, std::vector const& keypoints, + cv::Mat const& descriptors, std::string const& output_filename); + +// Read features yaml file +bool ReadFeatures(std::string const& input_filename, std::string const& detector_name, + std::vector* keypoints, cv::Mat* descriptors); + +// Read SIFT features in Lowe's format +int ReadFeaturesSIFT(std::string const& filename, cv::Mat* descriptors, std::vector* keypoints); + +// Triangulate metric camera point +// unnormalized point means that the point is: +// [px (image loc) - cx (optical center), py - cy, f (f length in px)] +Eigen::Vector3d TriangulatePoint(Eigen::Vector3d const& unnormalized_pt1, Eigen::Vector3d const& unnormalized_pt2, + Eigen::Matrix3d const& cam2_r_cam1, Eigen::Vector3d const& cam2_t_cam1, double* error); + +// Decompose Fundamental Matrix into Essential Matrix given known +// Intrinsics Matrix. +void DecomposeFMatIntoEMat(Eigen::Matrix3d const& fundamental, Eigen::Matrix3d const& intrinsics, + Eigen::Matrix3d* essential); + +// Decompose Essential Matrix into R and T +void DecomposeEMatIntoRT(Eigen::Matrix3d const& essential, Eigen::Matrix2Xd const& unnormalized_pts1, + Eigen::Matrix2Xd const& unnormalized_pts2, std::vector const& matches, + double focal_length1, // Camera 1 + double focal_length2, // Camera 2 + Eigen::Matrix3d* cam2_r_cam1, Eigen::Vector3d* cam2_t_cam1); + +// Utility to return the type of entries in a given matrix +std::string CvMatTypeStr(cv::Mat const& Mat); + +void ListToListMap(std::vector const& big_list, std::vector const& small_list, + std::map* map); + +void MergePids(int repeat_index, int num_unique, std::vector >* pid_to_cid_fid); + +void PrintPidStats(std::vector > const& pid_to_cid_fid); + +// Extract control points and the images they correspond to from +// a hugin project file +void ParseHuginControlPoints(std::string const& hugin_file, std::vector* images, Eigen::MatrixXd* points); + +// Parse a file having on each line xyz coordinates +void ParseXYZ(std::string const& xyz_file, Eigen::MatrixXd* xyz); + +// Parse a CSV file, with the first line having column names. Return +// the results as columns in an std::map, with the column name being +// the key. We assume all values are numbers (non-numbers are set to +// 0). +void ParseCSV(std::string const& csv_file, std::map >* cols); - // save size before file, then write file into rawOutput - bool WriteFileTo(const char* filename, +// save size before protbuf, to save multiple protobufs in one file +bool WriteProtobufTo(const google::protobuf::MessageLite& message, google::protobuf::io::ZeroCopyOutputStream* rawOutput); - // read size before protbuf, to save multiple protobufs in one file - bool ReadProtobufFrom(google::protobuf::io::ZeroCopyInputStream* rawInput, - google::protobuf::MessageLite* message); - - // read a size before protobuf, then read a file and write it to filename - bool ReadFileFrom(google::protobuf::io::ZeroCopyInputStream* rawInput, const char* filename); - - // Apply a given transform to the specified xyz points, and adjust - // accordingly the cameras for consistency. - void TransformCamerasAndPoints(Eigen::Affine3d const& A, - std::vector *cid_to_cam_t, - std::vector *xyz); - - // Get the error threshold based on a multiple of a percentile - double GetErrThresh(const std::vector & errors, double factor); - - // Find the maximum angle between n rays intersecting at given - // point. Must compute the camera centers in the global coordinate - // system before calling this function. - double ComputeRaysAngle(int pid, - std::vector > const& pid_to_cid_fid, - std::vector const & cam_ctrs, - std::vector const& pid_to_xyz); - - // Filter points by reprojection error and other criteria - void FilterPID(double reproj_thresh, - camera::CameraParameters const& camera_params, - std::vector const& cid_to_cam_t_global, - std::vector const& cid_to_keypoint_map, - std::vector > * pid_to_cid_fid, - std::vector * pid_to_xyz, - bool print_stats = true, double multiple_of_median = 3.0); - - // Write the BAL format. - bool WriteBAL(const std::string& filename, - camera::CameraParameters const& camera_params, - std::vector > const& pid_to_cid_fid, - std::vector const& pid_to_xyz, - std::vector const& cid_to_cam_t_global, - std::vector const& cid_to_keypoint_map); - - // Given a data sequence having camera pose information for - // a set of timestamps, interpolate those poses at the timestamps - // given in out_time. We assume timestamps are always in increasing values. - void PoseInterpolation(std::vector const& images, - std::vector const& out_time, - std::map< std::string, std::vector > - const& data, - std::vector * cid_to_cam_t, - std::vector * good_images); +// save size before file, then write file into rawOutput +bool WriteFileTo(const char* filename, google::protobuf::io::ZeroCopyOutputStream* rawOutput); + +// read size before protbuf, to save multiple protobufs in one file +bool ReadProtobufFrom(google::protobuf::io::ZeroCopyInputStream* rawInput, google::protobuf::MessageLite* message); + +// read a size before protobuf, then read a file and write it to filename +bool ReadFileFrom(google::protobuf::io::ZeroCopyInputStream* rawInput, const char* filename); + +// Apply a given transform to the specified xyz points, and adjust +// accordingly the cameras for consistency. +void TransformCamerasAndPoints(Eigen::Affine3d const& A, std::vector* cid_to_cam_t, + std::vector* xyz); + +// Get the error threshold based on a multiple of a percentile +double GetErrThresh(const std::vector& errors, double factor); + +// Find the maximum angle between n rays intersecting at given +// point. Must compute the camera centers in the global coordinate +// system before calling this function. +double ComputeRaysAngle(int pid, std::vector > const& pid_to_cid_fid, + std::vector const& cam_ctrs, std::vector const& pid_to_xyz); + +// Filter points by reprojection error and other criteria +void FilterPID(double reproj_thresh, camera::CameraParameters const& camera_params, + std::vector const& cid_to_cam_t_global, + std::vector const& cid_to_keypoint_map, + std::vector >* pid_to_cid_fid, std::vector* pid_to_xyz, + bool print_stats = true, double multiple_of_median = 3.0); + +// Write the BAL format. +bool WriteBAL(const std::string& filename, camera::CameraParameters const& camera_params, + std::vector > const& pid_to_cid_fid, std::vector const& pid_to_xyz, + std::vector const& cid_to_cam_t_global, + std::vector const& cid_to_keypoint_map); + +// Given a data sequence having camera pose information for +// a set of timestamps, interpolate those poses at the timestamps +// given in out_time. We assume timestamps are always in increasing values. +void PoseInterpolation(std::vector const& images, std::vector const& out_time, + std::map > const& data, + std::vector* cid_to_cam_t, std::vector* good_images); } // namespace sparse_mapping diff --git a/localization/sparse_mapping/include/sparse_mapping/tensor.h b/localization/sparse_mapping/include/sparse_mapping/tensor.h index f9bdc0a11b..e72cfc60ef 100644 --- a/localization/sparse_mapping/include/sparse_mapping/tensor.h +++ b/localization/sparse_mapping/include/sparse_mapping/tensor.h @@ -144,6 +144,12 @@ namespace sparse_mapping { void PrintTrackStats(std::vector >const& pid_to_cid_fid, std::string const& step); + // Filter the matches by a geometric constraint. Compute the essential matrix. + void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2Xd const& keypoints2, + std::vector const& matches, camera::CameraParameters const& camera_params, + std::vector* inlier_matches, std::vector* vec_inliers, + Eigen::Matrix3d* essential_matrix, const int ransac_iterations = 4096); + void BuildMapFindEssentialAndInliers(const Eigen::Matrix2Xd & keypoints1, const Eigen::Matrix2Xd & keypoints2, const std::vector & matches, diff --git a/localization/sparse_mapping/include/sparse_mapping/visualization_utilities.h b/localization/sparse_mapping/include/sparse_mapping/visualization_utilities.h new file mode 100644 index 0000000000..9a58b44fe4 --- /dev/null +++ b/localization/sparse_mapping/include/sparse_mapping/visualization_utilities.h @@ -0,0 +1,44 @@ +/* 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. + */ + +#ifndef SPARSE_MAPPING_VISUALIZATION_UTILITIES_H_ +#define SPARSE_MAPPING_VISUALIZATION_UTILITIES_H_ + +#include + +#include +#include +#include + +#include + +namespace sparse_mapping { +// Convert keypoints from matrix to cv::KeyPoints +std::vector Keypoints(const Eigen::Matrix2Xd& keypoints_mat); + +// Convert keypoints from undistorted centered matrix to distorted cv::KeyPoints +std::vector DistortedKeypoints(const Eigen::Matrix2Xd& keypoints_mat, + const camera::CameraParameters& camera_params); + +// View input keypoints, map keypoints, and matches +void ViewMatches(const Eigen::Matrix2Xd& input_keypoints_mat, const Eigen::Matrix2Xd& map_keypoints_mat, + const std::vector& matches, const camera::CameraParameters& camera_params, + const cv::Mat& input_image, const cv::Mat& map_image); +} // namespace sparse_mapping + +#endif // SPARSE_MAPPING_VISUALIZATION_UTILITIES_H_ diff --git a/localization/sparse_mapping/include/sparse_mapping/vocab_tree.h b/localization/sparse_mapping/include/sparse_mapping/vocab_tree.h index 6f6782d82d..4690648b70 100644 --- a/localization/sparse_mapping/include/sparse_mapping/vocab_tree.h +++ b/localization/sparse_mapping/include/sparse_mapping/vocab_tree.h @@ -94,11 +94,8 @@ namespace sparse_mapping { void ResetDB(VocabDB* db); // Query similar images from database - void QueryDB(std::string const& descriptor, - VocabDB * vocab_db, - int num_similar, - cv::Mat const& descriptors, - std::vector * indices); + void QueryDB(std::string const& descriptor, VocabDB* vocab_db, int num_similar, cv::Mat const& descriptors, + std::vector* indices, std::vector* scores); void BuildDBforDBoW2(sparse_mapping::SparseMap* map, std::string const& descriptor, diff --git a/localization/sparse_mapping/scripts/make_brisk_map.py b/localization/sparse_mapping/scripts/make_teblid512_map.py similarity index 72% rename from localization/sparse_mapping/scripts/make_brisk_map.py rename to localization/sparse_mapping/scripts/make_teblid512_map.py index 073dcf4f1c..75766ab10f 100755 --- a/localization/sparse_mapping/scripts/make_brisk_map.py +++ b/localization/sparse_mapping/scripts/make_teblid512_map.py @@ -17,7 +17,7 @@ # License for the specific language governing permissions and limitations # under the License. """ -Generates a new brisk map with a vocab database using the provided surf map. +Generates a new teblid512 map with a vocab database using the provided surf map. """ import argparse @@ -28,7 +28,7 @@ import localization_common.utilities as lu -def make_brisk_map(surf_map, world, robot_name, map_directory=None): +def make_teblid512_map(surf_map, world, robot_name, map_directory=None): # Set environment variables home = os.path.expanduser("~") robot_config_file = os.path.join("config/robots", robot_name + ".config") @@ -38,34 +38,36 @@ def make_brisk_map(surf_map, world, robot_name, map_directory=None): os.environ["ASTROBEE_ROBOT"] = os.path.join(astrobee_path, robot_config_file) os.environ["ASTROBEE_WORLD"] = world - # Convert SURF to BRISK map + # Convert SURF to TEBLID512 map # Get full path to output file to avoid permission errors when running # command in maps directory map_name = lu.basename(surf_map) - build_brisk_map_output_file = os.path.join( - os.getcwd(), "rebuild_map_as_brisk_map.txt" + build_teblid512_map_output_file = os.path.join( + os.getcwd(), "rebuild_map_as_teblid512_map.txt" ) - brisk_map = map_name + ".brisk.map" - shutil.copyfile(surf_map, brisk_map) - brisk_map_full_path = os.path.abspath(brisk_map) + teblid512_map = map_name + ".teblid512.map" + shutil.copyfile(surf_map, teblid512_map) + teblid512_map_full_path = os.path.abspath(teblid512_map) path = os.getcwd() # Change to map directory so relative image locations are correct if necessary if map_directory: os.chdir(map_directory) - build_brisk_map_command = ( - "rosrun sparse_mapping build_map -rebuild -histogram_equalization -output_map " - + brisk_map_full_path + build_teblid512_map_command = ( + "rosrun sparse_mapping build_map -rebuild -histogram_equalization -use_clahe -output_map " + + teblid512_map_full_path + ) + lu.run_command_and_save_output( + build_teblid512_map_command, build_teblid512_map_output_file ) - lu.run_command_and_save_output(build_brisk_map_command, build_brisk_map_output_file) # Change back to original directory so final map is saved there if map_directory: os.chdir(path) # Create vocabdb - brisk_vocabdb_map = map_name + ".brisk.vocabdb.map" - shutil.copyfile(brisk_map, brisk_vocabdb_map) + teblid512_vocabdb_map = map_name + ".teblid512.vocabdb.map" + shutil.copyfile(teblid512_map, teblid512_vocabdb_map) add_vocabdb_command = ( - "rosrun sparse_mapping build_map -vocab_db -output_map " + brisk_vocabdb_map + "rosrun sparse_mapping build_map -vocab_db -output_map " + teblid512_vocabdb_map ) lu.run_command_and_save_output(add_vocabdb_command, "build_vocabdb.txt") @@ -75,7 +77,7 @@ def make_brisk_map(surf_map, world, robot_name, map_directory=None): description=__doc__, formatter_class=argparse.ArgumentDefaultsHelpFormatter ) parser.add_argument( - "surf_map", help="Input SURF map to generate the BRISK map for." + "surf_map", help="Input SURF map to generate the TEBLID512 map for." ) parser.add_argument("-w", "--world", default="iss") parser.add_argument("-r", "--robot-name", default="bumble") @@ -83,7 +85,7 @@ def make_brisk_map(surf_map, world, robot_name, map_directory=None): "-d", "--map-directory", default=None, - help="Location where surf map was created, needed to load images for BRISK map building since relative paths are used during map creation for the image locations. Defaults to current working directory.", + help="Location where surf map was created, needed to load images for TEBLID512 map building since relative paths are used during map creation for the image locations. Defaults to current working directory.", ) args = parser.parse_args() @@ -98,4 +100,4 @@ def make_brisk_map(surf_map, world, robot_name, map_directory=None): surf_map = os.path.abspath(args.surf_map) if args.map_directory: args.map_directory = os.path.abspath(args.map_directory) - make_brisk_map(surf_map, args.world, args.robot_name, args.map_directory) + make_teblid512_map(surf_map, args.world, args.robot_name, args.map_directory) diff --git a/localization/sparse_mapping/src/sparse_map.cc b/localization/sparse_mapping/src/sparse_map.cc index b6fd572b49..1325929338 100644 --- a/localization/sparse_mapping/src/sparse_map.cc +++ b/localization/sparse_mapping/src/sparse_map.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -49,6 +50,10 @@ DEFINE_int32(num_similar, 20, "Use in localization this many images which " "are most similar to the image to localize."); +DEFINE_double(min_query_score_ratio, 0, + "Use in localization as a threshold for including " + "db images. Min ratio between best query and current query" + "db image scores for a map image to be matched with."); DEFINE_int32(num_ransac_iterations, 1000, "Use in localization this many ransac iterations."); DEFINE_int32(ransac_inlier_tolerance, 3, @@ -57,52 +62,55 @@ DEFINE_int32(early_break_landmarks, 100, "Break early when we have this many landmarks during localization."); DEFINE_bool(histogram_equalization, false, "If true, equalize the histogram for images to improve robustness to illumination conditions."); +DEFINE_int32(localization_hamming_distance, 85, + "Used for localization. A smaller value keeps fewer but more reliable binary descriptor matches."); +DEFINE_double(localization_goodness_ratio, 0.8, + "Used for localization. A smaller value keeps fewer but more reliable descriptor matches."); +DEFINE_bool(use_clahe, false, + "If true, use CLAHE if histogram equalization enabled."); DEFINE_int32(num_extra_localization_db_images, 0, "Match this many extra images from the Vocab DB, only keep num_similar."); DEFINE_bool(verbose_localization, false, "If true, list the images most similar to the one being localized."); +DEFINE_bool(visualize_localization_matches, false, + "If true, visualized matches between input image and each available map image during localization."); +DEFINE_bool(localization_check_essential_matrix, true, + "If true, verify a valid essential matrix can be calculated between the input image and each potential map " + "match image before adding map matches."); +DEFINE_bool(localization_add_similar_images, true, + "If true, for each cid matched to, also attempt to match to any cid with at least 5 of the same features " + "as the matched cid."); +DEFINE_bool(localization_add_best_previous_image, true, + "If true, add previous cid with the most matches to list of cids to check for" + "matches with."); namespace sparse_mapping { -SparseMap::SparseMap(const std::vector & filenames, - const std::string & detector, - const camera::CameraParameters & params): - cid_to_filename_(filenames), - detector_(detector), camera_params_(params), - 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) { +SparseMap::SparseMap(const std::vector& filenames, const std::string& detector, + const camera::CameraParameters& params) + : cid_to_filename_(filenames), + detector_(detector), + camera_params_(params) { + SetDefaultLocParams(); cid_to_descriptor_map_.resize(cid_to_filename_.size()); - // TODO(bcoltin): only record scale and orientation for opensift? cid_to_keypoint_map_.resize(cid_to_filename_.size()); } -SparseMap::SparseMap(const std::string & protobuf_file, bool localization) : - camera_params_(Eigen::Vector2i(-1, -1), Eigen::Vector2d::Constant(-1), - Eigen::Vector2d(-1, -1)), - 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) { +SparseMap::SparseMap(const std::string& protobuf_file, bool localization) + : camera_params_(Eigen::Vector2i(-1, -1), Eigen::Vector2d::Constant(-1), Eigen::Vector2d(-1, -1)), + protobuf_file_(protobuf_file) { + SetDefaultLocParams(); // The above camera params used bad values because we are expected to reload // later. Load(protobuf_file, localization); } // Form a sparse map with given cameras/images, and no features -SparseMap::SparseMap(const std::vector& cid_to_cam_t, - const std::vector & filenames, - const std::string & detector, - const camera::CameraParameters & params): - detector_(detector), camera_params_(params), - 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) { +SparseMap::SparseMap(const std::vector& cid_to_cam_t, const std::vector& filenames, + const std::string& detector, const camera::CameraParameters& params) + : detector_(detector), + camera_params_(params) { + SetDefaultLocParams(); if (filenames.size() != cid_to_cam_t.size()) LOG(FATAL) << "Expecting as many images as cameras"; @@ -123,15 +131,10 @@ SparseMap::SparseMap(const std::vector& cid_to_cam_t, // Form a sparse map by reading a text file from disk. This is for comparing // 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) { +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 */ { + SetDefaultLocParams(); std::string ext = ff_common::file_extension(filename); boost::to_lower(ext); @@ -235,6 +238,36 @@ SparseMap::SparseMap(bool bundler_format, std::string const& filename, InitializeCidFidToPid(); } +void SparseMap::SetDefaultLocParams() { + loc_params_.num_similar = FLAGS_num_similar; + loc_params_.min_query_score_ratio = FLAGS_min_query_score_ratio; + loc_params_.num_ransac_iterations = FLAGS_num_ransac_iterations; + loc_params_.ransac_inlier_tolerance = FLAGS_ransac_inlier_tolerance; + loc_params_.early_break_landmarks = FLAGS_early_break_landmarks; + loc_params_.histogram_equalization = FLAGS_histogram_equalization; + loc_params_.use_clahe = FLAGS_use_clahe; + loc_params_.check_essential_matrix = FLAGS_localization_check_essential_matrix; + loc_params_.add_similar_images = FLAGS_localization_add_similar_images; + loc_params_.add_best_previous_image = FLAGS_localization_add_best_previous_image; + loc_params_.hamming_distance = FLAGS_localization_hamming_distance; + loc_params_.goodness_ratio = FLAGS_localization_goodness_ratio; + loc_params_.num_extra_localization_db_images = FLAGS_num_extra_localization_db_images; + loc_params_.verbose_localization = FLAGS_verbose_localization; + loc_params_.visualize_localization_matches = FLAGS_visualize_localization_matches; + if (loc_params_.histogram_equalization && loc_params_.use_clahe) { + clahe_ = cv::createCLAHE(2, cv::Size(8, 8)); + loc_params_.histogram_equalization = HistogramEqualizationType::kCLAHE; + } +} + +void SparseMap::SetLocParams(const LocalizationParameters& loc_params) { + loc_params_ = loc_params; + // Load keypoints if required since these aren't loaded by default for localization + if (!protobuf_file_.empty() && (loc_params_.check_essential_matrix || loc_params_.visualize_localization_matches)) { + LoadKeypoints(protobuf_file_); + } +} + // Detect features in given images void SparseMap::DetectFeatures() { ff_common::ThreadPool pool; @@ -325,6 +358,7 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { else cid_to_filename_[cid] = ""; + // load keypoints if (!localization) cid_to_keypoint_map_[cid].resize(Eigen::NoChange_t(), frame.feature_size()); @@ -344,7 +378,6 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { for (int fid = 0; fid < frame.feature_size(); fid++) { sparse_mapping_protobuf::Feature feature = frame.feature(fid); - // Copy the features if (!localization) cid_to_keypoint_map_[cid].col(fid) << feature.x(), feature.y(); @@ -377,6 +410,8 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { // Create directly cid_fid_to_pid cid_fid_to_pid_.clear(); cid_fid_to_pid_.resize(cid_to_filename_.size(), std::map()); + cid_to_matching_cid_counts_.clear(); + cid_to_matching_cid_counts_.resize(cid_to_filename_.size(), std::map()); } for (int i = 0; i < num_landmarks; i++) { @@ -386,12 +421,24 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { } Eigen::Vector3d pos(l.loc().x(), l.loc().y(), l.loc().z()); pid_to_xyz_[i] = pos; + std::unordered_set cids; for (int j = 0; j < l.match_size(); j++) { sparse_mapping_protobuf::Matching m = l.match(j); - if (!localization) + if (!localization) { pid_to_cid_fid_[i][m.camera_id()] = m.feature_id(); - else + } else { cid_fid_to_pid_[m.camera_id()][m.feature_id()] = i; + cids.emplace(m.camera_id()); + } + } + if (localization) { + // Update matching cid counts + for (int cid : cids) { + for (int matching_cid : cids) { + if (cid == matching_cid) continue; + cid_to_matching_cid_counts_[cid][matching_cid]++; + } + } } } @@ -406,16 +453,16 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { if (map.has_vocab_db()) vocab_db_.LoadProtobuf(input, map.vocab_db()); - histogram_equalization_ = map.histogram_equalization(); + loc_params_.histogram_equalization = map.histogram_equalization(); - assert(histogram_equalization_ == 0 || - histogram_equalization_ == 1 || - histogram_equalization_ == 2); + assert(loc_params_.histogram_equalization >=0 && loc_params_.histogram_equalization <= 3); + loc_params_.use_clahe = loc_params_.histogram_equalization == HistogramEqualizationType::kCLAHE ? true : false; + if (loc_params_.use_clahe) clahe_ = cv::createCLAHE(2, cv::Size(8, 8)); // For backward compatibility with old maps, allow a map to have its // histogram_equalization flag unspecified, but it is best to avoid // that situation, and rebuild the map if necessary. - if (histogram_equalization_ == 2) + if (loc_params_.histogram_equalization == HistogramEqualizationType::kUnknown) std::cout << "Warning: Unknown value of histogram_equalization! " << "It is strongly suggested to rebuild this map to avoid " << "poor quality results." << std::endl; @@ -424,6 +471,41 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { close(input_fd); } +void SparseMap::LoadKeypoints(const std::string & protobuf_file) { + sparse_mapping_protobuf::Map map; + int input_fd = open(protobuf_file.c_str(), O_RDONLY); + if (input_fd < 0) + LOG(FATAL) << "Failed to open map file: " << protobuf_file; + + google::protobuf::io::ZeroCopyInputStream* input = + new google::protobuf::io::FileInputStream(input_fd); + if (!ReadProtobufFrom(input, &map)) { + LOG(FATAL) << "Failed to parse map file."; + } + + int num_frames = map.num_frames(); + cid_to_keypoint_map_.resize(num_frames); + + // load each frame + for (int cid = 0; cid < num_frames; cid++) { + sparse_mapping_protobuf::Frame frame; + if (!ReadProtobufFrom(input, &frame)) { + LOG(FATAL) << "Failed to parse frame."; + } + + // load keypoints + cid_to_keypoint_map_[cid].resize(Eigen::NoChange_t(), frame.feature_size()); + + for (int fid = 0; fid < frame.feature_size(); fid++) { + sparse_mapping_protobuf::Feature feature = frame.feature(fid); + cid_to_keypoint_map_[cid].col(fid) << feature.x(), feature.y(); + } + } + + delete input; + close(input_fd); +} + void SparseMap::SetDetectorParams(int min_features, int max_features, int retries, double min_thresh, double default_thresh, double max_thresh) { mutex_detector_.lock(); @@ -436,7 +518,7 @@ void SparseMap::Save(const std::string & protobuf_file) const { // For backward compatibility with old maps, allow a map to have its // histogram_equalization flag unspecified, but it is best to avoid // that situation, and rebuild the map if necessary. - if (histogram_equalization_ == 2) + if (loc_params_.histogram_equalization == HistogramEqualizationType::kUnknown) std::cout << "Warning: Unknown value of histogram_equalization! " << "It is strongly suggested to rebuild this map to avoid " << "poor quality results." << std::endl; @@ -472,7 +554,7 @@ void SparseMap::Save(const std::string & protobuf_file) const { if (vocab_db_.binary_db != NULL) map.set_vocab_db(sparse_mapping_protobuf::Map::BINARYDB); - map.set_histogram_equalization(histogram_equalization_); + map.set_histogram_equalization(loc_params_.histogram_equalization); LOG(INFO) << "Writing: " << protobuf_file; int output_fd = open(protobuf_file.c_str(), O_WRONLY | O_CREAT | O_TRUNC, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH); @@ -596,14 +678,19 @@ void SparseMap::DetectFeatures(const cv::Mat& image, // If using histogram equalization, need an extra image to store it cv::Mat * image_ptr = const_cast(&image); cv::Mat hist_image; - if (histogram_equalization_) { - cv::equalizeHist(image, hist_image); + if (loc_params_.histogram_equalization) { + cv::setNumThreads(0); + if (loc_params_.use_clahe) { + clahe_->apply(image, hist_image); + } else { + cv::equalizeHist(image, hist_image); + } image_ptr = &hist_image; } #if 0 // This is useful for debugging - std::cout << "Histogram equalization is " << histogram_equalization_ << std::endl; + std::cout << "Histogram equalization is " << loc_params_.histogram_equalization << std::endl; static int count = 10000; count++; std::ostringstream oss; @@ -633,7 +720,7 @@ void SparseMap::DetectFeatures(const cv::Mat& image, } mutex_detector_.unlock(); - if (FLAGS_verbose_localization) + if (loc_params_.verbose_localization) std::cout << "Features detected " << storage.size() << std::endl; keypoints->resize(2, storage.size()); @@ -646,46 +733,47 @@ void SparseMap::DetectFeatures(const cv::Mat& image, } } -// A non-member Localize() function that can be invoked for a non-fully -// formed map. -bool Localize(cv::Mat const& test_descriptors, - Eigen::Matrix2Xd const& test_keypoints, - camera::CameraParameters const& camera_params, - camera::CameraModel* pose, - std::vector* inlier_landmarks, - std::vector* inlier_observations, - int num_cid, - std::string const& detector_name, - sparse_mapping::VocabDB * vocab_db, - int num_similar, - std::vector const& cid_to_filename, - std::vector const& cid_to_descriptor_map, - std::vector const& cid_to_keypoint_map, - std::vector > const& cid_fid_to_pid, - std::vector const& pid_to_xyz, - int num_ransac_iterations, int ransac_inlier_tolerance, - int early_break_landmarks, int histogram_equalization, - std::vector * cid_list) { +bool SparseMap::Localize(cv::Mat const& test_descriptors, Eigen::Matrix2Xd const& test_keypoints, + camera::CameraModel* pose, std::vector* inlier_landmarks, + std::vector* inlier_observations, std::vector* cid_list, + const cv::Mat& image) { std::vector indices; + std::vector query_scores; // Query the vocab tree. if (cid_list == NULL) - sparse_mapping::QueryDB(detector_name, - vocab_db, + sparse_mapping::QueryDB(detector_.GetDetectorName(), + &vocab_db_, // Notice that we request more similar // images than what we need. We'll prune // them below. - num_similar + FLAGS_num_extra_localization_db_images, + loc_params_.num_similar + loc_params_.num_extra_localization_db_images, test_descriptors, - &indices); + &indices, &query_scores); else indices = *cid_list; if (indices.empty()) { LOG(WARNING) << "Localizing against all keyframes as the vocab database is missing."; // Use all images, as no tree is available. + const int num_cid = cid_to_filename_.size(); for (int cid = 0; cid < num_cid; cid++) indices.push_back(cid); } + if (loc_params_.add_best_previous_image && best_previous_cid_) { + bool add_cid = true; + for (const auto cid : indices) { + if (cid == *best_previous_cid_) { + add_cid = false; + break; + } + } + if (add_cid) { + indices.insert(indices.begin(), *best_previous_cid_); + query_scores.insert(query_scores.begin(), query_scores[0]); + } + best_previous_cid_ = boost::none; + } + // To turn on verbose localization for debugging // google::SetCommandLineOption("verbose_localization", "true"); @@ -696,65 +784,155 @@ bool Localize(cv::Mat const& test_descriptors, // We will not localize using all images having matches, there are too // many false positives that way. Instead, limit ourselves to the images // which have most observations in common with the current one. - std::vector similarity_rank(indices.size(), 0); - std::vector > all_matches(indices.size()); + std::vector similarity_rank; + std::vector> all_matches; int total = 0; + if (loc_params_.verbose_localization) { + for (size_t i = 0; i < indices.size(); i++) { + std::cout << "Potential matching cid: " << indices[i] << std::endl; + } + } // TODO(oalexan1): Use multiple threads here? for (size_t i = 0; i < indices.size(); i++) { int cid = indices[i]; - interest_point::FindMatches(test_descriptors, - cid_to_descriptor_map[cid], - &all_matches[i]); + if (loc_params_.verbose_localization) std::cout << "Checking index: " << i << ", cid: " << cid << std::endl; + similarity_rank.emplace_back(0); + all_matches.emplace_back(std::vector()); + if (!query_scores.empty() && query_scores[0] != 0 && + query_scores[i] / static_cast(query_scores[0]) < loc_params_.min_query_score_ratio) { + if (loc_params_.verbose_localization) + std::cout << "Query score ratio too low: " << query_scores[i] / static_cast(query_scores[0]) + << std::endl; + continue; + } + interest_point::FindMatches(test_descriptors, cid_to_descriptor_map_[cid], &all_matches[i], + loc_params_.hamming_distance, loc_params_.goodness_ratio); + + if (loc_params_.visualize_localization_matches && !image.empty()) { + const auto map_filename = cid_to_filename_[cid]; + std::cout << "CID: " << cid << ", filename: " << map_filename << std::endl; + const auto map_image = cv::imread(map_filename, cv::IMREAD_GRAYSCALE); + if (map_image.empty()) { + LOG(ERROR) << "Failed to load map image: " << map_filename; + } else { + ViewMatches(test_keypoints, cid_to_keypoint_map_[cid], all_matches[i], camera_params_, image, map_image); + } + } + + if (all_matches[i].size() < 5) continue; + + if (loc_params_.check_essential_matrix) { + if (loc_params_.verbose_localization) + std::cout << "Matches before essential filtering: " << all_matches[i].size() << std::endl; + std::vector inlier_matches; + std::vector vec_inliers; + Eigen::Matrix3d essential_matrix; + FindEssentialAndInliers(test_keypoints, cid_to_keypoint_map_[cid], all_matches[i], camera_params_, + &inlier_matches, &vec_inliers, &essential_matrix, + loc_params_.essential_ransac_iterations); + all_matches[i] = inlier_matches; + if (loc_params_.verbose_localization) + std::cout << "Matches after essential filtering: " << all_matches[i].size() << std::endl; + } + + if (all_matches[i].size() < 5) continue; + + if (loc_params_.add_similar_images) { + if (loc_params_.verbose_localization) + std::cout << "Num indices before adding similar images: " << indices.size() << std::endl; + // Add cids with more than 5 of the same features as the current cid to the list of cids to match to. + int index = i + 1; + for (auto matching_cid_it = cid_to_matching_cid_counts_[cid].rbegin(); + matching_cid_it != cid_to_matching_cid_counts_[cid].rend() && matching_cid_it->second > 5; + ++matching_cid_it) { + const int matching_cid = matching_cid_it->first; + bool add_cid = true; + // Make sure new matching cid isn't already in indices list + for (const auto already_added_cid : indices) { + if (already_added_cid == matching_cid) { + add_cid = false; + break; + } + } + // Make new matching cid the next cid to match to + if (add_cid) { + indices.insert(indices.begin() + index, matching_cid); + query_scores.insert(query_scores.begin() + index, query_scores[0]); + ++index; + } + } + if (loc_params_.verbose_localization) + std::cout << "Num indices after adding similar images: " << indices.size() << std::endl; + } for (size_t j = 0; j < all_matches[i].size(); j++) { - if (cid_fid_to_pid[cid].count(all_matches[i][j].trainIdx) == 0) + if (cid_fid_to_pid_[cid].count(all_matches[i][j].trainIdx) == 0) continue; similarity_rank[i]++; } - if (FLAGS_verbose_localization) + if (loc_params_.verbose_localization) std::cout << "Overall matches and validated matches to: " - << cid_to_filename[cid] << ": " + << cid_to_filename_[cid] << ": " << all_matches[i].size() << " " << similarity_rank[i] << "\n"; total += similarity_rank[i]; - if (total >= early_break_landmarks) + if (total >= loc_params_.early_break_landmarks) break; } + // Check if enough matches found for estimating pose + if (total < 5) { + if (loc_params_.verbose_localization) + std::cout << "Too few matches: " << total << std::endl; + return false; + } + + // Update best previous cid if there were enough matches in total + if (loc_params_.add_best_previous_image) { + // Make sure to only add best previous cid if it has at least 5 matches + int most_matches = 5; + for (int i = 0; i < all_matches.size(); ++i) { + if (all_matches[i].size() > most_matches) { + best_previous_cid_ = indices[i]; + most_matches = all_matches[i].size(); + } + } + } + std::vector observations; std::vector landmarks; std::vector highly_ranked = ff_common::rv_order(similarity_rank); - int end = std::min(static_cast(highly_ranked.size()), num_similar); + int end = std::min(static_cast(highly_ranked.size()), loc_params_.num_similar); std::set seen_landmarks; - if (FLAGS_verbose_localization) + if (loc_params_.verbose_localization) std::cout << "Similar images: "; for (int i = 0; i < end; i++) { int cid = indices[highly_ranked[i]]; std::vector* matches = &all_matches[highly_ranked[i]]; int num_matches = 0; for (size_t j = 0; j < matches->size(); j++) { - if (cid_fid_to_pid[cid].count(matches->at(j).trainIdx) == 0) + if (cid_fid_to_pid_[cid].count(matches->at(j).trainIdx) == 0) continue; - const int landmark_id = cid_fid_to_pid.at(cid).at(matches->at(j).trainIdx); + const int landmark_id = cid_fid_to_pid_.at(cid).at(matches->at(j).trainIdx); if (seen_landmarks.count(landmark_id) > 0) continue; Eigen::Vector2d obs(test_keypoints.col(matches->at(j).queryIdx)[0], test_keypoints.col(matches->at(j).queryIdx)[1]); observations.push_back(obs); - landmarks.push_back(pid_to_xyz[landmark_id]); + landmarks.push_back(pid_to_xyz_[landmark_id]); seen_landmarks.insert(landmark_id); num_matches++; } - if (FLAGS_verbose_localization && num_matches > 0) - std::cout << " " << cid_to_filename[cid]; + if (loc_params_.verbose_localization && num_matches > 0) + std::cout << " " << cid_to_filename_[cid]; } - if (FLAGS_verbose_localization) std::cout << std::endl; + if (loc_params_.verbose_localization) std::cout << std::endl; int ret = RansacEstimateCamera(landmarks, observations, - num_ransac_iterations, - ransac_inlier_tolerance, pose, + loc_params_.num_ransac_iterations, + loc_params_.ransac_inlier_tolerance, pose, inlier_landmarks, inlier_observations, - FLAGS_verbose_localization); + loc_params_.verbose_localization); return (ret == 0); } @@ -767,23 +945,9 @@ bool SparseMap::Localize(std::string const& img_file, Eigen::Matrix2Xd test_keypoints; bool multithreaded = false; DetectFeaturesFromFile(img_file, multithreaded, &test_descriptors, &test_keypoints); - return sparse_mapping::Localize(test_descriptors, test_keypoints, - std::cref(camera_params_), + return Localize(test_descriptors, test_keypoints, pose, inlier_landmarks, inlier_observations, - cid_to_filename_.size(), - detector_.GetDetectorName(), - &vocab_db_, - num_similar_, - cid_to_filename_, - cid_to_descriptor_map_, - cid_to_keypoint_map_, - cid_fid_to_pid_, - pid_to_xyz_, - num_ransac_iterations_, - ransac_inlier_tolerance_, - early_break_landmarks_, - histogram_equalization_, cid_list); } @@ -959,49 +1123,9 @@ bool SparseMap::Localize(const cv::Mat & image, camera::CameraModel* pose, cv::Mat test_descriptors; Eigen::Matrix2Xd test_keypoints; DetectFeatures(image, multithreaded, &test_descriptors, &test_keypoints); - return sparse_mapping::Localize(test_descriptors, test_keypoints, - std::cref(camera_params_), + return Localize(test_descriptors, test_keypoints, pose, inlier_landmarks, inlier_observations, - cid_to_filename_.size(), - detector_.GetDetectorName(), - &vocab_db_, - num_similar_, - cid_to_filename_, - cid_to_descriptor_map_, - cid_to_keypoint_map_, - cid_fid_to_pid_, - pid_to_xyz_, - num_ransac_iterations_, - ransac_inlier_tolerance_, - early_break_landmarks_, - histogram_equalization_, - cid_list); + cid_list, image); } - -bool SparseMap::Localize(const cv::Mat & test_descriptors, const Eigen::Matrix2Xd & test_keypoints, - camera::CameraModel* pose, - std::vector* inlier_landmarks, - std::vector* inlier_observations, - std::vector * cid_list) { - return sparse_mapping::Localize(test_descriptors, test_keypoints, - std::cref(camera_params_), - pose, - inlier_landmarks, inlier_observations, - cid_to_filename_.size(), - detector_.GetDetectorName(), - &vocab_db_, - num_similar_, - cid_to_filename_, - cid_to_descriptor_map_, - cid_to_keypoint_map_, - cid_fid_to_pid_, - pid_to_xyz_, - num_ransac_iterations_, - ransac_inlier_tolerance_, - early_break_landmarks_, - histogram_equalization_, - cid_list); -} - } // namespace sparse_mapping diff --git a/localization/sparse_mapping/src/sparse_mapping.cc b/localization/sparse_mapping/src/sparse_mapping.cc index 95d0b874ae..94642e2150 100644 --- a/localization/sparse_mapping/src/sparse_mapping.cc +++ b/localization/sparse_mapping/src/sparse_mapping.cc @@ -117,9 +117,13 @@ namespace sparse_mapping { // of unknown values, but intolerant when true and false are mixed. void sparse_mapping::HistogramEqualizationCheck(int histogram_equalization1, int histogram_equalization2) { - if ( (histogram_equalization1 == 0 && histogram_equalization2 == 1) || - (histogram_equalization1 == 1 && histogram_equalization2 == 0) ) + // Ignore if either has unknown equalization value + if (histogram_equalization1 == HistogramEqualizationType::kUnknown || + histogram_equalization2 == HistogramEqualizationType::kUnknown) { + return; + } else if (histogram_equalization1 != histogram_equalization2) { LOG(FATAL) << "Incompatible values of histogram equalization detected."; + } } bool sparse_mapping::IsBinaryDescriptor(std::string const& descriptor) { diff --git a/localization/sparse_mapping/src/tensor.cc b/localization/sparse_mapping/src/tensor.cc index de2eb100af..e51fa3f94b 100644 --- a/localization/sparse_mapping/src/tensor.cc +++ b/localization/sparse_mapping/src/tensor.cc @@ -221,10 +221,11 @@ void MatchFeatures(const std::string & essential_file, ff_common::PrintProgressBar(stdout, static_cast(cid) / static_cast (s->cid_to_keypoint_map_.size() - 1)); std::vector indices, queried_indices; + std::vector scores; sparse_mapping::QueryDB(s->detector_.GetDetectorName(), - &s->vocab_db_, s->num_similar_, + &s->vocab_db_, s->loc_params().num_similar, s->cid_to_descriptor_map_[cid], - &queried_indices); + &queried_indices, &scores); if (!queried_indices.empty()) { // always include the next three images @@ -1933,21 +1934,12 @@ void ReadAffineCSV(std::string const& input_filename, } // Filter the matches by a geometric constraint. Compute the essential matrix. -void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, - Eigen::Matrix2Xd const& keypoints2, - std::vector const& matches, - camera::CameraParameters const& camera_params, - bool compute_inliers_only, - size_t cam_a_idx, size_t cam_b_idx, - std::mutex * match_mutex, - CIDPairAffineMap * relative_affines, - std::vector * inlier_matches, - bool compute_rays_angle, - double * rays_angle) { +void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2Xd const& keypoints2, + std::vector const& matches, camera::CameraParameters const& camera_params, + std::vector* inlier_matches, std::vector* vec_inliers, + Eigen::Matrix3d* essential_matrix, const int ransac_iterations) { // Initialize the outputs inlier_matches->clear(); - if (compute_rays_angle) - *rays_angle = 0.0; int pt_count = matches.size(); Eigen::MatrixXd observationsa(2, pt_count); @@ -1961,25 +1953,56 @@ void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, camera_params.GetUndistortedSize()[1]); Eigen::Matrix3d k = camera_params.GetIntrinsicMatrix(); - Eigen::Matrix3d e; // Calculate the essential matrix - std::vector vec_inliers; double error_max = std::numeric_limits::max(); double max_expected_error = 2.5; if (!interest_point::RobustEssential(k, k, observationsa, observationsb, - &e, &vec_inliers, + essential_matrix, vec_inliers, image_size, image_size, &error_max, - max_expected_error)) { + max_expected_error, ransac_iterations)) { + VLOG(2) << " | Estimation of essential matrix failed!\n"; + return; + } + + int num_inliers = vec_inliers->size(); + inlier_matches->clear(); + inlier_matches->reserve(num_inliers); + for (int i = 0; i < num_inliers; i++) { + inlier_matches->push_back(matches[(*vec_inliers)[i]]); + } + return; +} + +// Filter the matches by a geometric constraint. Compute the essential matrix. +void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, + Eigen::Matrix2Xd const& keypoints2, + std::vector const& matches, + camera::CameraParameters const& camera_params, + bool compute_inliers_only, + size_t cam_a_idx, size_t cam_b_idx, + std::mutex * match_mutex, + CIDPairAffineMap * relative_affines, + std::vector * inlier_matches, + bool compute_rays_angle, + double * rays_angle) { + // Initialize the outputs + if (compute_rays_angle) + *rays_angle = 0.0; + + std::vector vec_inliers; + Eigen::Matrix3d e; + FindEssentialAndInliers(keypoints1, keypoints2, matches, camera_params, inlier_matches, &vec_inliers, &e); + if (!inlier_matches) { VLOG(2) << cam_a_idx << " " << cam_b_idx << " | Estimation of essential matrix failed!\n"; return; } - if (vec_inliers.size() < static_cast(FLAGS_min_valid)) { + if (inlier_matches->size() < static_cast(FLAGS_min_valid)) { VLOG(2) << cam_a_idx << " " << cam_b_idx - << " | Failed to get enough inliers " << vec_inliers.size(); + << " | Failed to get enough inliers " << inlier_matches->size(); return; } @@ -1989,7 +2012,6 @@ void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, int num_inliers = vec_inliers.size(); inlier_matches->clear(); inlier_matches->reserve(num_inliers); - std::vector observations2(2, Eigen::Matrix2Xd(2, num_inliers)); for (int i = 0; i < num_inliers; i++) { inlier_matches->push_back(matches[vec_inliers[i]]); } @@ -1999,6 +2021,14 @@ void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, // Estimate the best possible R & T from the found Essential Matrix Eigen::Matrix3d r; Eigen::Vector3d t; + int pt_count = matches.size(); + Eigen::MatrixXd observationsa(2, pt_count); + Eigen::MatrixXd observationsb(2, pt_count); + for (int i = 0; i < pt_count; i++) { + observationsa.col(i) = keypoints1.col(matches[i].queryIdx); + observationsb.col(i) = keypoints2.col(matches[i].trainIdx); + } + Eigen::Matrix3d k = camera_params.GetIntrinsicMatrix(); if (!interest_point::EstimateRTFromE(k, k, observationsa, observationsb, e, vec_inliers, &r, &t)) { diff --git a/localization/sparse_mapping/src/visualization_utilities.cc b/localization/sparse_mapping/src/visualization_utilities.cc new file mode 100644 index 0000000000..f145333eeb --- /dev/null +++ b/localization/sparse_mapping/src/visualization_utilities.cc @@ -0,0 +1,68 @@ +/* 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. + */ + +#include + +#include +#include + +namespace sparse_mapping { +std::vector Keypoints(const Eigen::Matrix2Xd& keypoints_mat) { + std::vector keypoints; + for (int i = 0; i < keypoints_mat.cols(); ++i) { + keypoints.emplace_back(cv::KeyPoint(keypoints_mat.col(i).x(), keypoints_mat.col(i).y(), 1.0)); + } + return keypoints; +} + +std::vector DistortedKeypoints(const Eigen::Matrix2Xd& keypoints_mat, + const camera::CameraParameters& camera_params) { + std::vector keypoints; + for (int i = 0; i < keypoints_mat.cols(); ++i) { + Eigen::Vector2d distorted_point; + camera_params.Convert(keypoints_mat.col(i), &distorted_point); + keypoints.emplace_back(cv::KeyPoint(distorted_point.x(), distorted_point.y(), 1.0)); + } + return keypoints; +} + +void ViewMatches(const Eigen::Matrix2Xd& input_keypoints_mat, const Eigen::Matrix2Xd& map_keypoints_mat, + const std::vector& matches, const camera::CameraParameters& camera_params, + const cv::Mat& input_image, const cv::Mat& map_image) { + const auto input_keypoints = DistortedKeypoints(input_keypoints_mat, camera_params); + const auto map_keypoints = DistortedKeypoints(map_keypoints_mat, camera_params); + cv::Mat input_keypoints_image, map_keypoints_image; + cv::drawKeypoints(input_image, input_keypoints, input_keypoints_image); + cv::drawKeypoints(map_image, map_keypoints, map_keypoints_image); + cv::Mat keypoints_image; + cv::resize(input_keypoints_image, input_keypoints_image, cv::Size(1.1 * 960, 1.1 * 540)); + cv::resize(map_keypoints_image, map_keypoints_image, cv::Size(1.1 * 960, 1.1 * 540)); + cv::Mat combined_image; + cv::hconcat(input_keypoints_image, map_keypoints_image, combined_image); + cv::imshow("Input and Map Keypoints", combined_image); + cv::waitKey(0); + cv::destroyAllWindows(); + cv::Mat matches_image; + cv::drawMatches(input_image, input_keypoints, map_image, map_keypoints, matches, matches_image, cv::Scalar::all(-1), + cv::Scalar::all(-1), std::vector(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS); + cv::resize(matches_image, matches_image, cv::Size(1.7 * 960, 1.7 * 540)); + cv::imshow("Input to Map Matches", matches_image); + cv::waitKey(0); + cv::destroyAllWindows(); +} +} // namespace sparse_mapping diff --git a/localization/sparse_mapping/src/vocab_tree.cc b/localization/sparse_mapping/src/vocab_tree.cc index db6bafacc3..ccad057f76 100644 --- a/localization/sparse_mapping/src/vocab_tree.cc +++ b/localization/sparse_mapping/src/vocab_tree.cc @@ -379,8 +379,9 @@ void MatDescrToVec(cv::Mat const& mat, DBoW2::BriefDescriptor * brief) { // at most num_similar such indices. void QueryDB(std::string const& descriptor, VocabDB * vocab_db, int num_similar, cv::Mat const& descriptors, - std::vector * indices) { + std::vector * indices, std::vector * scores) { indices->clear(); + scores->clear(); if (vocab_db->binary_db != NULL) { assert(IsBinaryDescriptor(descriptor)); @@ -398,6 +399,7 @@ void QueryDB(std::string const& descriptor, VocabDB * vocab_db, for (size_t j = 0; j < ret.size(); j++) { indices->push_back(ret[j].Id); + scores->push_back(ret[j].Score); } } else { // no database specified diff --git a/localization/sparse_mapping/test/test_build_db.cc b/localization/sparse_mapping/test/test_build_db.cc index 1ea65875f4..2315b270e0 100644 --- a/localization/sparse_mapping/test/test_build_db.cc +++ b/localization/sparse_mapping/test/test_build_db.cc @@ -80,7 +80,7 @@ void RunWithDB(std::string const& detector_name) { int num_similar = 6; std::string out_nvm = "output.nvm"; sparse_mapping::SparseMap map(features_out); - map.SetNumSimilar(num_similar); + map.loc_params().num_similar = num_similar; EXPECT_EQ(static_cast(map.vocab_db_.m_num_nodes), 3); map.DetectFeatures(); std::string essential_file = "essential.csv"; @@ -105,13 +105,19 @@ void RunWithDB(std::string const& detector_name) { // Localize features with database. sparse_mapping::SparseMap map2(out_nvm); - map2.SetNumSimilar(num_similar); + map2.loc_params().num_similar = num_similar; LOG(INFO) << "\n\n================================================\n"; LOG(INFO) << "\nLocalizing using the database\n"; camera::CameraModel camera(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), camera_params); + std::string img_file = data_dir + "/m0004033.jpg"; - EXPECT_TRUE(map2.Localize(img_file, &camera)); + map2.loc_params().check_essential_matrix = false; + map2.loc_params().add_similar_images = false; + map2.loc_params().add_best_previous_image = false; + map2.loc_params().goodness_ratio = 100000; + map2.loc_params().hamming_distance = 90; + ASSERT_TRUE(map2.Localize(img_file, &camera)); Eigen::Affine3d closest = map2.GetFrameGlobalTransform(1); Eigen::Affine3d closest2 = map2.GetFrameGlobalTransform(2); diff --git a/localization/sparse_mapping/test/test_registration.cc b/localization/sparse_mapping/test/test_registration.cc index 386e9fb19d..21a348625c 100644 --- a/localization/sparse_mapping/test/test_registration.cc +++ b/localization/sparse_mapping/test/test_registration.cc @@ -103,8 +103,8 @@ class SparseMapTest : public ::testing::Test { EXPECT_GT(surf_map->GetNumLandmarks(), 30u); // should probably do some sanity check on the landmarks... - EXPECT_EQ(surf_map->GetRansacIterations(), 1000); - EXPECT_EQ(surf_map->GetRansacInlierTolerance(), 3); + EXPECT_EQ(surf_map->loc_params().num_ransac_iterations, 1000); + EXPECT_EQ(surf_map->loc_params().ransac_inlier_tolerance, 3); EXPECT_NEAR(surf_map->GetCameraParameters().GetFocalLength(), 258.5, 1e-5); } diff --git a/localization/sparse_mapping/test/test_sparse_map.cc b/localization/sparse_mapping/test/test_sparse_map.cc index f3089b402b..ef130311d3 100644 --- a/localization/sparse_mapping/test/test_sparse_map.cc +++ b/localization/sparse_mapping/test/test_sparse_map.cc @@ -172,12 +172,15 @@ TEST_P(SparseMapTest, MapBuilding) { } } EXPECT_GT(map_loopback.GetNumLandmarks(), 30u); - EXPECT_EQ(map_loopback.GetRansacInlierTolerance(), 3); + EXPECT_EQ(map_loopback.loc_params().ransac_inlier_tolerance, 3); EXPECT_NEAR(map_loopback.GetCameraParameters().GetFocalLength(), 258.5, 1e-5); // Test Map Consistency? // check that each frame localizes to its own position camera::CameraModel guess(map_loopback.GetCameraParameters()); + map_loopback.loc_params().check_essential_matrix = false; + map_loopback.loc_params().add_similar_images = false; + map_loopback.loc_params().add_best_previous_image = false; for (size_t i = 0; i < map_loopback.GetNumFrames(); i++) { Eigen::Affine3d transform = map_loopback.GetFrameGlobalTransform(i); EXPECT_TRUE(map_loopback.Localize(map_loopback.GetFrameFilename(i), &guess)); diff --git a/localization/sparse_mapping/tools/build_map.cc b/localization/sparse_mapping/tools/build_map.cc index 69663811a1..bfe1fe002a 100644 --- a/localization/sparse_mapping/tools/build_map.cc +++ b/localization/sparse_mapping/tools/build_map.cc @@ -51,9 +51,9 @@ DEFINE_bool(save_individual_maps, false, // output map parameters DEFINE_string(detector, "SURF", - "Feature detector to use. Options are: SURF, ORGBRISK."); -DEFINE_string(rebuild_detector, "ORGBRISK", - "Feature detector to use. Options are: SURF, ORGBRISK."); + "Feature detector to use. Options are: SURF, ORGBRISK, TEBLID512, TEBLID256."); +DEFINE_string(rebuild_detector, "TEBLID512", + "Feature detector to use. Options are: SURF, ORGBRISK, TEBLID512, TEBLID256."); // control options DEFINE_bool(feature_detection, false, @@ -98,7 +98,7 @@ DEFINE_bool(fix_cameras, false, "Keep the cameras fixed during bundle adjustment."); DEFINE_bool(rebuild_refloat_cameras, false, "Optimize the cameras as well as part of rebuilding. Usually that is " - "avoided when rebuilding with ORGBRISK, but could be useful with SURF."); + "avoided when rebuilding with binary features, but could be useful with SURF."); DEFINE_int32(db_restarts, 1, "Number of restarts when building the tree."); DEFINE_int32(db_depth, 0, "Depth of the tree to build. Default: 4"); diff --git a/tools/localization_analysis/include/localization_analysis/map_matcher.h b/tools/localization_analysis/include/localization_analysis/map_matcher.h index 020bb2f2d4..eaeb75f41f 100644 --- a/tools/localization_analysis/include/localization_analysis/map_matcher.h +++ b/tools/localization_analysis/include/localization_analysis/map_matcher.h @@ -34,8 +34,7 @@ namespace localization_analysis { class MapMatcher { public: MapMatcher(const std::string& input_bag_name, const std::string& map_file, const std::string& image_topic, - const std::string& output_bag_name, const std::string& config_prefix = "", - const std::string& save_noloc_imgs = ""); + const std::string& output_bag_name, const std::string& save_noloc_imgs = ""); void AddMapMatches(); void LogResults(); @@ -48,7 +47,6 @@ class MapMatcher { std::string image_topic_; sparse_mapping::SparseMap map_; localization_node::Localizer map_feature_matcher_; - std::string config_prefix_; gtsam::Pose3 body_T_nav_cam_; localization_common::Averager feature_averager_; int sparse_mapping_min_num_landmarks_; diff --git a/tools/localization_analysis/scripts/make_groundtruth.py b/tools/localization_analysis/scripts/make_groundtruth.py index fa93646649..5d2274f71b 100755 --- a/tools/localization_analysis/scripts/make_groundtruth.py +++ b/tools/localization_analysis/scripts/make_groundtruth.py @@ -116,7 +116,7 @@ ) groundtruth_bag = map_name + ".bag" - groundtruth_map_file = map_name + ".brisk.vocabdb.map" + groundtruth_map_file = map_name + ".teblid512.vocabdb.map" groundtruth_pdf = "groundtruth.pdf" groundtruth_csv = "groundtruth.csv" make_groundtruth_command = ( diff --git a/tools/localization_analysis/scripts/make_map.py b/tools/localization_analysis/scripts/make_map.py index 437233d476..ede8dd16f7 100755 --- a/tools/localization_analysis/scripts/make_map.py +++ b/tools/localization_analysis/scripts/make_map.py @@ -89,7 +89,7 @@ def make_map( lu.run_command_and_save_output(merge_map_command, "merge_map.txt") bag_surf_map = merged_surf_map - # Link maps directory since conversion to BRISK map needs + # Link maps directory since conversion to TEBLID512 map needs # image files to appear to be in correct relative path build_map_command = ( "rosrun sparse_mapping build_map -info -output_map " + base_surf_map @@ -122,32 +122,33 @@ def make_map( os.symlink(bag_images, merged_bag_images) linked_map_images = True - # Convert SURF to BRISK map + # Convert SURF to TEBLID512 map # Get full path to output file to avoid permission errors when running # command in maps directory - rebuild_output_file = os.path.join(os.getcwd(), "rebuild_map_as_brisk_map.txt") - bag_brisk_map = map_name + ".brisk.map" - shutil.copyfile(bag_surf_map, bag_brisk_map) - bag_brisk_map_full_path = os.path.abspath(bag_brisk_map) + rebuild_output_file = os.path.join(os.getcwd(), "rebuild_map_as_teblid512_map.txt") + bag_teblid512_map = map_name + ".teblid512.map" + shutil.copyfile(bag_surf_map, bag_teblid512_map) + bag_teblid512_map_full_path = os.path.abspath(bag_teblid512_map) bag_path = os.getcwd() if merge_with_base_map: os.chdir("maps") rebuild_map_command = ( - "rosrun sparse_mapping build_map -rebuild -output_map " - + bag_brisk_map_full_path + "rosrun sparse_mapping build_map -rebuild -rebuild_detector TEBLID512 -output_map " + + bag_teblid512_map_full_path ) if histogram_equalization: - rebuild_map_command += " -histogram_equalization" + rebuild_map_command += " -histogram_equalization -use_clahe " lu.run_command_and_save_output(rebuild_map_command, rebuild_output_file) # Use bag_path since relative commands would now be wrt maps directory simlink if merge_with_base_map: os.chdir(bag_path) # Create vocabdb - bag_brisk_vocabdb_map = map_name + ".brisk.vocabdb.map" - shutil.copyfile(bag_brisk_map, bag_brisk_vocabdb_map) + bag_teblid512_vocabdb_map = map_name + ".teblid512.vocabdb.map" + shutil.copyfile(bag_teblid512_map, bag_teblid512_vocabdb_map) add_vocabdb_command = ( - "rosrun sparse_mapping build_map -vocab_db -output_map " + bag_brisk_vocabdb_map + "rosrun sparse_mapping build_map -vocab_db -output_map " + + bag_teblid512_vocabdb_map ) lu.run_command_and_save_output(add_vocabdb_command, "build_vocabdb.txt") @@ -192,7 +193,7 @@ def make_map( "--no-histogram_equalization", dest="histogram_equalization", action="store_false", - help="Do not apply histrogram equalization during map creation. Default behavior uses histogram equalization.", + help="Do not apply histrogram equalization during map creation. Default behavior uses histogram equalization using CLAHE.", ) parser.set_defaults(histogram_equalization=True) diff --git a/tools/localization_analysis/src/live_measurement_simulator.cc b/tools/localization_analysis/src/live_measurement_simulator.cc index e52ab2ad3f..7377d53c65 100644 --- a/tools/localization_analysis/src/live_measurement_simulator.cc +++ b/tools/localization_analysis/src/live_measurement_simulator.cc @@ -49,7 +49,7 @@ LiveMeasurementSimulator::LiveMeasurementSimulator(const LiveMeasurementSimulato exit(0); } - map_feature_matcher_.ReadParams(&config); + map_feature_matcher_.ReadParams(config); optical_flow_tracker_.ReadParams(&config); std::vector topics; topics.push_back(std::string("/") + TOPIC_HARDWARE_IMU); diff --git a/tools/localization_analysis/src/map_matcher.cc b/tools/localization_analysis/src/map_matcher.cc index e66e2856f3..a5949b6c08 100644 --- a/tools/localization_analysis/src/map_matcher.cc +++ b/tools/localization_analysis/src/map_matcher.cc @@ -30,26 +30,26 @@ namespace localization_analysis { namespace lc = localization_common; namespace mc = msg_conversions; MapMatcher::MapMatcher(const std::string& input_bag_name, const std::string& map_file, const std::string& image_topic, - const std::string& output_bag_name, const std::string& config_prefix, - const std::string& save_noloc_imgs) + const std::string& output_bag_name, const std::string& save_noloc_imgs) : input_bag_(input_bag_name, rosbag::bagmode::Read), output_bag_(output_bag_name, rosbag::bagmode::Write), nonloc_bag_(), image_topic_(image_topic), map_(map_file, true), map_feature_matcher_(&map_), - config_prefix_(config_prefix), feature_averager_("Total number of features detected"), match_count_(0), image_count_(0) { config_reader::ConfigReader config; config.AddFile("geometry.config"); - lc::LoadGraphLocalizerConfig(config, config_prefix); + config.AddFile("cameras.config"); + config.AddFile("localization.config"); + lc::LoadGraphLocalizerConfig(config); if (!config.ReadFiles()) { LogFatal("Failed to read config files."); } + map_feature_matcher_.ReadParams(config); body_T_nav_cam_ = lc::LoadTransform(config, "nav_cam_transform"); - sparse_mapping_min_num_landmarks_ = mc::LoadInt(config, "loc_adder_min_num_matches"); if (!save_noloc_imgs.empty()) { nonloc_bag_.open(save_noloc_imgs, rosbag::bagmode::Write); } diff --git a/tools/localization_analysis/tools/run_map_matcher.cc b/tools/localization_analysis/tools/run_map_matcher.cc index 059e973508..22c3e0df3d 100644 --- a/tools/localization_analysis/tools/run_map_matcher.cc +++ b/tools/localization_analysis/tools/run_map_matcher.cc @@ -32,20 +32,17 @@ int main(int argc, char** argv) { std::string image_topic; std::string robot_config_file; std::string world; - std::string config_path_prefix; std::string save_noloc_imgs; po::options_description desc("Matches images to provided map and saves matches features and poses to a new bag file"); desc.add_options()("help,h", "produce help message")("bagfile,b", po::value()->required(), "Input bagfile containing image messages.")( "map-file,m", po::value()->required(), "Map file")( - "config-path,c", po::value()->required(), "Path to config directory.")( "image-topic,i", po::value(&image_topic)->default_value("mgt/img_sampler/nav_cam/image_record"), "Image topic")("robot-config-file,r", po::value(&robot_config_file)->default_value("config/robots/bumble.config"), "Robot config file")("world,w", po::value(&world)->default_value("iss"), "World name")( "output-bagfile,o", po::value(&output_bagfile)->default_value(""), "Output bagfile, defaults to input_bag + _map_matches.bag")( - "config-path-prefix,p", po::value(&config_path_prefix)->default_value(""), "Config path prefix")( "save-noloc-imgs,s", po::value(&save_noloc_imgs)->default_value("")->implicit_value(""), "Save non-localized images to a bag, defaults to input_bag + _nonloc_imgs.bag"); po::positional_options_description p; @@ -67,7 +64,6 @@ int main(int argc, char** argv) { const std::string input_bag = vm["bagfile"].as(); const std::string map_file = vm["map-file"].as(); - const std::string config_path = vm["config-path"].as(); // Only pass program name to free flyer so that boost command line options // are ignored when parsing gflags. @@ -95,8 +91,7 @@ int main(int argc, char** argv) { } lc::SetEnvironmentConfigs(world, robot_config_file); config_reader::ConfigReader config; - localization_analysis::MapMatcher map_matcher(input_bag, map_file, image_topic, output_bagfile, config_path_prefix, - save_noloc_imgs); + localization_analysis::MapMatcher map_matcher(input_bag, map_file, image_topic, output_bagfile, save_noloc_imgs); map_matcher.AddMapMatches(); ROS_INFO_STREAM("Using " << input_bag << " on map " << map_file);