Skip to content

Commit 3816085

Browse files
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.
1 parent 0ed6fda commit 3816085

File tree

2 files changed

+216
-1
lines changed

2 files changed

+216
-1
lines changed
Lines changed: 211 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,211 @@
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+
}

surface/src/on_nurbs/on_nurbs.cmake

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,11 @@ set(ON_NURBS_SOURCES
4848
)
4949

5050
set(USE_UMFPACK 0 CACHE BOOL "Use UmfPack for solving sparse systems of equations (e.g. in surface/on_nurbs)")
51-
if(USE_UMFPACK)
51+
option(USE_NURBS_EIGEN_SPARSE_SOLVER "Use Eigen sparse solver" ON)
52+
53+
if(USE_NURBS_EIGEN_SPARSE_SOLVER)
54+
set(ON_NURBS_SOURCES ${ON_NURBS_SOURCES} src/on_nurbs/nurbs_solve_eigen_sparse.cpp)
55+
elseif(USE_UMFPACK)
5256
find_package(CHOLMOD REQUIRED)
5357
find_package(UMFPACK REQUIRED)
5458
set(ON_NURBS_SOURCES ${ON_NURBS_SOURCES} src/on_nurbs/nurbs_solve_umfpack.cpp)

0 commit comments

Comments
 (0)