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)