|
| 1 | +/* |
| 2 | +* SPDX-License-Identifier: BSD-3-Clause |
| 3 | +* |
| 4 | +* Point Cloud Library (PCL) - www.pointclouds.org |
| 5 | +* Copyright (c) 2025-, Open Perception Inc. |
| 6 | +* |
| 7 | +* All rights reserved |
| 8 | +*/ |
| 9 | + |
| 10 | +#include <iostream> |
| 11 | +#include <stdexcept> |
| 12 | + |
| 13 | +#include <pcl/surface/on_nurbs/nurbs_solve.h> |
| 14 | + |
| 15 | +#include <Eigen/Dense> |
| 16 | +#include <Eigen/Sparse> |
| 17 | + |
| 18 | +#include <chrono> |
| 19 | +#include <vector> |
| 20 | + |
| 21 | +using namespace pcl; |
| 22 | +using namespace on_nurbs; |
| 23 | + |
| 24 | +void |
| 25 | +NurbsSolve::assign(unsigned rows, unsigned cols, unsigned dims) |
| 26 | +{ |
| 27 | + m_Ksparse.clear(); |
| 28 | + m_xeig = Eigen::MatrixXd::Zero(cols, dims); |
| 29 | + m_feig = Eigen::MatrixXd::Zero(rows, dims); |
| 30 | +} |
| 31 | + |
| 32 | +void |
| 33 | +NurbsSolve::K(unsigned i, unsigned j, double v) |
| 34 | +{ |
| 35 | + m_Ksparse.set(i, j, v); |
| 36 | +} |
| 37 | + |
| 38 | +void |
| 39 | +NurbsSolve::x(unsigned i, unsigned j, double v) |
| 40 | +{ |
| 41 | + m_xeig(i, j) = v; |
| 42 | +} |
| 43 | + |
| 44 | +void |
| 45 | +NurbsSolve::f(unsigned i, unsigned j, double v) |
| 46 | +{ |
| 47 | + m_feig(i, j) = v; |
| 48 | +} |
| 49 | + |
| 50 | +double |
| 51 | +NurbsSolve::K(unsigned i, unsigned j) |
| 52 | +{ |
| 53 | + return m_Ksparse.get(i, j); |
| 54 | +} |
| 55 | + |
| 56 | +double |
| 57 | +NurbsSolve::x(unsigned i, unsigned j) |
| 58 | +{ |
| 59 | + return m_xeig(i, j); |
| 60 | +} |
| 61 | + |
| 62 | +double |
| 63 | +NurbsSolve::f(unsigned i, unsigned j) |
| 64 | +{ |
| 65 | + return m_feig(i, j); |
| 66 | +} |
| 67 | + |
| 68 | +void |
| 69 | +NurbsSolve::resize(unsigned rows) |
| 70 | +{ |
| 71 | + m_feig.conservativeResize(rows, m_feig.cols()); |
| 72 | + // Note: m_Ksparse is not resized here; assumed handled by assign() |
| 73 | +} |
| 74 | + |
| 75 | +void |
| 76 | +NurbsSolve::printK() |
| 77 | +{ |
| 78 | + m_Ksparse.printLong(); |
| 79 | +} |
| 80 | + |
| 81 | +void |
| 82 | +NurbsSolve::printX() |
| 83 | +{ |
| 84 | + std::cout << m_xeig << std::endl; |
| 85 | +} |
| 86 | + |
| 87 | +void |
| 88 | +NurbsSolve::printF() |
| 89 | +{ |
| 90 | + std::cout << m_feig << std::endl; |
| 91 | +} |
| 92 | + |
| 93 | +bool |
| 94 | +NurbsSolve::solve() |
| 95 | +{ |
| 96 | + auto start_time = std::chrono::high_resolution_clock::now(); |
| 97 | + if (!m_quiet) { |
| 98 | + std::cout << "[NurbsSolve] Start solving..." << std::endl; |
| 99 | + } |
| 100 | + |
| 101 | + int n_rows, n_cols; |
| 102 | + m_Ksparse.size(n_rows, n_cols); |
| 103 | + |
| 104 | + unsigned rows, cols, dims; |
| 105 | + getSize(rows, cols, dims); |
| 106 | + |
| 107 | + if (n_rows <= 0 || n_cols <= 0) { |
| 108 | + if (!m_quiet) |
| 109 | + std::cerr << "[NurbsSolve::solve] Invalid matrix size." << std::endl; |
| 110 | + return false; |
| 111 | + } |
| 112 | + |
| 113 | + // Convert SparseMat to Eigen::SparseMatrix |
| 114 | + std::vector<int> rowinds, colinds; |
| 115 | + std::vector<double> values; |
| 116 | + m_Ksparse.get(rowinds, colinds, values); |
| 117 | + |
| 118 | + // Use triplet list for efficient construction |
| 119 | + std::vector<Eigen::Triplet<double>> tripletList; |
| 120 | + tripletList.reserve(values.size()); |
| 121 | + for (size_t k = 0; k < values.size(); ++k) { |
| 122 | + tripletList.emplace_back(rowinds[k], colinds[k], values[k]); |
| 123 | + } |
| 124 | + |
| 125 | + Eigen::SparseMatrix<double> Keig_sparse(n_rows, n_cols); |
| 126 | + Keig_sparse.setFromTriplets(tripletList.begin(), tripletList.end()); |
| 127 | + Keig_sparse.makeCompressed(); |
| 128 | + |
| 129 | + // Choose solver |
| 130 | +// std::string solver_type = "Eigen::SparseQR"; |
| 131 | +// Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> solver; |
| 132 | + |
| 133 | + |
| 134 | +// // NOTE: SparseLU may get wrong result in windows |
| 135 | +// std::string solver_type = "Eigen::SparseLU COLAMDOrdering"; |
| 136 | +// Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> solver; |
| 137 | +// std::string solver_type = "Eigen::SparseLU AMDOrdering"; |
| 138 | +// Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::AMDOrdering<int>> solver; |
| 139 | + |
| 140 | + std::string solver_type = "Eigen::SimplicialLDLT"; |
| 141 | + Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver; |
| 142 | + |
| 143 | + if (!m_quiet) { |
| 144 | + std::cout << "[NurbsSolve::solve] solver_type: " << solver_type << std::endl; |
| 145 | + } |
| 146 | + |
| 147 | + Eigen::SparseMatrix<double> KtK; |
| 148 | + Eigen::MatrixXd Ktf; |
| 149 | + Eigen::SparseMatrix<double> Kt; |
| 150 | + |
| 151 | + // For least-squares: solve min ||K * x - f||^2 |
| 152 | + // We solve normal equations: (K^T K) x = K^T f |
| 153 | + Kt = Keig_sparse.transpose(); |
| 154 | + KtK = Kt * Keig_sparse; |
| 155 | + Ktf = Kt * m_feig; |
| 156 | + |
| 157 | + // Solve KtK * x = Ktf |
| 158 | + solver.compute(KtK); |
| 159 | + if (solver.info() != Eigen::Success) { |
| 160 | + if (!m_quiet) { |
| 161 | + std::cerr << "[NurbsSolve::solve] compute failed" << std::endl; |
| 162 | + std::cerr << "[NurbsSolve::solve] solver.info: " << solver.info() << std::endl; |
| 163 | + } |
| 164 | + return false; |
| 165 | + } |
| 166 | + |
| 167 | + m_xeig = solver.solve(Ktf); |
| 168 | + if (solver.info() != Eigen::Success) { |
| 169 | + if (!m_quiet) { |
| 170 | + std::cerr << "[NurbsSolve::solve] Solve failed" << std::endl; |
| 171 | + std::cerr << "[NurbsSolve::solve] solver.info: " << solver.info() << std::endl; |
| 172 | + } |
| 173 | + return false; |
| 174 | + } |
| 175 | + |
| 176 | + if (!m_quiet) { |
| 177 | + auto end_time = std::chrono::high_resolution_clock::now(); // Record the end time |
| 178 | + auto duration = |
| 179 | + std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - start_time) |
| 180 | + .count(); |
| 181 | + auto elapsed_time = static_cast<double>(duration) / 1000000000.0; |
| 182 | + std::cout << "[NurbsSolve] Solving completed. Time elapsed: " << elapsed_time |
| 183 | + << " seconds" << std::endl; |
| 184 | + } |
| 185 | + |
| 186 | + return true; |
| 187 | +} |
| 188 | + |
| 189 | +Eigen::MatrixXd |
| 190 | +NurbsSolve::diff() |
| 191 | +{ |
| 192 | + |
| 193 | + int n_rows, n_cols, n_dims; |
| 194 | + m_Ksparse.size(n_rows, n_cols); |
| 195 | + n_dims = m_feig.cols(); |
| 196 | + |
| 197 | + if (n_rows != m_feig.rows()) { |
| 198 | + printf("[NurbsSolve::diff] K.rows: %d f.rows: %d\n", n_rows, static_cast<int>(m_feig.rows())); |
| 199 | + throw std::runtime_error("[NurbsSolve::diff] Rows of equation do not match\n"); |
| 200 | + } |
| 201 | + |
| 202 | + Eigen::MatrixXd f = Eigen::MatrixXd::Zero(n_rows, n_dims); |
| 203 | + |
| 204 | + for (int r = 0; r < n_rows; r++) { |
| 205 | + for (int c = 0; c < n_cols; c++) { |
| 206 | + f.row(r) = f.row(r) + m_xeig.row(c) * m_Ksparse.get(r, c); |
| 207 | + } |
| 208 | + } |
| 209 | + |
| 210 | + return (f - m_feig); |
| 211 | +} |
0 commit comments