From 759de6dc911773ce732539ff2da6b31f02931cad Mon Sep 17 00:00:00 2001 From: Nikolaus Demmel Date: Wed, 16 Mar 2022 19:09:03 +0100 Subject: [PATCH 1/3] calibration: fix hardcoded number of tags -> use value from config --- include/basalt/calibration/calibration_helper.h | 1 + src/calibration/calibraiton_helper.cpp | 4 +++- src/calibration/cam_calib.cpp | 3 ++- src/calibration/cam_imu_calib.cpp | 3 ++- thirdparty/apriltag/include/basalt/utils/apriltag.h | 2 +- thirdparty/apriltag/src/apriltag.cpp | 13 +++++++++---- 6 files changed, 18 insertions(+), 8 deletions(-) diff --git a/include/basalt/calibration/calibration_helper.h b/include/basalt/calibration/calibration_helper.h index 95aac3a..a93ede3 100644 --- a/include/basalt/calibration/calibration_helper.h +++ b/include/basalt/calibration/calibration_helper.h @@ -78,6 +78,7 @@ using CalibInitPoseMap = class CalibHelper { public: static void detectCorners(const VioDatasetPtr& vio_data, + const AprilGrid& april_grid, CalibCornerMap& calib_corners, CalibCornerMap& calib_corners_rejected); diff --git a/src/calibration/calibraiton_helper.cpp b/src/calibration/calibraiton_helper.cpp index 323f26d..8861c38 100644 --- a/src/calibration/calibraiton_helper.cpp +++ b/src/calibration/calibraiton_helper.cpp @@ -106,6 +106,7 @@ bool estimateTransformation( } void CalibHelper::detectCorners(const VioDatasetPtr &vio_data, + const AprilGrid &april_grid, CalibCornerMap &calib_corners, CalibCornerMap &calib_corners_rejected) { calib_corners.clear(); @@ -114,7 +115,8 @@ void CalibHelper::detectCorners(const VioDatasetPtr &vio_data, tbb::parallel_for( tbb::blocked_range(0, vio_data->get_image_timestamps().size()), [&](const tbb::blocked_range &r) { - ApriltagDetector ad; + const int numTags = april_grid.getTagCols() * april_grid.getTagRows(); + ApriltagDetector ad(numTags); for (size_t j = r.begin(); j != r.end(); ++j) { int64_t timestamp_ns = vio_data->get_image_timestamps()[j]; diff --git a/src/calibration/cam_calib.cpp b/src/calibration/cam_calib.cpp index b70f402..2d52071 100644 --- a/src/calibration/cam_calib.cpp +++ b/src/calibration/cam_calib.cpp @@ -415,7 +415,8 @@ void CamCalib::detectCorners() { processing_thread.reset(new std::thread([this]() { std::cout << "Started detecting corners" << std::endl; - CalibHelper::detectCorners(this->vio_dataset, this->calib_corners, + CalibHelper::detectCorners(this->vio_dataset, this->april_grid, + this->calib_corners, this->calib_corners_rejected); std::string path = diff --git a/src/calibration/cam_imu_calib.cpp b/src/calibration/cam_imu_calib.cpp index f1f5fb9..b68bbb3 100644 --- a/src/calibration/cam_imu_calib.cpp +++ b/src/calibration/cam_imu_calib.cpp @@ -241,7 +241,8 @@ void CamImuCalib::detectCorners() { processing_thread.reset(new std::thread([this]() { std::cout << "Started detecting corners" << std::endl; - CalibHelper::detectCorners(this->vio_dataset, this->calib_corners, + CalibHelper::detectCorners(this->vio_dataset, this->april_grid, + this->calib_corners, this->calib_corners_rejected); std::string path = diff --git a/thirdparty/apriltag/include/basalt/utils/apriltag.h b/thirdparty/apriltag/include/basalt/utils/apriltag.h index 612979b..72ac755 100644 --- a/thirdparty/apriltag/include/basalt/utils/apriltag.h +++ b/thirdparty/apriltag/include/basalt/utils/apriltag.h @@ -9,7 +9,7 @@ struct ApriltagDetectorData; class ApriltagDetector { public: - ApriltagDetector(); + ApriltagDetector(int numTags); ~ApriltagDetector(); diff --git a/thirdparty/apriltag/src/apriltag.cpp b/thirdparty/apriltag/src/apriltag.cpp index 1aaa533..38c208f 100644 --- a/thirdparty/apriltag/src/apriltag.cpp +++ b/thirdparty/apriltag/src/apriltag.cpp @@ -8,13 +8,14 @@ namespace basalt { struct ApriltagDetectorData { - ApriltagDetectorData() + ApriltagDetectorData(int numTags) : doSubpixRefinement(true), maxSubpixDisplacement(0), minTagsForValidObs(4), minBorderDistance(4.0), blackTagBorder(2), - _tagCodes(AprilTags::tagCodes36h11) { + _tagCodes(AprilTags::tagCodes36h11), + _numTags(numTags) { _tagDetector = std::make_shared(_tagCodes, blackTagBorder); } @@ -30,10 +31,14 @@ struct ApriltagDetectorData { AprilTags::TagCodes _tagCodes; std::shared_ptr _tagDetector; - inline int size() { return 36 * 4; } + int _numTags; //!< number of tags in the grid (determines the valid ids) + + inline int size() { return _numTags * 4; } }; -ApriltagDetector::ApriltagDetector() { data = new ApriltagDetectorData; } +ApriltagDetector::ApriltagDetector(int numTags) { + data = new ApriltagDetectorData(numTags); +} ApriltagDetector::~ApriltagDetector() { delete data; } From a00d6289acf5a1cbecdf2f901e92f80de7446d69 Mon Sep 17 00:00:00 2001 From: Nikolaus Demmel Date: Wed, 16 Mar 2022 19:09:30 +0100 Subject: [PATCH 2/3] cmake: add apriltag header file --- thirdparty/apriltag/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/thirdparty/apriltag/CMakeLists.txt b/thirdparty/apriltag/CMakeLists.txt index ff2bf05..9c36fa8 100644 --- a/thirdparty/apriltag/CMakeLists.txt +++ b/thirdparty/apriltag/CMakeLists.txt @@ -17,7 +17,7 @@ include_directories(../../include) include_directories(../basalt-headers/include) include_directories(../basalt-headers/thirdparty/Sophus) -add_library(apriltag STATIC ${APRILTAG_SRCS} src/apriltag.cpp) +add_library(apriltag STATIC ${APRILTAG_SRCS} include/basalt/utils/apriltag.h src/apriltag.cpp) target_include_directories(apriltag PUBLIC include) target_link_libraries(apriltag PUBLIC basalt::opencv Sophus::Sophus) From a2c50712646a6ed37d0ac83a5469886bab9b3803 Mon Sep 17 00:00:00 2001 From: Nikolaus Demmel Date: Wed, 16 Mar 2022 19:09:46 +0100 Subject: [PATCH 3/3] fix unsed-variable warning --- test/src/test_patch.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test/src/test_patch.cpp b/test/src/test_patch.cpp index 6cedea2..42a0d28 100644 --- a/test/src/test_patch.cpp +++ b/test/src/test_patch.cpp @@ -26,6 +26,8 @@ struct SmoothFunction { BASALT_HOST_DEVICE inline bool InBounds( const Eigen::MatrixBase& p, const typename Derived::Scalar border) const { + UNUSED(p); + UNUSED(border); return true; } };