From dbd3c629c1f7eb85c8ff6638418ddfe99ec45ae7 Mon Sep 17 00:00:00 2001 From: ZhuLingfeng1993 <664675691@qq.com> Date: Wed, 17 Dec 2025 14:00:34 +0800 Subject: [PATCH] Improve nurbs surface fitting efficiency with Eigen sparse matrix solver Compared to the existing Eigen dense solver, using the Eigen sparse solver significantly reduces solution time and memory usage, without relying on UMFPACK/SuiteSparse libraries that are based on the GPL open source license. --- .../src/on_nurbs/nurbs_solve_eigen_sparse.cpp | 225 ++++++++++++++++++ surface/src/on_nurbs/on_nurbs.cmake | 6 +- 2 files changed, 230 insertions(+), 1 deletion(-) create mode 100644 surface/src/on_nurbs/nurbs_solve_eigen_sparse.cpp diff --git a/surface/src/on_nurbs/nurbs_solve_eigen_sparse.cpp b/surface/src/on_nurbs/nurbs_solve_eigen_sparse.cpp new file mode 100644 index 00000000000..47afdb00047 --- /dev/null +++ b/surface/src/on_nurbs/nurbs_solve_eigen_sparse.cpp @@ -0,0 +1,225 @@ +#include +#include + +#include + +#include +#include + +#include +#include + +using namespace pcl; +using namespace on_nurbs; + +void +NurbsSolve::assign(unsigned rows, unsigned cols, unsigned dims) +{ + m_Ksparse.clear(); + m_xeig = Eigen::MatrixXd::Zero(cols, dims); + m_feig = Eigen::MatrixXd::Zero(rows, dims); +} + +void +NurbsSolve::K(unsigned i, unsigned j, double v) +{ + m_Ksparse.set(i, j, v); +} + +void +NurbsSolve::x(unsigned i, unsigned j, double v) +{ + m_xeig(i, j) = v; +} + +void +NurbsSolve::f(unsigned i, unsigned j, double v) +{ + m_feig(i, j) = v; +} + +double +NurbsSolve::K(unsigned i, unsigned j) +{ + return m_Ksparse.get(i, j); +} + +double +NurbsSolve::x(unsigned i, unsigned j) +{ + return m_xeig(i, j); +} + +double +NurbsSolve::f(unsigned i, unsigned j) +{ + return m_feig(i, j); +} + +void +NurbsSolve::resize(unsigned rows) +{ + m_feig.conservativeResize(rows, m_feig.cols()); + // Note: m_Ksparse is not resized here; assumed handled by assign() +} + +void +NurbsSolve::printK() +{ + m_Ksparse.printLong(); +} + +void +NurbsSolve::printX() +{ + std::cout << m_xeig << std::endl; +} + +void +NurbsSolve::printF() +{ + std::cout << m_feig << std::endl; +} + +bool +NurbsSolve::solve() +{ + auto start_time = std::chrono::high_resolution_clock::now(); + if (!m_quiet) { + std::cout << "[NurbsSolve] Start solving..." << std::endl; + } + + int n_rows, n_cols; + m_Ksparse.size(n_rows, n_cols); + + unsigned rows, cols, dims; + getSize(rows, cols, dims); + + if (n_rows <= 0 || n_cols <= 0) { + if (!m_quiet) + std::cerr << "[NurbsSolve::solve] Invalid matrix size." << std::endl; + return false; + } + + // Convert SparseMat to Eigen::SparseMatrix + std::vector rowinds, colinds; + std::vector values; + m_Ksparse.get(rowinds, colinds, values); + + // Use triplet list for efficient construction + std::vector> tripletList; + tripletList.reserve(values.size()); + for (size_t k = 0; k < values.size(); ++k) { + tripletList.emplace_back(rowinds[k], colinds[k], values[k]); + } + + Eigen::SparseMatrix Keig_sparse(n_rows, n_cols); + Keig_sparse.setFromTriplets(tripletList.begin(), tripletList.end()); + Keig_sparse.makeCompressed(); + + // Choose solver +// std::string solver_type = "Eigen::SparseQR"; +// Eigen::SparseQR, Eigen::COLAMDOrdering> solver; + + +// // NOTE: SparseLU may get wrong result in windows +// std::string solver_type = "Eigen::SparseLU COLAMDOrdering"; +// Eigen::SparseLU, Eigen::COLAMDOrdering> solver; +// std::string solver_type = "Eigen::SparseLU AMDOrdering"; +// Eigen::SparseLU, Eigen::AMDOrdering> solver; + + std::string solver_type = "Eigen::SimplicialLDLT"; + Eigen::SimplicialLDLT> solver; + + if (!m_quiet) { + std::cout << "[NurbsSolve::solve] solver_type: " << solver_type << std::endl; + } + + Eigen::SparseMatrix KtK; + Eigen::MatrixXd Ktf; + Eigen::SparseMatrix Kt; + + // For least-squares: solve min ||K * x - f||^2 + // We solve normal equations: (K^T K) x = K^T f + Kt = Keig_sparse.transpose(); + KtK = Kt * Keig_sparse; + Ktf = Kt * m_feig; + + // Solve KtK * x = Ktf + solver.compute(KtK); + if (solver.info() != Eigen::Success) { + if (!m_quiet) { + std::cerr << "[NurbsSolve::solve] compute failed" << std::endl; + std::cerr << "[NurbsSolve::solve] solver.info: " << solver.info() << std::endl; + } + return false; + } + + m_xeig = solver.solve(Ktf); + if (solver.info() != Eigen::Success) { + if (!m_quiet) { + std::cerr << "[NurbsSolve::solve] Solve failed" << std::endl; + std::cerr << "[NurbsSolve::solve] solver.info: " << solver.info() << std::endl; + } + return false; + } + + if (!m_quiet) { + auto end_time = std::chrono::high_resolution_clock::now(); // Record the end time + auto duration = + std::chrono::duration_cast(end_time - start_time) + .count(); + auto elapsed_time = static_cast(duration) / 1000000000.0; + std::cout << "[NurbsSolve] Solving completed. Time elapsed: " << elapsed_time + << " seconds" << std::endl; + } + + return true; +} +/* +Eigen::MatrixXd +NurbsSolve::diff () +{ + int n_rows, n_cols; + m_Ksparse.size (n_rows, n_cols); + + // Convert to Eigen sparse for multiplication + std::vector rowinds, colinds; + std::vector values; + m_Ksparse.get (rowinds, colinds, values); + + std::vector> tripletList; + for (size_t k = 0; k < values.size(); ++k) + { + tripletList.emplace_back(rowinds[k], colinds[k], values[k]); + } + + Eigen::SparseMatrix Keig_sparse(n_rows, n_cols); + Keig_sparse.setFromTriplets(tripletList.begin(), tripletList.end()); + + Eigen::MatrixXd Kx = Keig_sparse * m_xeig; + return Kx - m_feig; +}*/ +Eigen::MatrixXd +NurbsSolve::diff() +{ + + int n_rows, n_cols, n_dims; + m_Ksparse.size(n_rows, n_cols); + n_dims = m_feig.cols(); + + if (n_rows != m_feig.rows()) { + printf("[NurbsSolve::diff] K.rows: %d f.rows: %d\n", n_rows, (int)m_feig.rows()); + throw std::runtime_error("[NurbsSolve::diff] Rows of equation do not match\n"); + } + + Eigen::MatrixXd f = Eigen::MatrixXd::Zero(n_rows, n_dims); + + for (int r = 0; r < n_rows; r++) { + for (int c = 0; c < n_cols; c++) { + f.row(r) = f.row(r) + m_xeig.row(c) * m_Ksparse.get(r, c); + } + } + + return (f - m_feig); +} diff --git a/surface/src/on_nurbs/on_nurbs.cmake b/surface/src/on_nurbs/on_nurbs.cmake index 985e6ef72a1..9d6dea53214 100644 --- a/surface/src/on_nurbs/on_nurbs.cmake +++ b/surface/src/on_nurbs/on_nurbs.cmake @@ -48,7 +48,11 @@ set(ON_NURBS_SOURCES ) set(USE_UMFPACK 0 CACHE BOOL "Use UmfPack for solving sparse systems of equations (e.g. in surface/on_nurbs)") -if(USE_UMFPACK) +option(USE_NURBS_EIGEN_SPARSE_SOLVER "Use Eigen sparse solver" ON) + +if(USE_NURBS_EIGEN_SPARSE_SOLVER) + set(ON_NURBS_SOURCES ${ON_NURBS_SOURCES} src/on_nurbs/nurbs_solve_eigen_sparse.cpp) +elseif(USE_UMFPACK) find_package(CHOLMOD REQUIRED) find_package(UMFPACK REQUIRED) set(ON_NURBS_SOURCES ${ON_NURBS_SOURCES} src/on_nurbs/nurbs_solve_umfpack.cpp)