Merge branch 'feature/radar_lidar_calibration' of github.com:tier4/Ca…

…librationTools into feature/radar_lidar_calibration

calibration_tools.repos

@@ -1,27 +1,27 @@
 repositories:
   autoware/calibration_tools:
     type: git
-    url: git@github.com:tier4/CalibrationTools.git
+    url: https://github.com/tier4/CalibrationTools.git
     version: tier4/universe
   vendor/lidartag:
     type: git
-    url: git@github.com:tier4/LiDARTag.git
+    url: https://github.com/tier4/LiDARTag.git
     version: humble
   vendor/lidartag_msgs:
     type: git
-    url: git@github.com:tier4/LiDARTag_msgs.git
+    url: https://github.com/tier4/LiDARTag_msgs.git
     version: tier4/universe
   vendor/apriltag_msgs:
     type: git
-    url: git@github.com:christianrauch/apriltag_msgs.git
+    url: https://github.com/christianrauch/apriltag_msgs.git
     version: 2.0.0
   vendor/apriltag_ros:
     type: git
-    url: git@github.com:christianrauch/apriltag_ros.git
+    url: https://github.com/christianrauch/apriltag_ros.git
     version: e814e9e5d5f1bfb60a4aa685d30977c632bbc540
   vendor/ros2_numpy:
     type: git
-    url: git@github.com:Box-Robotics/ros2_numpy.git
+    url: https://github.com/Box-Robotics/ros2_numpy.git
     version: humble
   vendor/image_pipeline:
     type: git

common/tier4_calibration_msgs/CMakeLists.txt

@@ -17,9 +17,13 @@ ament_auto_find_build_dependencies()
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/BoolStamped.msg"
   "msg/CalibrationPoints.msg"
+  "msg/Files.msg"
   "msg/Float32Stamped.msg"
   "msg/EstimationResult.msg"
   "msg/TimeDelay.msg"
+  "srv/Empty.srv"
+  "srv/FilesSrv.srv"
+  "srv/FilesListSrv.srv"
   "srv/CalibrationDatabase.srv"
   "srv/ExtrinsicCalibrationManager.srv"
   "srv/ExtrinsicCalibrator.srv"

common/tier4_calibration_msgs/msg/Files.msg

@@ -0,0 +1 @@
+string[] files

common/tier4_calibration_msgs/srv/Empty.srv

@@ -0,0 +1,2 @@
+---
+bool success

common/tier4_calibration_msgs/srv/FilesListSrv.srv

@@ -0,0 +1,3 @@
+Files[] files_list
+---
+bool success

common/tier4_calibration_msgs/srv/FilesSrv.srv

@@ -0,0 +1,3 @@
+Files files
+---
+bool success

common/tier4_tag_utils/CMakeLists.txt

@@ -0,0 +1,61 @@
+
+cmake_minimum_required(VERSION 3.5)
+project(tier4_tag_utils)
+
+find_package(autoware_cmake REQUIRED)
+find_package(OpenCV REQUIRED)
+
+autoware_package()
+
+ament_auto_add_library(tier4_tag_utils_lib SHARED
+  src/cv/sqpnp.cpp
+  src/lidartag_hypothesis.cpp
+  src/apriltag_hypothesis.cpp
+)
+
+target_link_libraries(tier4_tag_utils_lib
+${OpenCV_LIBS}
+)
+
+target_include_directories(tier4_tag_utils_lib
+  PUBLIC
+  include
+  ${OpenCV_INCLUDE_DIRS})
+
+  ament_export_include_directories(
+    include
+    ${OpenCV_INCLUDE_DIRS}
+  )
+
+ament_auto_add_library(apriltag_filter SHARED
+  src/apriltag_filter.cpp
+)
+
+ament_auto_add_library(lidartag_filter SHARED
+  src/lidartag_filter.cpp
+)
+
+target_link_libraries(apriltag_filter
+  ${OpenCV_LIBS}
+  tier4_tag_utils_lib
+)
+
+target_link_libraries(lidartag_filter
+  ${OpenCV_LIBS}
+  tier4_tag_utils_lib
+)
+
+rclcpp_components_register_node(apriltag_filter
+  PLUGIN "tier4_tag_utils::ApriltagFilter"
+  EXECUTABLE apriltag_filter_node
+)
+
+rclcpp_components_register_node(lidartag_filter
+  PLUGIN "tier4_tag_utils::LidartagFilter"
+  EXECUTABLE lidartag_filter_node
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    launch
+)

common/tier4_tag_utils/include/tier4_tag_utils/apriltag_filter.hpp

@@ -0,0 +1,77 @@
+// Copyright 2022 Tier IV, Inc.
+//
+// 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 TIER4_TAG_UTILS__APRILTAG_FILTER_HPP_
+#define TIER4_TAG_UTILS__APRILTAG_FILTER_HPP_
+
+#include <opencv2/opencv.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_tag_utils/apriltag_hypothesis.hpp>
+
+#include <apriltag_msgs/msg/april_tag_detection.hpp>
+#include <apriltag_msgs/msg/april_tag_detection_array.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace tier4_tag_utils
+{
+
+class ApriltagFilter : public rclcpp::Node
+{
+public:
+  explicit ApriltagFilter(const rclcpp::NodeOptions & options);
+
+protected:
+  void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg);
+
+  void detectionsCallback(
+    const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr detections_msg);
+
+  void publishFilteredDetections(const std_msgs::msg::Header & header);
+
+  // Parameters
+  std::unordered_map<std::string, double> tag_sizes_map_;
+
+  double max_no_observation_time_;
+  double new_hypothesis_transl_;
+  double measurement_noise_transl_;
+  double process_noise_transl_;
+  double min_tag_size_;
+  double max_tag_distance_;
+  double max_allowed_homography_error_;
+  int max_hamming_error_;
+  double min_margin_;
+
+  // ROS Interface
+  rclcpp::Publisher<apriltag_msgs::msg::AprilTagDetectionArray>::SharedPtr pub_;
+
+  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_sub_;
+
+  rclcpp::Subscription<apriltag_msgs::msg::AprilTagDetectionArray>::SharedPtr
+    apriltag_detections_array_sub_;
+
+  // ROS Data
+  sensor_msgs::msg::CameraInfo camera_info_;
+  image_geometry::PinholeCameraModel pinhole_camera_model_;
+
+  std::unordered_map<std::string, ApriltagHypothesis> hypotheses_map_;
+  std::unordered_map<std::string, apriltag_msgs::msg::AprilTagDetection> detection_map_;
+};
+
+}  // namespace tier4_tag_utils
+
+#endif  // TIER4_TAG_UTILS__APRILTAG_FILTER_HPP_

sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/apriltag_hypothesis.hpp → common/tier4_tag_utils/include/tier4_tag_utils/apriltag_hypothesis.hpp

@@ -12,23 +12,34 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef EXTRINSIC_TAG_BASED_CALIBRATOR__APRILTAG_HYPOTHESIS_HPP_
-#define EXTRINSIC_TAG_BASED_CALIBRATOR__APRILTAG_HYPOTHESIS_HPP_
+#ifndef TIER4_TAG_UTILS__APRILTAG_HYPOTHESIS_HPP_
+#define TIER4_TAG_UTILS__APRILTAG_HYPOTHESIS_HPP_
 
-#include <extrinsic_tag_based_calibrator/types.hpp>
 #include <opencv2/core.hpp>
 #include <opencv2/opencv.hpp>
 #include <opencv2/video/tracking.hpp>
 #include <rclcpp/time.hpp>
+#include <tier4_tag_utils/types.hpp>
 
 #include <image_geometry/pinhole_camera_model.h>
 
+#include <vector>
+
+namespace tier4_tag_utils
+{
+
 class ApriltagHypothesis
 {
 public:
+  ApriltagHypothesis() = default;
   ApriltagHypothesis(int id, image_geometry::PinholeCameraModel & pinhole_camera_model);
   ~ApriltagHypothesis();
 
+  ApriltagHypothesis(const ApriltagHypothesis &) = default;
+  ApriltagHypothesis(ApriltagHypothesis &&) = default;
+  ApriltagHypothesis & operator=(const ApriltagHypothesis &) = default;
+  ApriltagHypothesis & operator=(ApriltagHypothesis &&) = default;
+
   bool update(const std::vector<cv::Point2d> & corners, const rclcpp::Time & stamp);
   bool update(const rclcpp::Time & stamp);
 
@@ -82,9 +93,11 @@ class ApriltagHypothesis
   int id_;
   rclcpp::Time first_observation_timestamp_;
   rclcpp::Time last_observation_timestamp_;
-  image_geometry::PinholeCameraModel & pinhole_camera_model_;
+  image_geometry::PinholeCameraModel pinhole_camera_model_;
 
   std::vector<cv::Point2d> latest_corner_points_2d_, filtered_corner_points_2d_;
 };
 
-#endif  // EXTRINSIC_TAG_BASED_CALIBRATOR__APRILTAG_HYPOTHESIS_HPP_
+}  // namespace tier4_tag_utils
+
+#endif  // TIER4_TAG_UTILS__APRILTAG_HYPOTHESIS_HPP_

sensor/extrinsic_tag_based_calibrator/include/cv/_precomp.hpp → common/tier4_tag_utils/include/tier4_tag_utils/cv/_precomp.hpp

@@ -39,8 +39,8 @@
 // the use of this software, even if advised of the possibility of such damage.
 //
 //M*/
-#ifndef CV___PRECOMP_HPP_
-#define CV___PRECOMP_HPP_
+#ifndef TIER4_TAG_UTILS__CV___PRECOMP_HPP_
+#define TIER4_TAG_UTILS__CV___PRECOMP_HPP_
 
 #include "opencv2/core/utility.hpp"
 
@@ -136,4 +136,4 @@ static inline bool haveCollinearPoints(const Mat & m, int count)
 
 int checkChessboardBinary(const cv::Mat & img, const cv::Size & size);
 
-#endif  // CV___PRECOMP_HPP_
+#endif  // TIER4_TAG_UTILS__CV___PRECOMP_HPP_

sensor/extrinsic_tag_based_calibrator/include/cv/sqpnp.hpp → common/tier4_tag_utils/include/tier4_tag_utils/cv/sqpnp.hpp

@@ -36,8 +36,8 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#ifndef CV__SQPNP_HPP_
-#define CV__SQPNP_HPP_
+#ifndef TIER4_TAG_UTILS__CV__SQPNP_HPP_
+#define TIER4_TAG_UTILS__CV__SQPNP_HPP_
 
 #include <opencv2/core.hpp>
 #include <opencv2/core/matx.hpp>
@@ -192,4 +192,4 @@ class PoseSolver
 }  // namespace sqpnp
 }  // namespace cv
 
-#endif  // CV__SQPNP_HPP_
+#endif  // TIER4_TAG_UTILS__CV__SQPNP_HPP_

common/tier4_tag_utils/include/tier4_tag_utils/lidartag_filter.hpp

@@ -0,0 +1,70 @@
+// Copyright 2022 Tier IV, Inc.
+//
+// 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 TIER4_TAG_UTILS__LIDARTAG_FILTER_HPP_
+#define TIER4_TAG_UTILS__LIDARTAG_FILTER_HPP_
+
+#include <opencv2/opencv.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_tag_utils/lidartag_hypothesis.hpp>
+
+#include <lidartag_msgs/msg/lidar_tag_detection_array.hpp>
+
+#include <iostream>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace tier4_tag_utils
+{
+
+class LidartagFilter : public rclcpp::Node
+{
+public:
+  LidartagFilter(const rclcpp::NodeOptions & options);
+
+protected:
+  void lidarTagDetectionsCallback(
+    const lidartag_msgs::msg::LidarTagDetectionArray::SharedPtr detections_msg);
+
+  void updateHypothesis(
+    const lidartag_msgs::msg::LidarTagDetection & detection, const std_msgs::msg::Header & header);
+
+  void publishFilteredDetections(const std_msgs::msg::Header & header);
+
+  // Parameters
+  double max_no_observation_time_;
+  double new_hypothesis_distance_;
+  double new_hypothesis_transl_;
+  double new_hypothesis_rot_;
+  double measurement_noise_transl_;
+  double measurement_noise_rot_;
+  double process_noise_transl_;
+  double process_noise_transl_dot_;
+  double process_noise_rot_;
+  double process_noise_rot_dot_;
+
+  // ROS Interface
+  rclcpp::Publisher<lidartag_msgs::msg::LidarTagDetectionArray>::SharedPtr pub_;
+
+  rclcpp::Subscription<lidartag_msgs::msg::LidarTagDetectionArray>::SharedPtr sub_;
+
+  // Hypotheses
+  std::unordered_map<int, LidartagHypothesis> hypotheses_map_;
+  std::unordered_map<int, lidartag_msgs::msg::LidarTagDetection> detection_map_;
+};
+
+}  // namespace tier4_tag_utils
+
+#endif  // TIER4_TAG_UTILS__LIDARTAG_FILTER_HPP_

sensor/extrinsic_tag_based_calibrator/include/extrinsic_tag_based_calibrator/lidartag_hypothesis.hpp → common/tier4_tag_utils/include/tier4_tag_utils/lidartag_hypothesis.hpp

@@ -12,19 +12,25 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef EXTRINSIC_TAG_BASED_CALIBRATOR__LIDARTAG_HYPOTHESIS_HPP_
-#define EXTRINSIC_TAG_BASED_CALIBRATOR__LIDARTAG_HYPOTHESIS_HPP_
+#ifndef TIER4_TAG_UTILS__LIDARTAG_HYPOTHESIS_HPP_
+#define TIER4_TAG_UTILS__LIDARTAG_HYPOTHESIS_HPP_
 
-#include <extrinsic_tag_based_calibrator/types.hpp>
 #include <opencv2/core.hpp>
 #include <opencv2/opencv.hpp>
 #include <opencv2/video/tracking.hpp>
 #include <rclcpp/time.hpp>
+#include <tier4_tag_utils/types.hpp>
+
+#include <vector>
+
+namespace tier4_tag_utils
+{
 
 class LidartagHypothesis
 {
 public:
-  LidartagHypothesis(int id);
+  LidartagHypothesis() = default;
+  explicit LidartagHypothesis(int id);
 
   bool update(
     const cv::Matx31d & pose_translation, const cv::Matx33d & pose_rotation, double tag_size,
@@ -117,4 +123,6 @@ class LidartagHypothesis
   double estimated_speed_;
 };
 
-#endif  // EXTRINSIC_TAG_BASED_CALIBRATOR__LIDARTAG_HYPOTHESIS_HPP_
+}  // namespace tier4_tag_utils
+
+#endif  // TIER4_TAG_UTILS__LIDARTAG_HYPOTHESIS_HPP_