feat(calibration_tools): new api and 2.0 release (#148)

* Implementing the new calibration manager Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * Did some heavy refactoring in the tag-based camera-lidar calibration Added an option to use rectified images Deleted the raw sqpnp method since it is included in the standard opencv now Enabled the use of hardcoded system time (receiving time) for when sensors are now synchronized Parameterized rviz profiles to avoid having multiple profiles (for this method one should be enough for all use cases) Exposed the number of pnp pairs for calibration and the distance between calibration data and new detections to ease the work of field engineers Changed timers since the default work now (for years now though) Still have not done the spell checking Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * Fixed spellings in the tag based camera lidar calibrator Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * feat: adapter the lidar-lidar 2d to the new API Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: fixed spellings Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * feat: adapted the ground plane calibrator to the new api Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * feat: applied the changes required for the mapping-based lidar-lidar & base-lidar calibrators Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * fix: fixed non-optional parameters in the launcher configuration ui Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: removed unused dummy calibrator Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * feat: partial implementation of the changes for radar-lidar and implementation of a native way to edit the launchers Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * feat: partial implementation of the changes for radar-lidar and implementation of a native way to edit the launchers Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * fix: fixed the camera-lidar calibrator that was broken due to some new parameters ant the new launcher configuration schmeme Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * feat: complete implementation for the radar-lidar, partial implementation for the rdv, and others Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: attempting to fix CI/CD Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * feat: implemented the new api changes for the sfm calibrator and fixed bugs got discovered while doing do. Need to integrate all the options and the products Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * feat: base-lidars working on the rdv Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * feat: sfm calibrator integrated completely in rdv and although untested in the xx1. can use the initial calibration as a fixed ground plane Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * feat: integrated all the sfm methods to the x2 Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * feat: integrated the camera-lidar into the remaining project/products Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * feat: refactored the itneractive ui, integrated it into the new api, and implemented the fixes for the spell checking Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: deleted the old manager/client and renamed the new calibrators Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: deleted deprecated mesasges Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: fixed extra mispells found in ci/cd Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: more spell fixes Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * fix: fixed compile error (eigen vs. opencv) Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: applied the fix to another file Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: fixed dependencies for ci cd Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * fix: typo in launcher (thx vivid) Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * fix: forgot to set the initial solution after the refactoring in the ground plane calibrator Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: added explanations regarding the coordinate systems Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: fixed typo in launcher Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: updated documentation and added missing marker in rviz profile Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: forgot to ass the mapping base-lidar for the default project Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: deleted the new part of the service Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * feat: added the radar lidar calibrator to the default project Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: replaced all instances of transl -> translation and updated the copyrights Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: replaced rot -> rotation and missing transl -> translation Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: code explanation Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: removed redundant pass Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: typo Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: added explanation of a variable in the header file Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * ci(pre-commit): autofix * chore: attempt to fix pep257 Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: fixing ci/cd and standalone installation due to changes in autoware Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: more pep stuff Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: fized pep Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: deleted redundant pass statements Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: updated old name Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> * chore: updated misleading names Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> --------- Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp> Co-authored-by: yabuta <makoto.yabuta@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Apr 11, 2024 · a8a4b6a · a8a4b6a
1 parent 43ad25d
commit a8a4b6a
Show file tree

Hide file tree

Showing 423 changed files with 14,893 additions and 40,078 deletions.
diff --git a/.cspell.json b/.cspell.json
@@ -1,17 +1,108 @@
 {
   "words": [
-    "apriltags",
+    "3dpoints",
     "Rodrigues",
-    "subsampled",
-    "undistortion",
-    "uniformingly",
+    "antialiasing",
+    "apriltag",
+    "apriltags",
+    "arange",
+    "autoware",
+    "astype",
+    "auxiliar",
+    "axisd",
+    "beforementioned",
+    "calib",
+    "cmap",
+    "coeffs",
+    "crossval",
+    "crossvalidation",
+    "opencv",
+    "discretization",
+    "distro",
+    "downsampling",
+    "downsample",
+    "dtype",
+    "eigen",
+    "eulers",
+    "extrinsics",
+    "figsize",
+    "gicp",
+    "hesai",
+    "homography",
     "hsize",
+    "icp",
+    "idless",
+    "idxs",
+    "imdecode",
+    "imread",
+    "imshow",
+    "imwrite",
+    "intrinsics",
+    "kalman",
+    "keyframes",
+    "libceres",
+    "lidars",
+    "lidartag",
+    "lidartags",
+    "linalg",
+    "matplotlib",
+    "matx",
+    "meshgrid",
+    "misdetection",
+    "nanosec",
+    "neighbours",
+    "ncols",
+    "nrows",
+    "omiya",
+    "overfits",
+    "pandar",
+    "permutate",
+    "pixmap",
+    "pnp",
+    "pointcloud",
+    "pointclouds",
+    "polyline",
+    "prerejective",
+    "pydot",
+    "pyplot",
+    "qcolor",
+    "quaterniond",
+    "ransac",
+    "rclcpp",
+    "rclpy",
+    "registrator",
+    "registrators",
+    "remappings",
+    "representer",
+    "reprojected",
+    "reprojection",
+    "rosbag",
+    "rosidl",
+    "ruamel",
     "rvec",
-    "tvec",
     "rvecs",
+    "rviz",
+    "slerp",
+    "solvepnp",
+    "sqpnp",
+    "srvs",
+    "subsampled",
+    "subsamples",
+    "subsampling",
+    "tvec",
     "tvecs",
-    "nrows",
-    "ncols",
-    "crossval"
+    "undistort",
+    "undistortion",
+    "uniformingly",
+    "velodyne",
+    "vectord",
+    "voxel",
+    "voxels",
+    "xaxis",
+    "xlabel",
+    "xlim",
+    "yaxis",
+    "ylabel",
+    "ylim"
   ]
 }
diff --git a/build_depends.repos b/build_depends.repos
@@ -19,6 +19,10 @@ repositories:
     type: git
     url: https://github.com/astuff/astuff_sensor_msgs.git
     version: 3.2.0
+  core/autoware_internal_msgs:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_internal_msgs.git
+    version: main
   core/common:
     type: git
     url: https://github.com/autowarefoundation/autoware_common.git

diff --git a/...or/point_cloud_accumulator/CMakeLists.txt → ...on/point_cloud_accumulator/CMakeLists.txt b/...or/point_cloud_accumulator/CMakeLists.txt → ...on/point_cloud_accumulator/CMakeLists.txt
diff --git a/...rinsic_interactive_calibrator/__init__.py → common/point_cloud_accumulator/COLCON_IGNORE b/...rinsic_interactive_calibrator/__init__.py → common/point_cloud_accumulator/COLCON_IGNORE
diff --git a/...d_accumulator/point_cloud_accumulator.hpp → ...d_accumulator/point_cloud_accumulator.hpp b/...d_accumulator/point_cloud_accumulator.hpp → ...d_accumulator/point_cloud_accumulator.hpp
diff --git a/...launch/point_cloud_accumulator.launch.xml → ...launch/point_cloud_accumulator.launch.xml b/...launch/point_cloud_accumulator.launch.xml → ...launch/point_cloud_accumulator.launch.xml
diff --git a/sensor/point_cloud_accumulator/package.xml → common/point_cloud_accumulator/package.xml b/sensor/point_cloud_accumulator/package.xml → common/point_cloud_accumulator/package.xml
diff --git a/...cumulator/src/point_cloud_accumulator.cpp → ...cumulator/src/point_cloud_accumulator.cpp b/...cumulator/src/point_cloud_accumulator.cpp → ...cumulator/src/point_cloud_accumulator.cpp
diff --git a/common/tier4_calibration_msgs/CMakeLists.txt b/common/tier4_calibration_msgs/CMakeLists.txt
@@ -17,6 +17,7 @@ ament_auto_find_build_dependencies()
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/BoolStamped.msg"
   "msg/CalibrationPoints.msg"
+  "msg/CalibrationResult.msg"
   "msg/Files.msg"
   "msg/Float32Stamped.msg"
   "msg/EstimationResult.msg"
@@ -25,10 +26,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/FilesSrv.srv"
   "srv/FilesListSrv.srv"
   "srv/CalibrationDatabase.srv"
-  "srv/ExtrinsicCalibrationManager.srv"
-  "srv/ExtrinsicCalibrator.srv"
   "srv/Frame.srv"
   "srv/IntrinsicsOptimizer.srv"
+  "srv/ExtrinsicCalibrator.srv"
   DEPENDENCIES
     std_msgs
     sensor_msgs

diff --git a/common/tier4_calibration_msgs/msg/CalibrationResult.msg b/common/tier4_calibration_msgs/msg/CalibrationResult.msg
@@ -0,0 +1,4 @@
+geometry_msgs/TransformStamped transform_stamped
+bool success
+float32 score
+std_msgs/String message
diff --git a/common/tier4_calibration_msgs/srv/ExtrinsicCalibrationManager.srv b/common/tier4_calibration_msgs/srv/ExtrinsicCalibrationManager.srv
diff --git a/common/tier4_calibration_msgs/srv/ExtrinsicCalibrator.srv b/common/tier4_calibration_msgs/srv/ExtrinsicCalibrator.srv
@@ -1,6 +1,2 @@
-geometry_msgs/Pose initial_pose
 ---
-geometry_msgs/Pose result_pose
-bool success
-float32 score
-sensor_msgs/PointCloud2 debug_pointcloud
+tier4_calibration_msgs/CalibrationResult[] results
diff --git a/...alibration_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended.hpp b/...alibration_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended.hpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// Copyright 2024 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.

diff --git a/...ation_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended_impl.hpp b/...ation_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended_impl.hpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// Copyright 2024 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.
@@ -354,7 +354,7 @@ void pcl::JointIterativeClosestPointExtended<PointSource, PointTarget, Scalar>::
 
     ++nr_iterations_;
 
-    // Update the vizualization of icp convergence
+    // Update the visualization of icp convergence
     // if (update_visualizer_ != 0)
     //  update_visualizer_(output, source_indices_good, *target_, target_indices_good );
 

diff --git a/...libration_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets.hpp b/...libration_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets.hpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// Copyright 2024 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.
@@ -82,7 +82,7 @@ struct cloud_point_index_idx_triplets
   {
     return (idx0 < p.idx0) || (idx0 == p.idx0 && idx1 < p.idx1) ||
            (idx0 == p.idx0 && idx1 == p.idx1 && idx2 < p.idx2);
-  }  // test brancheless agains branched version with a dataset at some point \ along with some sort
+  }  // test branchless against branched version with a dataset at some point \ along with some sort
      // implementations
 };
 
@@ -102,7 +102,7 @@ class VoxelGridTriplets : public VoxelGrid<PointT>
   using VoxelGrid<PointT>::min_b_;
   using VoxelGrid<PointT>::max_b_;
   using VoxelGrid<PointT>::div_b_;
-  using VoxelGrid<PointT>::divb_mul_;
+  using VoxelGrid<PointT>::divb_mul_;  // cSpell:ignore divb
   using VoxelGrid<PointT>::min_points_per_voxel_;
   using VoxelGrid<PointT>::save_leaf_layout_;
   using VoxelGrid<PointT>::leaf_layout_;

diff --git a/...tion_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets_impl.hpp b/...tion_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets_impl.hpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// Copyright 2024 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.
@@ -123,6 +123,7 @@ void pcl::VoxelGridTriplets<PointT>::applyFilter(PointCloud & output)
   div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones();
   div_b_[3] = 0;
 
+  // cSpell:ignore divb
   // Set up the division multiplier
   divb_mul_ = Eigen::Vector4i(1, div_b_[0], div_b_[0] * div_b_[1], 0);
 
@@ -199,6 +200,8 @@ void pcl::VoxelGridTriplets<PointT>::applyFilter(PointCloud & output)
     }
   }
 
+  // cSpell:ignore spreadsort
+  // cSpell:ignore rightshift
   // Second pass: sort the index_vector vector using value representing target cell as index
   // in effect all points belonging to the same output cell will be next to each other
   // auto rightshift_func = [](const cloud_point_index_idx_triplets &x, const unsigned offset) {

diff --git a/common/tier4_calibration_pcl_extensions/src/joint_icp_extended.cpp b/common/tier4_calibration_pcl_extensions/src/joint_icp_extended.cpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// Copyright 2024 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.

diff --git a/common/tier4_calibration_pcl_extensions/src/voxel_grid_triplets.cpp b/common/tier4_calibration_pcl_extensions/src/voxel_grid_triplets.cpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// Copyright 2024 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.

diff --git a/common/tier4_calibration_views/CMakeLists.txt b/common/tier4_calibration_views/CMakeLists.txt
@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.5)
+project(tier4_calibration_views)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(autoware_cmake REQUIRED)
+
+autoware_package()
+ament_python_install_package(${PROJECT_NAME})
+
+install(PROGRAMS
+  scripts/image_view_node.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_export_dependencies(ament_cmake)
+ament_export_dependencies(ament_cmake_python)
+ament_package()
diff --git a/common/tier4_calibration_views/package.xml b/common/tier4_calibration_views/package.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>tier4_calibration_views</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="kenzo.lobos@tier4.jp">Kenzo Lobos Tsunekawa</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <exec_depend>python3-matplotlib</exec_depend>
+  <exec_depend>python3-pyside2.qtquick</exec_depend>
+  <exec_depend>python3-transforms3d</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>ros2_numpy</exec_depend>
+  <exec_depend>ros2launch</exec_depend>
+  <exec_depend>tier4_calibration_msgs</exec_depend>
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/...resource/extrinsic_interactive_calibrator → ...on_views/resource/tier4_calibration_views b/...resource/extrinsic_interactive_calibrator → ...on_views/resource/tier4_calibration_views
diff --git a/common/tier4_calibration_views/scripts/image_view_node.py b/common/tier4_calibration_views/scripts/image_view_node.py
@@ -0,0 +1,53 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 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.
+
+import logging
+import os
+import signal
+import sys
+
+from PySide2.QtWidgets import QApplication
+import rclpy
+from tier4_calibration_views.image_view_ros_interface import ImageViewRosInterface
+from tier4_calibration_views.image_view_ui import ImageViewUI
+
+
+def main(args=None):
+    os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = ""
+    app = QApplication(sys.argv)
+
+    rclpy.init(args=args)
+
+    try:
+        signal.signal(signal.SIGINT, sigint_handler)
+
+        ros_interface = ImageViewRosInterface()
+        ex = ImageViewUI(ros_interface)  # noqa: F841
+
+        ros_interface.spin()
+
+        sys.exit(app.exec_())
+    except (KeyboardInterrupt, SystemExit):
+        logging.info("Received sigint. Quitting...")
+        rclpy.shutdown()
+
+
+def sigint_handler(*args):
+    QApplication.quit()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/...nteractive_calibrator/test/test_pep257.py → ...er4_calibration_views/test/test_pep257.py b/...nteractive_calibrator/test/test_pep257.py → ...er4_calibration_views/test/test_pep257.py
diff --git a/common/tier4_calibration_views/tier4_calibration_views/__init__.py b/common/tier4_calibration_views/tier4_calibration_views/__init__.py