diff --git a/slam_lib/CMakeLists.txt b/slam_lib/CMakeLists.txt index cf856297a15d1dfe8efbe08ed02b026b9995613b..2f842f60274914ed3090a8b8543b9685c0a40205 100644 --- a/slam_lib/CMakeLists.txt +++ b/slam_lib/CMakeLists.txt @@ -11,8 +11,9 @@ set(LidarSlam_PUBLIC_HEADERS include/LidarSlam/Enums.h include/LidarSlam/GlobalTrajectoriesRegistration.h include/LidarSlam/KDTreePCLAdaptor.h - include/LidarSlam/KeypointsRegistration.h + include/LidarSlam/KeypointsMatcher.h include/LidarSlam/LidarPoint.h + include/LidarSlam/LocalOptimizer.h include/LidarSlam/MotionModel.h include/LidarSlam/PointCloudStorage.h include/LidarSlam/RollingGrid.h @@ -28,7 +29,8 @@ set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) add_library(LidarSlam SHARED src/GlobalTrajectoriesRegistration.cxx - src/KeypointsRegistration.cxx + src/KeypointsMatcher.cxx + src/LocalOptimizer.cxx src/MotionModel.cxx src/RollingGrid.cxx src/Slam.cxx diff --git a/slam_lib/include/LidarSlam/CeresCostFunctions.h b/slam_lib/include/LidarSlam/CeresCostFunctions.h index 4fbdc1193a0863bd2fedfa2deb4b107e6e213e49..41ea0152085def7fa08c7f54aa20065b2369509c 100644 --- a/slam_lib/include/LidarSlam/CeresCostFunctions.h +++ b/slam_lib/include/LidarSlam/CeresCostFunctions.h @@ -22,12 +22,20 @@ // LOCAL #include "LidarSlam/MotionModel.h" // CERES -#include +#include // EIGEN #include namespace LidarSlam { +namespace CeresTools +{ +struct Residual +{ + ceres::CostFunction* Cost = nullptr; + ceres::LossFunction* Robustifier = nullptr; +}; +} namespace CeresCostFunctions { diff --git a/slam_lib/include/LidarSlam/KeypointsRegistration.h b/slam_lib/include/LidarSlam/KeypointsMatcher.h similarity index 77% rename from slam_lib/include/LidarSlam/KeypointsRegistration.h rename to slam_lib/include/LidarSlam/KeypointsMatcher.h index da0cbfe6bb9f13047753ca1aff26467a7c78088e..16ae70c9e057940ab7363745fbeea2e5f54f95e9 100644 --- a/slam_lib/include/LidarSlam/KeypointsRegistration.h +++ b/slam_lib/include/LidarSlam/KeypointsMatcher.h @@ -1,7 +1,7 @@ //============================================================================== // Copyright 2019-2020 Kitware, Inc., Kitware SAS -// Author: Cadart Nicolas (Kitware SAS) -// Creation date: 2020-10-16 +// Author: Sanchez Julia (Kitware SAS) +// Creation date: 2021-03-01 // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -19,24 +19,20 @@ #pragma once #include "LidarSlam/KDTreePCLAdaptor.h" +#include "LidarSlam/CeresCostFunctions.h" #include "LidarSlam/LidarPoint.h" #include "LidarSlam/MotionModel.h" #include "LidarSlam/Utilities.h" #include "LidarSlam/Enums.h" #include -#include #include namespace LidarSlam { -// Helper class to register a set of edge/plane/blob keypoints onto -// another to estimate the transformation between them. -// Firstly, a matching step is performed : we need to build the point-to-line, -// point-to-plane and point-to-blob residuals that will be optimized. -// Then, we use CERES Levenberg-Marquardt optimization to minimize the problem. -class KeypointsRegistration +// Helper class to match edge/planar/blob keypoints and build ceres residuals +class KeypointsMatcher { public: using Point = LidarPoint; @@ -49,8 +45,6 @@ public: // Max number of threads to use to parallelize computations unsigned int NbThreads = 1; - // **** ICP Parameters **** - // When searching edge keypoint nearest neighbors, we can follow different // strategies to keep only relevant matches, instead of taking all k-nearest points. // If false, the method GetRansacLineNeighbors() will be used. @@ -86,11 +80,6 @@ public: unsigned int BlobDistanceNbrNeighbors = 25; //< number of blob neighbors required to approximate the corresponding ellipsoid - // **** LM optimization Parameters **** - - // Maximum number of iteration - unsigned int LMMaxIter = 15; - // Maximum distance (in meters) beyond which the residual errors are // saturated to robustify the optimization against outlier constraints. // The residuals will be robustified by Tukey loss at scale sqrt(SatDist). @@ -118,8 +107,12 @@ public: { MatchStatus Status; double Weight; + CeresTools::Residual Cost; }; + // Vector of residual functions to add to ceres problem + std::vector Residuals; + // Matching result of each keypoint std::vector Rejections; std::vector Weights; @@ -127,51 +120,33 @@ public: std::array RejectionsHistogram = {}; // Number of successful matches (shortcut to RejectionsHistogram[SUCCESS]) - unsigned int NbMatches() const { return RejectionsHistogram[SUCCESS]; } - }; + unsigned int NbMatches() const { return this->RejectionsHistogram[SUCCESS]; } - //! Estimation of registration error - struct RegistrationError - { - // Estimation of the maximum position error - double PositionError = 0.; - // Direction of the maximum position error - Eigen::Vector3d PositionErrorDirection = Eigen::Vector3d::Zero(); - - // Estimation of the maximum orientation error (in radians) - double OrientationError = 0.; - // Direction of the maximum orientation error - Eigen::Vector3d OrientationErrorDirection = Eigen::Vector3d::Zero(); - - // Covariance matrix encoding the estimation of the pose's errors about the 6-DoF parameters - // (DoF order : X, Y, Z, rX, rY, rZ) - Eigen::Matrix6d Covariance = Eigen::Matrix6d::Zero(); + void Reset(const unsigned int N) + { + this->Weights.assign(N, 0.); + this->Rejections.assign(N, MatchingResults::MatchStatus::UNKOWN); + this->RejectionsHistogram.fill(0); + this->Residuals.assign(N, CeresTools::Residual()); + } }; //---------------------------------------------------------------------------- - // Init ICP-LM optimizer. - KeypointsRegistration(const Parameters& params, const Eigen::Isometry3d& posePrior); + // Init matcher + // It needs matching parameters and the prior transform to apply to keypoints + KeypointsMatcher(const Parameters& params, const Eigen::Isometry3d& posePrior); // Build point-to-neighborhood residuals - MatchingResults BuildAndMatchResiduals(const PointCloud::Ptr& currPoints, - const KDTree& prevPoints, - Keypoint keypointType); - - // Optimize the Ceres problem - ceres::Solver::Summary Solve(); - - // Get optimization results - Eigen::Isometry3d GetOptimizedPose() const { return Utils::XYZRPYtoIsometry(this->PoseArray); } - - // Estimate registration error - RegistrationError EstimateRegistrationError(); + MatchingResults BuildMatchResiduals(const PointCloud::Ptr& currPoints, + const KDTree& prevPoints, + Keypoint keypointType); //---------------------------------------------------------------------------- private: - // Add an ICP match residual. + // Build ICP match residual functions. // To recover the motion, we have to minimize the function // f(R, T) = sum(d(edge_kpt, line)^2) + sum(d(plane_kpt, plane)^2) + sum(d(blob_kpt, blob)^2) // In all cases, the squared Mahalanobis distance between the keypoint and the line/plane/blob can be written : @@ -186,7 +161,7 @@ private: // * A = C^{-1/2} is the squared information matrix, aka stiffness matrix, where // C is the covariance matrix encoding the shape of the neighborhood for a blob. // - weight attenuates the distance function for outliers - void AddIcpResidual(const Eigen::Matrix3d& A, const Eigen::Vector3d& P, const Eigen::Vector3d& X, double weight = 1.); + CeresTools::Residual BuildResidual(const Eigen::Matrix3d& A, const Eigen::Vector3d& P, const Eigen::Vector3d& X, double weight = 1.); // Match the current keypoint with its neighborhood in the map / previous MatchingResults::MatchInfo BuildLineMatch(const KDTree& kdtreePreviousEdges, const Point& p); @@ -212,9 +187,6 @@ private: const Parameters Params; - // The problem to build and optimize - ceres::Problem Problem; - // Initialization of DoF to optimize const Eigen::Isometry3d PosePrior; ///< Initial guess of the pose to optimize diff --git a/slam_lib/include/LidarSlam/LocalOptimizer.h b/slam_lib/include/LidarSlam/LocalOptimizer.h new file mode 100644 index 0000000000000000000000000000000000000000..bf41d696a45563bad2d1c9e57c04b94ec0e99266 --- /dev/null +++ b/slam_lib/include/LidarSlam/LocalOptimizer.h @@ -0,0 +1,96 @@ +//============================================================================== +// Copyright 2019-2020 Kitware, Inc., Kitware SAS +// Author: Sanchez Julia (Kitware SAS) +// Creation date: 2021-03-01 +// +// 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. +//============================================================================== + +#pragma once + +#include +#include +#include "LidarSlam/Utilities.h" +#include "LidarSlam/CeresCostFunctions.h" + +namespace LidarSlam +{ +// Helper class to optimize the LidarSlam problem +class LocalOptimizer +{ +public: + + //! Estimation of registration error + struct RegistrationError + { + // Estimation of the maximum position error + double PositionError = 0.; + // Direction of the maximum position error + Eigen::Vector3d PositionErrorDirection = Eigen::Vector3d::Zero(); + + // Estimation of the maximum orientation error (in radians) + double OrientationError = 0.; + // Direction of the maximum orientation error + Eigen::Vector3d OrientationErrorDirection = Eigen::Vector3d::Zero(); + + // Covariance matrix encoding the estimation of the pose's errors about the 6-DoF parameters + // (DoF order : X, Y, Z, rX, rY, rZ) + Eigen::Matrix6d Covariance = Eigen::Matrix6d::Zero(); + }; + + //---------------------------------------------------------------------------- + // Set params + // Set Levenberg Marquardt maximum number of iterations + void SetLMMaxIter(unsigned int iter); + + // Set number of threads + void SetNbThreads(unsigned int n); + + // Set prior pose + void SetPosePrior(const Eigen::Isometry3d& posePrior); + + // Add Sensor residuals to residuals vector + void AddResiduals(std::vector& sensorRes); + + // Clear all residuals + void Clear(); + + // Build and optimize the Ceres problem + ceres::Solver::Summary Solve(); + + // Get optimization results + Eigen::Isometry3d GetOptimizedPose() const { return Utils::XYZRPYtoIsometry(this->PoseArray); } + + // Estimate registration error + RegistrationError EstimateRegistrationError(); + //---------------------------------------------------------------------------- + +private: + + // Max number of threads to use to parallelize computations + unsigned int NbThreads = 1; + + // Maximum number of iteration + unsigned int LMMaxIter = 15; + + // DoF to optimize (= output) + Eigen::Vector6d PoseArray; ///< Pose parameters to optimize (XYZRPY) + + // Residuals vector + // These residuals must involve the full 6D pose array (X, Y, Z, rX, rY, rZ) + std::vector Residuals; + + std::unique_ptr Problem; +}; + +} // end of LidarSlam namespace \ No newline at end of file diff --git a/slam_lib/include/LidarSlam/Slam.h b/slam_lib/include/LidarSlam/Slam.h index 892437c9b78b3498155000d6e31a1e9854beb78d..c2ef4b53d241e75c25a5803911cc7bbc4e75ae9a 100644 --- a/slam_lib/include/LidarSlam/Slam.h +++ b/slam_lib/include/LidarSlam/Slam.h @@ -79,7 +79,8 @@ #include "LidarSlam/Enums.h" #include "LidarSlam/SpinningSensorKeypointExtractor.h" #include "LidarSlam/KDTreePCLAdaptor.h" -#include "LidarSlam/KeypointsRegistration.h" +#include "LidarSlam/KeypointsMatcher.h" +#include "LidarSlam/LocalOptimizer.h" #include "LidarSlam/MotionModel.h" #include "LidarSlam/RollingGrid.h" #include "LidarSlam/PointCloudStorage.h" @@ -189,7 +190,7 @@ public: // --------------------------------------------------------------------------- GetMacro(NbThreads, int) - void SetNbThreads(int n) { this->NbThreads = n; for (const auto& kv : this->KeyPointsExtractors) kv.second->SetNbThreads(n); } + void SetNbThreads(int n); SetMacro(Verbosity, int) GetMacro(Verbosity, int) @@ -469,13 +470,13 @@ private: // --------------------------------------------------------------------------- //! Matching results - std::map EgoMotionMatchingResults; - std::map LocalizationMatchingResults; + std::map EgoMotionMatchingResults; + std::map LocalizationMatchingResults; // Optimization results // Variance-Covariance matrix that estimates the localization error about the // 6-DoF parameters (DoF order : X, Y, Z, rX, rY, rZ) - KeypointsRegistration::RegistrationError LocalizationUncertainty; + LocalOptimizer::RegistrationError LocalizationUncertainty; // --------------------------------------------------------------------------- // Optimization parameters diff --git a/slam_lib/src/KeypointsRegistration.cxx b/slam_lib/src/KeypointsMatcher.cxx similarity index 80% rename from slam_lib/src/KeypointsRegistration.cxx rename to slam_lib/src/KeypointsMatcher.cxx index ed7e57162485272329100cb08980f57735f9ed1c..dbe8e0bed40b4dc918c2c4e5a3926746729bfc30 100644 --- a/slam_lib/src/KeypointsRegistration.cxx +++ b/slam_lib/src/KeypointsMatcher.cxx @@ -16,15 +16,15 @@ // limitations under the License. //============================================================================== -#include "LidarSlam/KeypointsRegistration.h" +#include "LidarSlam/KeypointsMatcher.h" #include "LidarSlam/CeresCostFunctions.h" namespace LidarSlam { //----------------------------------------------------------------------------- -KeypointsRegistration::KeypointsRegistration(const KeypointsRegistration::Parameters& params, - const Eigen::Isometry3d& posePrior) +KeypointsMatcher::KeypointsMatcher(const KeypointsMatcher::Parameters& params, + const Eigen::Isometry3d& posePrior) : Params(params) , PosePrior(posePrior) { @@ -33,12 +33,12 @@ KeypointsRegistration::KeypointsRegistration(const KeypointsRegistration::Parame } //----------------------------------------------------------------------------- -KeypointsRegistration::MatchingResults KeypointsRegistration::BuildAndMatchResiduals(const PointCloud::Ptr& currPoints, - const KDTree& prevPoints, - Keypoint keypointType) +KeypointsMatcher::MatchingResults KeypointsMatcher::BuildMatchResiduals(const PointCloud::Ptr& currPoints, + const KDTree& prevPoints, + Keypoint keypointType) { // Call the correct point-to-neighborhood method - auto BuildAndMatchSingleResidual = [&](const Point& currentPoint) + auto BuildMatchResidual = [&](const Point& currentPoint) { switch(keypointType) { @@ -49,15 +49,13 @@ KeypointsRegistration::MatchingResults KeypointsRegistration::BuildAndMatchResid case Keypoint::BLOB: return this->BuildBlobMatch(prevPoints, currentPoint); default: - return MatchingResults::MatchInfo{ MatchingResults::MatchStatus::UNKOWN, 0. }; + return MatchingResults::MatchInfo{ MatchingResults::MatchStatus::UNKOWN, 0., CeresTools::Residual() }; } }; // Reset matching results MatchingResults matchingResults; - matchingResults.Weights.assign(currPoints->size(), 0.); - matchingResults.Rejections.assign(currPoints->size(), MatchingResults::MatchStatus::UNKOWN); - matchingResults.RejectionsHistogram.fill(0); + matchingResults.Reset(currPoints->size()); // Loop over keypoints and try to build residuals if (!currPoints->empty() && prevPoints.GetInputCloud() && !prevPoints.GetInputCloud()->empty()) @@ -66,9 +64,10 @@ KeypointsRegistration::MatchingResults KeypointsRegistration::BuildAndMatchResid for (int ptIndex = 0; ptIndex < static_cast(currPoints->size()); ++ptIndex) { const Point& currentPoint = currPoints->points[ptIndex]; - const auto& match = BuildAndMatchSingleResidual(currentPoint); + const auto& match = BuildMatchResidual(currentPoint); matchingResults.Rejections[ptIndex] = match.Status; matchingResults.Weights[ptIndex] = match.Weight; + matchingResults.Residuals[ptIndex] = match.Cost; #pragma omp atomic matchingResults.RejectionsHistogram[match.Status]++; } @@ -77,70 +76,24 @@ KeypointsRegistration::MatchingResults KeypointsRegistration::BuildAndMatchResid return matchingResults; } -//---------------------------------------------------------------------------- -ceres::Solver::Summary KeypointsRegistration::Solve() -{ - ceres::Solver::Options options; - options.max_num_iterations = this->Params.LMMaxIter; - options.linear_solver_type = ceres::DENSE_QR; // TODO : try also DENSE_NORMAL_CHOLESKY or SPARSE_NORMAL_CHOLESKY - options.minimizer_progress_to_stdout = false; - options.num_threads = this->Params.NbThreads; - - ceres::Solver::Summary summary; - ceres::Solve(options, &(this->Problem), &summary); - return summary; -} - -//---------------------------------------------------------------------------- -KeypointsRegistration::RegistrationError KeypointsRegistration::EstimateRegistrationError() -{ - RegistrationError err; - - // Covariance computation options - ceres::Covariance::Options covOptions; - covOptions.apply_loss_function = true; - covOptions.algorithm_type = ceres::CovarianceAlgorithmType::DENSE_SVD; - covOptions.null_space_rank = -1; - covOptions.num_threads = this->Params.NbThreads; - - // Computation of the variance-covariance matrix - ceres::Covariance covarianceSolver(covOptions); - std::vector> covarianceBlocks; - const double* paramBlock = this->PoseArray.data(); - covarianceBlocks.emplace_back(paramBlock, paramBlock); - covarianceSolver.Compute(covarianceBlocks, &this->Problem); - covarianceSolver.GetCovarianceBlock(paramBlock, paramBlock, err.Covariance.data()); - - // Estimate max position/orientation errors and directions from covariance - Eigen::SelfAdjointEigenSolver eigPosition(err.Covariance.topLeftCorner<3, 3>()); - err.PositionError = std::sqrt(eigPosition.eigenvalues()(2)); - err.PositionErrorDirection = eigPosition.eigenvectors().col(2); - Eigen::SelfAdjointEigenSolver eigOrientation(err.Covariance.bottomRightCorner<3, 3>()); - err.OrientationError = Utils::Rad2Deg(std::sqrt(eigOrientation.eigenvalues()(2))); - err.OrientationErrorDirection = eigOrientation.eigenvectors().col(2); - - return err; -} //---------------------------------------------------------------------------- -void KeypointsRegistration::AddIcpResidual(const Eigen::Matrix3d& A, const Eigen::Vector3d& P, const Eigen::Vector3d& X, double weight) +CeresTools::Residual KeypointsMatcher::BuildResidual(const Eigen::Matrix3d& A, const Eigen::Vector3d& P, const Eigen::Vector3d& X, double weight) { + CeresTools::Residual res; // Create the point-to-line/plane/blob cost function - using Residual = CeresCostFunctions::MahalanobisDistanceAffineIsometryResidual; - ceres::CostFunction* costFunction = new ceres::AutoDiffCostFunction(new Residual(A, P, X)); + using Distance = CeresCostFunctions::MahalanobisDistanceAffineIsometryResidual; + res.Cost = new ceres::AutoDiffCostFunction(new Distance(A, P, X)); // Use a robustifier to limit the contribution of an outlier match auto* robustifier = new ceres::TukeyLoss(std::sqrt(this->Params.SaturationDistance)); // Weight the contribution of the given match by its reliability - auto* loss = new ceres::ScaledLoss(robustifier, weight, ceres::TAKE_OWNERSHIP); - - // Add weighted constraint to the problem - #pragma omp critical(addIcpResidual) - this->Problem.AddResidualBlock(costFunction, loss, this->PoseArray.data()); + res.Robustifier = new ceres::ScaledLoss(robustifier, weight, ceres::TAKE_OWNERSHIP); + return res; } //----------------------------------------------------------------------------- -KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildLineMatch(const KDTree& kdtreePreviousEdges, const Point& p) +KeypointsMatcher::MatchingResults::MatchInfo KeypointsMatcher::BuildLineMatch(const KDTree& kdtreePreviousEdges, const Point& p) { // ===================================================== // Transform the point using the current pose estimation @@ -164,14 +117,14 @@ KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildLi unsigned int neighborhoodSize = knnIndices.size(); if (neighborhoodSize < this->Params.MinimumLineNeighborRejection) { - return { MatchingResults::MatchStatus::NOT_ENOUGH_NEIGHBORS, 0. }; + return { MatchingResults::MatchStatus::NOT_ENOUGH_NEIGHBORS, 0., CeresTools::Residual() }; } // If the nearest edges are too far from the current edge keypoint, // we skip this point. if (knnSqDist.back() > this->Params.MaxDistanceForICPMatching * this->Params.MaxDistanceForICPMatching) { - return { MatchingResults::MatchStatus::NEIGHBORS_TOO_FAR, 0. }; + return { MatchingResults::MatchStatus::NEIGHBORS_TOO_FAR, 0., CeresTools::Residual() }; } // Shortcut to keypoints cloud @@ -193,7 +146,7 @@ KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildLi // Otherwise, discard this bad unstructured neighborhood. if (eigVals(2) < this->Params.LineDistancefactor * eigVals(1)) { - return { MatchingResults::MatchStatus::BAD_PCA_STRUCTURE, 0. }; + return { MatchingResults::MatchStatus::BAD_PCA_STRUCTURE, 0., CeresTools::Residual() }; } // ============================================= @@ -215,7 +168,7 @@ KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildLi // returns that hit the same point. if (!std::isfinite(A(0, 0))) { - return { MatchingResults::MatchStatus::INVALID_NUMERICAL, 0. }; + return { MatchingResults::MatchStatus::INVALID_NUMERICAL, 0., CeresTools::Residual() }; } // Evaluate the distance from the fitted line distribution of the neighborhood @@ -229,7 +182,7 @@ KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildLi // CHECK invalidate all neighborhood even if only one point is bad? if (squaredDist > squaredMaxDist) { - return { MatchingResults::MatchStatus::MSE_TOO_LARGE, 0. }; + return { MatchingResults::MatchStatus::MSE_TOO_LARGE, 0., CeresTools::Residual() }; } meanSquaredDist += squaredDist; } @@ -240,15 +193,12 @@ KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildLi // Quality score of the point-to-line match double fitQualityCoeff = 1.0 - std::sqrt(meanSquaredDist / squaredMaxDist); - - // Store the distance parameters values - this->AddIcpResidual(A, mean, localPoint, fitQualityCoeff); - - return { MatchingResults::MatchStatus::SUCCESS, fitQualityCoeff }; + CeresTools::Residual res = this->BuildResidual(A, mean, localPoint, fitQualityCoeff); + return { MatchingResults::MatchStatus::SUCCESS, fitQualityCoeff, res }; } //----------------------------------------------------------------------------- -KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildPlaneMatch(const KDTree& kdtreePreviousPlanes, const Point& p) +KeypointsMatcher::MatchingResults::MatchInfo KeypointsMatcher::BuildPlaneMatch(const KDTree& kdtreePreviousPlanes, const Point& p) { // ===================================================== // Transform the point using the current pose estimation @@ -268,14 +218,14 @@ KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildPl // It means that there is not enough keypoints in the neighborhood if (neighborhoodSize < this->Params.PlaneDistanceNbrNeighbors) { - return { MatchingResults::MatchStatus::NOT_ENOUGH_NEIGHBORS, 0. }; + return { MatchingResults::MatchStatus::NOT_ENOUGH_NEIGHBORS, 0., CeresTools::Residual() }; } // If the nearest planar points are too far from the current keypoint, // we skip this point. if (knnSqDist.back() > this->Params.MaxDistanceForICPMatching * this->Params.MaxDistanceForICPMatching) { - return { MatchingResults::MatchStatus::NEIGHBORS_TOO_FAR, 0. }; + return { MatchingResults::MatchStatus::NEIGHBORS_TOO_FAR, 0., CeresTools::Residual() }; } // Shortcut to keypoints cloud @@ -298,7 +248,7 @@ KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildPl if (this->Params.PlaneDistancefactor2 * eigVals(1) < eigVals(2) || eigVals(1) < this->Params.PlaneDistancefactor1 * eigVals(0)) { - return { MatchingResults::MatchStatus::BAD_PCA_STRUCTURE, 0. }; + return { MatchingResults::MatchStatus::BAD_PCA_STRUCTURE, 0., CeresTools::Residual() }; } // ============================================== @@ -312,7 +262,7 @@ KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildPl // sensor has some dual returns that hit the same point. if (!std::isfinite(A(0, 0))) { - return { MatchingResults::MatchStatus::INVALID_NUMERICAL, 0. }; + return { MatchingResults::MatchStatus::INVALID_NUMERICAL, 0., CeresTools::Residual() }; } // Evaluate the distance from the fitted plane distribution of the neighborhood @@ -326,7 +276,7 @@ KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildPl // CHECK invalidate all neighborhood even if only one point is bad? if (squaredDist > squaredMaxDist) { - return { MatchingResults::MatchStatus::MSE_TOO_LARGE, 0. }; + return { MatchingResults::MatchStatus::MSE_TOO_LARGE, 0., CeresTools::Residual() }; } meanSquaredDist += squaredDist; } @@ -337,15 +287,12 @@ KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildPl // Quality score of the point-to-plane match double fitQualityCoeff = 1.0 - std::sqrt(meanSquaredDist / squaredMaxDist); - - // Store the distance parameters values - this->AddIcpResidual(A, mean, localPoint, fitQualityCoeff); - - return { MatchingResults::MatchStatus::SUCCESS, fitQualityCoeff }; + CeresTools::Residual res = this->BuildResidual(A, mean, localPoint, fitQualityCoeff); + return { MatchingResults::MatchStatus::SUCCESS, fitQualityCoeff, res }; } //----------------------------------------------------------------------------- -KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildBlobMatch(const KDTree& kdtreePreviousBlobs, const Point& p) +KeypointsMatcher::MatchingResults::MatchInfo KeypointsMatcher::BuildBlobMatch(const KDTree& kdtreePreviousBlobs, const Point& p) { // ===================================================== // Transform the point using the current pose estimation @@ -368,14 +315,14 @@ KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildBl // It means that there is not enough keypoints in the neighborhood if (neighborhoodSize < this->Params.BlobDistanceNbrNeighbors) { - return { MatchingResults::MatchStatus::NOT_ENOUGH_NEIGHBORS, 0. }; + return { MatchingResults::MatchStatus::NOT_ENOUGH_NEIGHBORS, 0., CeresTools::Residual() }; } // If the nearest blob points are too far from the current keypoint, // we skip this point. if (knnSqDist.back() > this->Params.MaxDistanceForICPMatching * this->Params.MaxDistanceForICPMatching) { - return { MatchingResults::MatchStatus::NEIGHBORS_TOO_FAR, 0. }; + return { MatchingResults::MatchStatus::NEIGHBORS_TOO_FAR, 0., CeresTools::Residual() }; } // Shortcut to keypoints cloud @@ -400,7 +347,7 @@ KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildBl } if (squaredDiameter > maxDiameter * maxDiameter) { - return { MatchingResults::MatchStatus::MSE_TOO_LARGE, 0. }; + return { MatchingResults::MatchStatus::MSE_TOO_LARGE, 0., CeresTools::Residual() }; } // ====================================================== @@ -428,7 +375,7 @@ KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildBl // Check the determinant of the matrix if (!std::isfinite(eigValsSqrtInv.prod())) { - return { MatchingResults::MatchStatus::INVALID_NUMERICAL, 0. }; + return { MatchingResults::MatchStatus::INVALID_NUMERICAL, 0., CeresTools::Residual() }; } // =========================================== @@ -437,15 +384,12 @@ KeypointsRegistration::MatchingResults::MatchInfo KeypointsRegistration::BuildBl // Quality score of the point-to-blob match // The aim is to prevent wrong matching pulling the pointcloud in a bad direction. double fitQualityCoeff = 1.0;//1.0 - knnSqDist.back() / maxDist; - - // store the distance parameters values - this->AddIcpResidual(A, mean, localPoint, fitQualityCoeff); - - return { MatchingResults::MatchStatus::SUCCESS, fitQualityCoeff }; + CeresTools::Residual res = this->BuildResidual(A, mean, localPoint, fitQualityCoeff); + return { MatchingResults::MatchStatus::SUCCESS, fitQualityCoeff, res }; } //----------------------------------------------------------------------------- -void KeypointsRegistration::GetPerRingLineNeighbors(const KDTree& kdtreePreviousEdges, const double pos[3], unsigned int knearest, +void KeypointsMatcher::GetPerRingLineNeighbors(const KDTree& kdtreePreviousEdges, const double pos[3], unsigned int knearest, std::vector& validKnnIndices, std::vector& validKnnSqDist) const { // Get nearest neighbors of the query point @@ -504,8 +448,8 @@ void KeypointsRegistration::GetPerRingLineNeighbors(const KDTree& kdtreePrevious } //----------------------------------------------------------------------------- -void KeypointsRegistration::GetRansacLineNeighbors(const KDTree& kdtreePreviousEdges, const double pos[3], unsigned int knearest, double maxDistInlier, - std::vector& validKnnIndices, std::vector& validKnnSqDist) const +void KeypointsMatcher::GetRansacLineNeighbors(const KDTree& kdtreePreviousEdges, const double pos[3], unsigned int knearest, double maxDistInlier, + std::vector& validKnnIndices, std::vector& validKnnSqDist) const { // Get nearest neighbors of the query point std::vector knnIndices; diff --git a/slam_lib/src/LocalOptimizer.cxx b/slam_lib/src/LocalOptimizer.cxx new file mode 100644 index 0000000000000000000000000000000000000000..ce8f8259769d294926fb2aba317f0fb63e554cdc --- /dev/null +++ b/slam_lib/src/LocalOptimizer.cxx @@ -0,0 +1,120 @@ +//============================================================================== +// Copyright 2019-2020 Kitware, Inc., Kitware SAS +// Author: Sanchez Julia (Kitware SAS) +// Creation date: 2021-03-01 +// +// 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 "LidarSlam/LocalOptimizer.h" +#include "LidarSlam/CeresCostFunctions.h" + +namespace LidarSlam +{ + +//---------------------------------------------------------------------------- +// Set params +//---------------------------------------------------------------------------- +// Set Levenberg Marquardt maximum number of iterations +void LocalOptimizer::SetLMMaxIter(const unsigned int maxIt) +{ + this->LMMaxIter = maxIt; +} + +// Set maximum number of iterations for Levenberg Marquardt algorithm +void LocalOptimizer::SetNbThreads(const unsigned int nbThreads) +{ + this->NbThreads = nbThreads; +} + +void LocalOptimizer::SetPosePrior(const Eigen::Isometry3d& posePrior) +{ + // Convert isometry to 6D state vector : X, Y, Z, rX, rY, rZ + this->PoseArray = Utils::IsometryToXYZRPY(posePrior); +} + +//---------------------------------------------------------------------------- +// Set residuals +//---------------------------------------------------------------------------- + +void LocalOptimizer::AddResiduals(std::vector& lidarRes) +{ + this->Residuals.insert(this->Residuals.end(), lidarRes.begin(), lidarRes.end()); +} + +//---------------------------------------------------------------------------- +// Clear all residuals at each ICP step +void LocalOptimizer::Clear() +{ + this->Residuals.clear(); +} + +//---------------------------------------------------------------------------- +ceres::Solver::Summary LocalOptimizer::Solve() +{ + this->Problem = std::make_unique(); + for (CeresTools::Residual& res : this->Residuals) + { + if (res.Cost) + this->Problem->AddResidualBlock(res.Cost, res.Robustifier, this->PoseArray.data()); + } + + for (CeresTools::Residual& res : this->Residuals) + { + if (res.Cost) + this->Problem->AddResidualBlock(res.Cost, res.Robustifier, this->PoseArray.data()); + } + + ceres::Solver::Options options; + options.max_num_iterations = this->LMMaxIter; + options.linear_solver_type = ceres::DENSE_QR; // TODO : try also DENSE_NORMAL_CHOLESKY or SPARSE_NORMAL_CHOLESKY + options.minimizer_progress_to_stdout = false; + options.num_threads = this->NbThreads; + + ceres::Solver::Summary summary; + ceres::Solve(options, &(*this->Problem), &summary); + return summary; +} + +//---------------------------------------------------------------------------- +LocalOptimizer::RegistrationError LocalOptimizer::EstimateRegistrationError() +{ + RegistrationError err; + + // Covariance computation options + ceres::Covariance::Options covOptions; + covOptions.apply_loss_function = true; + covOptions.algorithm_type = ceres::CovarianceAlgorithmType::DENSE_SVD; + covOptions.null_space_rank = -1; + covOptions.num_threads = this->NbThreads; + + // Computation of the variance-covariance matrix + ceres::Covariance covarianceSolver(covOptions); + std::vector> covarianceBlocks; + const double* paramBlock = this->PoseArray.data(); + covarianceBlocks.emplace_back(paramBlock, paramBlock); + covarianceSolver.Compute(covarianceBlocks, &(*this->Problem)); + covarianceSolver.GetCovarianceBlock(paramBlock, paramBlock, err.Covariance.data()); + + // Estimate max position/orientation errors and directions from covariance + Eigen::SelfAdjointEigenSolver eigPosition(err.Covariance.topLeftCorner<3, 3>()); + err.PositionError = std::sqrt(eigPosition.eigenvalues()(2)); + err.PositionErrorDirection = eigPosition.eigenvectors().col(2); + Eigen::SelfAdjointEigenSolver eigOrientation(err.Covariance.bottomRightCorner<3, 3>()); + err.OrientationError = Utils::Rad2Deg(std::sqrt(eigOrientation.eigenvalues()(2))); + err.OrientationErrorDirection = eigOrientation.eigenvectors().col(2); + + return err; +} + +} // end of LidarSlam namespace \ No newline at end of file diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx index 5fad5d67dd7e8eb4b7449942f62f550865f7f379..ab9cabe0990bcbc13e8e51c6180fdedefe4c832f 100644 --- a/slam_lib/src/Slam.cxx +++ b/slam_lib/src/Slam.cxx @@ -164,7 +164,7 @@ void Slam::Reset(bool resetLog) this->WithinFrameMotion.SetTransforms(Eigen::Isometry3d::Identity(), Eigen::Isometry3d::Identity()); // Reset pose uncertainty - this->LocalizationUncertainty = KeypointsRegistration::RegistrationError(); + this->LocalizationUncertainty = LocalOptimizer::RegistrationError(); // Reset point clouds this->CurrentFrames.clear(); @@ -178,9 +178,9 @@ void Slam::Reset(bool resetLog) // Reset keypoints matching results for (auto k : {EDGE, PLANE}) - this->EgoMotionMatchingResults[k] = KeypointsRegistration::MatchingResults(); + this->EgoMotionMatchingResults[k] = KeypointsMatcher::MatchingResults(); for (auto k : KeypointTypes) - this->LocalizationMatchingResults[k] = KeypointsRegistration::MatchingResults(); + this->LocalizationMatchingResults[k] = KeypointsMatcher::MatchingResults(); // Reset log history if (resetLog) @@ -195,6 +195,16 @@ void Slam::Reset(bool resetLog) } } +//----------------------------------------------------------------------------- +void Slam::SetNbThreads(int n) +{ + // Set number of threads for main processes + this->NbThreads = n; + // Set number of threads for keypoints extraction + for (const auto& kv : this->KeyPointsExtractors) + kv.second->SetNbThreads(n); +} + //----------------------------------------------------------------------------- void Slam::AddFrames(const std::vector& frames) { @@ -837,6 +847,21 @@ void Slam::ComputeEgoMotion() // Reset ICP results unsigned int totalMatchedKeypoints = 0; + // Init matching parameters + KeypointsMatcher::Parameters matchingParams; + matchingParams.NbThreads = this->NbThreads; + matchingParams.SingleEdgePerRing = true; + matchingParams.MaxDistanceForICPMatching = this->MaxDistanceForICPMatching; + matchingParams.MinNbrMatchedKeypoints = this->MinNbrMatchedKeypoints; + matchingParams.LineDistanceNbrNeighbors = this->EgoMotionLineDistanceNbrNeighbors; + matchingParams.MinimumLineNeighborRejection = this->EgoMotionMinimumLineNeighborRejection; + matchingParams.LineDistancefactor = this->EgoMotionLineDistancefactor; + matchingParams.MaxLineDistance = this->EgoMotionMaxLineDistance; + matchingParams.PlaneDistanceNbrNeighbors = this->EgoMotionPlaneDistanceNbrNeighbors; + matchingParams.PlaneDistancefactor1 = this->EgoMotionPlaneDistancefactor1; + matchingParams.PlaneDistancefactor2 = this->EgoMotionPlaneDistancefactor2; + matchingParams.MaxPlaneDistance = this->EgoMotionMaxPlaneDistance; + // ICP - Levenberg-Marquardt loop // At each step of this loop an ICP matching is performed. Once the keypoints // are matched, we estimate the the 6-DOF parameters by minimizing the @@ -851,28 +876,16 @@ void Slam::ComputeEgoMotion() // First, we need to build the point-to-line, point-to-plane and // point-to-blob ICP matches that will be optimized. // Then, we use CERES Levenberg-Marquardt optimization to minimize the problem. - KeypointsRegistration::Parameters optimParams; - optimParams.NbThreads = this->NbThreads; - optimParams.SingleEdgePerRing = true; - optimParams.MaxDistanceForICPMatching = this->MaxDistanceForICPMatching; - optimParams.MinNbrMatchedKeypoints = this->MinNbrMatchedKeypoints; - optimParams.LineDistanceNbrNeighbors = this->EgoMotionLineDistanceNbrNeighbors; - optimParams.MinimumLineNeighborRejection = this->EgoMotionMinimumLineNeighborRejection; - optimParams.LineDistancefactor = this->EgoMotionLineDistancefactor; - optimParams.MaxLineDistance = this->EgoMotionMaxLineDistance; - optimParams.PlaneDistanceNbrNeighbors = this->EgoMotionPlaneDistanceNbrNeighbors; - optimParams.PlaneDistancefactor1 = this->EgoMotionPlaneDistancefactor1; - optimParams.PlaneDistancefactor2 = this->EgoMotionPlaneDistancefactor2; - optimParams.MaxPlaneDistance = this->EgoMotionMaxPlaneDistance; - optimParams.LMMaxIter = this->EgoMotionLMMaxIter; - double iterRatio = icpIter / static_cast(this->EgoMotionICPMaxIter - 1); - optimParams.SaturationDistance = (1 - iterRatio) * this->EgoMotionInitSaturationDistance + iterRatio * this->EgoMotionFinalSaturationDistance; - KeypointsRegistration optim(optimParams, this->Trelative); + // Create a keypoints matcher + // At each ICP iteration, the outliers removal is refined to be stricter + double iterRatio = icpIter / static_cast(this->EgoMotionICPMaxIter - 1); + matchingParams.SaturationDistance = (1 - iterRatio) * this->EgoMotionInitSaturationDistance + iterRatio * this->EgoMotionFinalSaturationDistance; + KeypointsMatcher matcher(matchingParams, this->Trelative); // Loop over keypoints to build the residuals for (auto k : {EDGE, PLANE}) - this->EgoMotionMatchingResults[k] = optim.BuildAndMatchResiduals(this->CurrentRawKeypoints[k], kdtreePrevious[k], k); + this->EgoMotionMatchingResults[k] = matcher.BuildMatchResiduals(this->CurrentRawKeypoints[k], kdtreePrevious[k], k); // Count matches and skip this frame // if there are too few geometric keypoints matched @@ -889,12 +902,21 @@ void Slam::ComputeEgoMotion() IF_VERBOSE(3, Utils::Timer::StopAndDisplay(" Ego-Motion : ICP")); IF_VERBOSE(3, Utils::Timer::Init(" Ego-Motion : LM optim")); + LocalOptimizer optimizer; + // Set number of optimization iterations for a set of matches + optimizer.SetLMMaxIter(this->EgoMotionLMMaxIter); + // Set number of threads for optimization + optimizer.SetNbThreads(this->NbThreads); + // Set pose prior + optimizer.SetPosePrior(this->Trelative); + for (auto k : {EDGE, PLANE}) + optimizer.AddResiduals(this->EgoMotionMatchingResults[k].Residuals); // Run LM optimization - ceres::Solver::Summary summary = optim.Solve(); + ceres::Solver::Summary summary = optimizer.Solve(); PRINT_VERBOSE(4, summary.BriefReport()); // Get back optimized Trelative - this->Trelative = optim.GetOptimizedPose(); + this->Trelative = optimizer.GetOptimizedPose(); IF_VERBOSE(3, Utils::Timer::StopAndDisplay(" Ego-Motion : LM optim")); @@ -1000,6 +1022,22 @@ void Slam::Localization() // Reset ICP results unsigned int totalMatchedKeypoints = 0; + // Init matching parameters + KeypointsMatcher::Parameters matchingParams; + matchingParams.NbThreads = this->NbThreads; + matchingParams.SingleEdgePerRing = false; + matchingParams.MaxDistanceForICPMatching = this->MaxDistanceForICPMatching; + matchingParams.MinNbrMatchedKeypoints = this->MinNbrMatchedKeypoints; + matchingParams.LineDistanceNbrNeighbors = this->LocalizationLineDistanceNbrNeighbors; + matchingParams.MinimumLineNeighborRejection = this->LocalizationMinimumLineNeighborRejection; + matchingParams.LineDistancefactor = this->LocalizationLineDistancefactor; + matchingParams.MaxLineDistance = this->LocalizationMaxLineDistance; + matchingParams.PlaneDistanceNbrNeighbors = this->LocalizationPlaneDistanceNbrNeighbors; + matchingParams.PlaneDistancefactor1 = this->LocalizationPlaneDistancefactor1; + matchingParams.PlaneDistancefactor2 = this->LocalizationPlaneDistancefactor2; + matchingParams.MaxPlaneDistance = this->LocalizationMaxPlaneDistance; + matchingParams.BlobDistanceNbrNeighbors = this->LocalizationBlobDistanceNbrNeighbors; + // ICP - Levenberg-Marquardt loop // At each step of this loop an ICP matching is performed. Once the keypoints // are matched, we estimate the the 6-DOF parameters by minimizing the @@ -1014,29 +1052,16 @@ void Slam::Localization() // First, we need to build the point-to-line, point-to-plane and // point-to-blob ICP matches that will be optimized. // Then, we use CERES Levenberg-Marquardt optimization to minimize problem. - KeypointsRegistration::Parameters optimParams; - optimParams.NbThreads = this->NbThreads; - optimParams.SingleEdgePerRing = false; - optimParams.MaxDistanceForICPMatching = this->MaxDistanceForICPMatching; - optimParams.MinNbrMatchedKeypoints = this->MinNbrMatchedKeypoints; - optimParams.LineDistanceNbrNeighbors = this->LocalizationLineDistanceNbrNeighbors; - optimParams.MinimumLineNeighborRejection = this->LocalizationMinimumLineNeighborRejection; - optimParams.LineDistancefactor = this->LocalizationLineDistancefactor; - optimParams.MaxLineDistance = this->LocalizationMaxLineDistance; - optimParams.PlaneDistanceNbrNeighbors = this->LocalizationPlaneDistanceNbrNeighbors; - optimParams.PlaneDistancefactor1 = this->LocalizationPlaneDistancefactor1; - optimParams.PlaneDistancefactor2 = this->LocalizationPlaneDistancefactor2; - optimParams.MaxPlaneDistance = this->LocalizationMaxPlaneDistance; - optimParams.BlobDistanceNbrNeighbors = this->LocalizationBlobDistanceNbrNeighbors; - optimParams.LMMaxIter = this->LocalizationLMMaxIter; - double iterRatio = icpIter / static_cast(this->LocalizationICPMaxIter - 1); - optimParams.SaturationDistance = (1 - iterRatio) * this->LocalizationInitSaturationDistance + iterRatio * this->LocalizationFinalSaturationDistance; - KeypointsRegistration optim(optimParams, this->Tworld); + // Create a keypoints matcher + // At each ICP iteration, the outliers removal is refined to be stricter + double iterRatio = icpIter / static_cast(this->LocalizationICPMaxIter - 1); + matchingParams.SaturationDistance = (1 - iterRatio) * this->LocalizationInitSaturationDistance + iterRatio * this->LocalizationFinalSaturationDistance; + KeypointsMatcher matcher(matchingParams, this->Tworld); // Loop over keypoints to build the point to line residuals for (auto k : KeypointTypes) - this->LocalizationMatchingResults[k] = optim.BuildAndMatchResiduals(this->CurrentUndistortedKeypoints[k], kdtrees[k], k); + this->LocalizationMatchingResults[k] = matcher.BuildMatchResiduals(this->CurrentUndistortedKeypoints[k], kdtrees[k], k); // Count matches and skip this frame // if there is too few geometric keypoints matched @@ -1058,12 +1083,24 @@ void Slam::Localization() IF_VERBOSE(3, Utils::Timer::StopAndDisplay(" Localization : ICP")); IF_VERBOSE(3, Utils::Timer::Init(" Localization : LM optim")); + LocalOptimizer optimizer; + // Set number of optimization iterations for a set of matches + optimizer.SetLMMaxIter(this->LocalizationLMMaxIter); + // Set number of threads for optimization + optimizer.SetNbThreads(this->NbThreads); + // Set pose prior + optimizer.SetPosePrior(this->Tworld); + for (auto k : KeypointTypes) + optimizer.AddResiduals(this->LocalizationMatchingResults[k].Residuals); + + // *********************Add sensor constraints here********************* + // Run LM optimization - ceres::Solver::Summary summary = optim.Solve(); + ceres::Solver::Summary summary = optimizer.Solve(); PRINT_VERBOSE(4, summary.BriefReport()); // Update Tworld and Trelative from optimization results - this->Tworld = optim.GetOptimizedPose(); + this->Tworld = optimizer.GetOptimizedPose(); this->Trelative = this->PreviousTworld.inverse() * this->Tworld; // Optionally refine undistortion @@ -1078,7 +1115,7 @@ void Slam::Localization() // computation of the variance covariance matrix. if ((summary.num_successful_steps == 1) || (icpIter == this->LocalizationICPMaxIter - 1)) { - this->LocalizationUncertainty = optim.EstimateRegistrationError(); + this->LocalizationUncertainty = optimizer.EstimateRegistrationError(); break; } }