From 8c619770803bd9e0885eae2acd4b7d4b08992888 Mon Sep 17 00:00:00 2001 From: Julia Sanchez Date: Fri, 26 Mar 2021 19:09:36 +0100 Subject: [PATCH 1/2] [feat] Split Matching and optimization * Split KeypointsRegistration file into LocalOptimizer and KeypointsMatcher files. * LocalOptimizer * It is a member of the SLAM class. * A residual structure is created in this file. It contains pointers to a cost function and a loss function (optimization estimator to remove outliers). * It receives the ceres LiDAR residuals. * It will be able to receive other sensors' residuals. * It is entirely cleared at each registration step. * It contains a ceres problem pointer (the destructor is updated). This problem is created and filled after each new matching step of ICP. The problem is conserved in order to be able to recover covariance from the optimization after convergence for last ICP iteration. * KeypointsMatcher * It is an object created locally in Ego-Motion and/or Localization functions. * It builds the LiDAR residuals (as described above) from matching between frame keypoints and map neighborhoods. * It contains a vector of residuals (directly added in the matching result structure). * A residual is also added to the matching info structure. --- slam_lib/CMakeLists.txt | 6 +- .../include/LidarSlam/CeresCostFunctions.h | 10 +- ...ointsRegistration.h => KeypointsMatcher.h} | 78 +++------- slam_lib/include/LidarSlam/LocalOptimizer.h | 103 +++++++++++++ slam_lib/include/LidarSlam/Slam.h | 11 +- ...sRegistration.cxx => KeypointsMatcher.cxx} | 142 ++++++------------ slam_lib/src/LocalOptimizer.cxx | 136 +++++++++++++++++ slam_lib/src/Slam.cxx | 133 ++++++++++------ 8 files changed, 414 insertions(+), 205 deletions(-) rename slam_lib/include/LidarSlam/{KeypointsRegistration.h => KeypointsMatcher.h} (77%) create mode 100644 slam_lib/include/LidarSlam/LocalOptimizer.h rename slam_lib/src/{KeypointsRegistration.cxx => KeypointsMatcher.cxx} (80%) create mode 100644 slam_lib/src/LocalOptimizer.cxx diff --git a/slam_lib/CMakeLists.txt b/slam_lib/CMakeLists.txt index cf856297..2f842f60 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 4fbdc119..41ea0152 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 da0cbfe6..16ae70c9 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 00000000..54cedf9f --- /dev/null +++ b/slam_lib/include/LidarSlam/LocalOptimizer.h @@ -0,0 +1,103 @@ +//============================================================================== +// 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 Lidar residuals to residuals vector + void AddLidarResiduals(std::vector& lidarRes); + + // Add Lidar residuals to residuals vector + void AddSensorResiduals(std::vector& sensorRes); + + // Clear Lidar residuals (at each ICP iteration) + void ClearLidarResiduals(); + + // Clear all residuals (at each new frame) + 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) + + // Residual vectors + // These residuals must involve the full 6D pose array (X, Y, Z, rX, rY, rZ) + std::vector LidarResiduals; + std::vector SensorResiduals; + + 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 892437c9..c2ef4b53 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 ed7e5716..dbe8e0be 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 00000000..cdec6883 --- /dev/null +++ b/slam_lib/src/LocalOptimizer.cxx @@ -0,0 +1,136 @@ +//============================================================================== +// 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::AddLidarResiduals(std::vector& lidarRes) +{ + this->LidarResiduals.insert(this->LidarResiduals.end(), lidarRes.begin(), lidarRes.end()); +} + +//---------------------------------------------------------------------------- +void LocalOptimizer::AddSensorResiduals(std::vector& sensorRes) +{ + this->SensorResiduals.insert(this->SensorResiduals.end(), sensorRes.begin(), sensorRes.end()); +} + +//---------------------------------------------------------------------------- +// Clear +//---------------------------------------------------------------------------- +// Clear LiDAR residuals at each ICP iteration +void LocalOptimizer::ClearLidarResiduals() +{ + this->LidarResiduals.clear(); +} + +//---------------------------------------------------------------------------- +// Clear all residuals at each new frame entry +void LocalOptimizer::Clear() +{ + this->LidarResiduals.clear(); + this->SensorResiduals.clear(); +} + +//---------------------------------------------------------------------------- +ceres::Solver::Summary LocalOptimizer::Solve() +{ + this->Problem = std::make_unique(); + for (CeresTools::Residual& res : this->LidarResiduals) + { + if (res.Cost) + this->Problem->AddResidualBlock(res.Cost, res.Robustifier, this->PoseArray.data()); + } + + for (CeresTools::Residual& res : this->SensorResiduals) + { + 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 5fad5d67..c152e6e6 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,27 @@ void Slam::ComputeEgoMotion() // Reset ICP results unsigned int totalMatchedKeypoints = 0; + 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); + + // 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 +882,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 +908,18 @@ void Slam::ComputeEgoMotion() IF_VERBOSE(3, Utils::Timer::StopAndDisplay(" Ego-Motion : ICP")); IF_VERBOSE(3, Utils::Timer::Init(" Ego-Motion : LM optim")); + // Set pose prior + optimizer.SetPosePrior(this->Trelative); + // Reset Lidar part of optimizer + optimizer.ClearLidarResiduals(); + 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 +1025,30 @@ void Slam::Localization() // Reset ICP results unsigned int totalMatchedKeypoints = 0; + 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); + + // *********************Add sensor constraints here********************* + + // 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 +1063,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 +1094,19 @@ void Slam::Localization() IF_VERBOSE(3, Utils::Timer::StopAndDisplay(" Localization : ICP")); IF_VERBOSE(3, Utils::Timer::Init(" Localization : LM optim")); + // Set pose prior + optimizer.SetPosePrior(this->Tworld); + // Reset Lidar part of optimizer + optimizer.ClearLidarResiduals(); + for (auto k : KeypointTypes) + optimizer.AddLidarResiduals(this->LocalizationMatchingResults[k].Residuals); + // 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 +1121,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; } } -- GitLab From dc9684b15751270341a76c4554d1bccb18ba8318 Mon Sep 17 00:00:00 2001 From: Julia Sanchez Date: Fri, 26 Mar 2021 19:12:21 +0100 Subject: [PATCH 2/2] [refact] Gather sensor residuals * Lidar and external sensor residuals are gathered in the same vector * This vector is refilled entirely at each ICP step --- slam_lib/include/LidarSlam/LocalOptimizer.h | 17 ++++------- slam_lib/src/LocalOptimizer.cxx | 30 +++++-------------- slam_lib/src/Slam.cxx | 32 +++++++++------------ 3 files changed, 25 insertions(+), 54 deletions(-) diff --git a/slam_lib/include/LidarSlam/LocalOptimizer.h b/slam_lib/include/LidarSlam/LocalOptimizer.h index 54cedf9f..bf41d696 100644 --- a/slam_lib/include/LidarSlam/LocalOptimizer.h +++ b/slam_lib/include/LidarSlam/LocalOptimizer.h @@ -59,16 +59,10 @@ public: // Set prior pose void SetPosePrior(const Eigen::Isometry3d& posePrior); - // Add Lidar residuals to residuals vector - void AddLidarResiduals(std::vector& lidarRes); + // Add Sensor residuals to residuals vector + void AddResiduals(std::vector& sensorRes); - // Add Lidar residuals to residuals vector - void AddSensorResiduals(std::vector& sensorRes); - - // Clear Lidar residuals (at each ICP iteration) - void ClearLidarResiduals(); - - // Clear all residuals (at each new frame) + // Clear all residuals void Clear(); // Build and optimize the Ceres problem @@ -92,10 +86,9 @@ private: // DoF to optimize (= output) Eigen::Vector6d PoseArray; ///< Pose parameters to optimize (XYZRPY) - // Residual vectors + // Residuals vector // These residuals must involve the full 6D pose array (X, Y, Z, rX, rY, rZ) - std::vector LidarResiduals; - std::vector SensorResiduals; + std::vector Residuals; std::unique_ptr Problem; }; diff --git a/slam_lib/src/LocalOptimizer.cxx b/slam_lib/src/LocalOptimizer.cxx index cdec6883..ce8f8259 100644 --- a/slam_lib/src/LocalOptimizer.cxx +++ b/slam_lib/src/LocalOptimizer.cxx @@ -47,45 +47,29 @@ void LocalOptimizer::SetPosePrior(const Eigen::Isometry3d& posePrior) // Set residuals //---------------------------------------------------------------------------- -void LocalOptimizer::AddLidarResiduals(std::vector& lidarRes) +void LocalOptimizer::AddResiduals(std::vector& lidarRes) { - this->LidarResiduals.insert(this->LidarResiduals.end(), lidarRes.begin(), lidarRes.end()); + this->Residuals.insert(this->Residuals.end(), lidarRes.begin(), lidarRes.end()); } //---------------------------------------------------------------------------- -void LocalOptimizer::AddSensorResiduals(std::vector& sensorRes) -{ - this->SensorResiduals.insert(this->SensorResiduals.end(), sensorRes.begin(), sensorRes.end()); -} - -//---------------------------------------------------------------------------- -// Clear -//---------------------------------------------------------------------------- -// Clear LiDAR residuals at each ICP iteration -void LocalOptimizer::ClearLidarResiduals() -{ - this->LidarResiduals.clear(); -} - -//---------------------------------------------------------------------------- -// Clear all residuals at each new frame entry +// Clear all residuals at each ICP step void LocalOptimizer::Clear() { - this->LidarResiduals.clear(); - this->SensorResiduals.clear(); + this->Residuals.clear(); } //---------------------------------------------------------------------------- ceres::Solver::Summary LocalOptimizer::Solve() { this->Problem = std::make_unique(); - for (CeresTools::Residual& res : this->LidarResiduals) + for (CeresTools::Residual& res : this->Residuals) { if (res.Cost) this->Problem->AddResidualBlock(res.Cost, res.Robustifier, this->PoseArray.data()); } - - for (CeresTools::Residual& res : this->SensorResiduals) + + for (CeresTools::Residual& res : this->Residuals) { if (res.Cost) this->Problem->AddResidualBlock(res.Cost, res.Robustifier, this->PoseArray.data()); diff --git a/slam_lib/src/Slam.cxx b/slam_lib/src/Slam.cxx index c152e6e6..ab9cabe0 100644 --- a/slam_lib/src/Slam.cxx +++ b/slam_lib/src/Slam.cxx @@ -847,12 +847,6 @@ void Slam::ComputeEgoMotion() // Reset ICP results unsigned int totalMatchedKeypoints = 0; - 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); - // Init matching parameters KeypointsMatcher::Parameters matchingParams; matchingParams.NbThreads = this->NbThreads; @@ -908,10 +902,13 @@ 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); - // Reset Lidar part of optimizer - optimizer.ClearLidarResiduals(); for (auto k : {EDGE, PLANE}) optimizer.AddResiduals(this->EgoMotionMatchingResults[k].Residuals); // Run LM optimization @@ -1025,14 +1022,6 @@ void Slam::Localization() // Reset ICP results unsigned int totalMatchedKeypoints = 0; - 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); - - // *********************Add sensor constraints here********************* - // Init matching parameters KeypointsMatcher::Parameters matchingParams; matchingParams.NbThreads = this->NbThreads; @@ -1094,12 +1083,17 @@ 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); - // Reset Lidar part of optimizer - optimizer.ClearLidarResiduals(); for (auto k : KeypointTypes) - optimizer.AddLidarResiduals(this->LocalizationMatchingResults[k].Residuals); + optimizer.AddResiduals(this->LocalizationMatchingResults[k].Residuals); + + // *********************Add sensor constraints here********************* // Run LM optimization ceres::Solver::Summary summary = optimizer.Solve(); -- GitLab