-
Notifications
You must be signed in to change notification settings - Fork 46
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Camera-lidar calibration tools initial release (#9)
* Changes to fix the remappings. Also deleted unused or redundant code/parameters * deleted unused members in structures and unused code * The lidartag was using pandar as the lidar_frame in a hardcoded fashion. Since we plan to use several pandar instances with different names, we must parameterize this frame from the calibration tools * Fixed several errors and implemented several improvements to increase the detection rate and precision of the detections. - Fixes clusters that could not be formed correctly in one pass - Adds an optional rectangle estimation rather tha 4x line estimation to get the corners, it is much more stable and increases both the accuracy and detection rate - Refines the initial center of the tag based on the corner fitting which seems to improve the pose estimation. In many cases the nlopt optimizator is not even needed * - Calibration tools compliance - Fix edge groups provided by PCA using the partial rectangle models produced by the RANSAC iterations. Provides great results. - Some debug drawings to help understand the previous fix - Parameterized min decoding score * Truned off the verbosity that is lidartag targetting its release * Updated linkage * Added empty file because git does not support empty folders * Removed comments and unused files
- Loading branch information
Showing
30 changed files
with
1,782 additions
and
817 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
File renamed without changes.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
File renamed without changes.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,90 @@ | ||
| // Copyright 2021 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 RECTANGLE_ESTIMATOR_HPP | ||
| #define RECTANGLE_ESTIMATOR_HPP | ||
|
|
||
| #include <pcl/point_types.h> | ||
| #include <pcl/point_cloud.h> | ||
| #include <pcl/PCLPointCloud2.h> | ||
|
|
||
| class RectangleEstimator { | ||
| public: | ||
| EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
|
|
||
| RectangleEstimator(); | ||
|
|
||
| void setInputPoints( | ||
| pcl::PointCloud<pcl::PointXYZ>::Ptr & points1, | ||
| pcl::PointCloud<pcl::PointXYZ>::Ptr & points2, | ||
| pcl::PointCloud<pcl::PointXYZ>::Ptr & points3, | ||
| pcl::PointCloud<pcl::PointXYZ>::Ptr & points4); | ||
|
|
||
| bool estimate(); | ||
| bool estimate_ransac(); | ||
| bool estimate_imlp( | ||
| pcl::PointCloud<pcl::PointXYZ>::Ptr points1, | ||
| pcl::PointCloud<pcl::PointXYZ>::Ptr points2, | ||
| pcl::PointCloud<pcl::PointXYZ>::Ptr points3, | ||
| pcl::PointCloud<pcl::PointXYZ>::Ptr points4, | ||
| Eigen::Vector2d & n_out, | ||
| Eigen::Vector4d & c_out); | ||
|
|
||
| std::vector<Eigen::Vector2d> getCorners(); | ||
| std::vector<Eigen::Vector2d> getCorners(Eigen::Vector2d & n, Eigen::Vector4d & c); | ||
|
|
||
| Eigen::Vector2d getNCoefficients() const; | ||
| Eigen::Vector4d getCCoefficients() const; | ||
|
|
||
| void setRANSAC(bool enable); | ||
| void setFilterByCoefficients(bool enable); | ||
| void setMaxIterations(int iterations); | ||
| void setInlierError(double error); | ||
| void setFixPointGroups(bool enabe); | ||
|
|
||
| void preparePointsMatrix(); | ||
| double getModelErrorAndInliers(Eigen::Vector2d & n, Eigen::Vector4d & c, int & inliers, | ||
| std::vector<Eigen::Vector2d> & inliers1, std::vector<Eigen::Vector2d> & inliers2, | ||
| std::vector<Eigen::Vector2d> & inliers3, std::vector<Eigen::Vector2d> & inliers4, | ||
| Eigen::VectorXd & errors1, Eigen::VectorXd & errors2, | ||
| Eigen::VectorXd & errors3, Eigen::VectorXd & errors4, | ||
| bool add_inliers); | ||
|
|
||
| protected: | ||
|
|
||
| bool checkCoefficients() const; | ||
| void fixPointGroups(Eigen::VectorXd & errors1, Eigen::VectorXd & errors2, | ||
| Eigen::VectorXd & errors3, Eigen::VectorXd & errors4); | ||
|
|
||
| bool use_ransac_; | ||
| bool filter_by_coefficients_; | ||
| int max_ransac_iterations_; | ||
| double max_error_; | ||
| bool fix_point_groups_; | ||
|
|
||
| Eigen::MatrixXd augmented_matrix_; | ||
| Eigen::MatrixXd points_matrix_; | ||
| Eigen::Vector4d estimated_c; | ||
| Eigen::Vector2d estimated_n; | ||
|
|
||
| pcl::PointCloud<pcl::PointXYZ>::Ptr points1_; | ||
| pcl::PointCloud<pcl::PointXYZ>::Ptr points2_; | ||
| pcl::PointCloud<pcl::PointXYZ>::Ptr points3_; | ||
| pcl::PointCloud<pcl::PointXYZ>::Ptr points4_; | ||
|
|
||
| pcl::PointCloud<pcl::PointXYZ> points; | ||
|
|
||
| }; | ||
|
|
||
| #endif // RECTANGLE_ESTIMATOR_HPP |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
File renamed without changes.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -28,6 +28,9 @@ | |
| * AUTHOR: Bruce JK Huang ([email protected]) | ||
| * WEBSITE: https://www.brucerobot.com/ | ||
| */ | ||
|
|
||
| #pragma once | ||
|
|
||
| #include "nanoflann.hpp" | ||
| #include <Eigen/Core> | ||
| #include <Eigen/Sparse> | ||
|
|
@@ -194,6 +197,26 @@ typedef struct { | |
| point right; | ||
| } corners; | ||
|
|
||
| enum class LidartagErrorCode //C++11 scoped enum | ||
| { | ||
| NoError = 0, | ||
| ClusterMinPointsCriteria = 1, | ||
| ClusterMinPointsCriteria2 = 2, | ||
| PlanarCheckCriteria = 3, | ||
| PlanarOutliersCriteria = 4, | ||
| DecodingPointsCriteria = 5, | ||
| DecodingRingsCriteria = 6, | ||
| CornerEstimationMinPointsCriteria = 7, | ||
| Line1EstimationCriteria = 8, | ||
| Line2EstimationCriteria = 9, | ||
| Line3EstimationCriteria = 10, | ||
| Line4EstimationCriteria = 11, | ||
| RectangleEstimationCirteria = 12, | ||
| TagSizeEstimationCriteria = 13, | ||
| OptimizationErrorCriteria = 14, | ||
| DecodingErrorCriteria = 15 | ||
| }; | ||
|
|
||
| typedef struct ClusterFamily { | ||
| // EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| int cluster_id; | ||
|
|
@@ -215,6 +238,7 @@ typedef struct ClusterFamily { | |
| pcl::PointCloud<LidarPoints_t> data; // data doesn't have edge points | ||
| pcl::PointCloud<LidarPoints_t> edge_points; | ||
| pcl::PointCloud<LidarPoints_t> transformed_edge_points; | ||
| pcl::PointCloud<PointXYZRI> initial_corners; | ||
|
|
||
| // If the first point of the ring is the cluster. | ||
| // If so, the the indices fo the two sides will be far away | ||
|
|
@@ -279,15 +303,10 @@ typedef struct ClusterFamily { | |
| */ | ||
| std::vector<Eigen::VectorXf> | ||
| line_coeff; // Upper, left, bottom, right line (count-clockwise) | ||
| int detail_valid; | ||
| LidartagErrorCode detail_valid; | ||
| int pose_estimation_status; | ||
| int expected_points; | ||
|
|
||
| corners tag_corners; // KL: adding here for now | ||
| corners tag_boundary_corners; | ||
| std::vector<point> corner_offset_array; | ||
| std::vector<point> boundary_corner_offset_array; | ||
|
|
||
| } ClusterFamily_t; | ||
|
|
||
| typedef struct GrizTagFamily { | ||
|
|
||
Oops, something went wrong.