14 #include <Eigen/Dense>
20 #if EIGEN_VERSION_AT_LEAST(3, 1, 0) // Requires Eigen>=3.1
21 #include <Eigen/SparseCore>
22 #include <Eigen/SparseQR>
92 #if EIGEN_VERSION_AT_LEAST(3, 1, 0)
100 const size_t m = m1 + m2;
106 std::vector<Eigen::Triplet<double>> A_tri;
107 A_tri.reserve(m1 + 2 * m2);
112 int edge_counter = 0;
117 const double w = std::sqrt(e->getInformation());
119 e->evalJacobian(dr_dx);
120 const int node_id = e->node_id;
121 A_tri.emplace_back(edge_counter, node_id, w * dr_dx);
123 g[edge_counter] -= w * e->evaluateResidual();
131 const double w = std::sqrt(e->getInformation());
132 double dr_dxi, dr_dxj;
133 e->evalJacobian(dr_dxi, dr_dxj);
134 const int node_id_i = e->node_id_i, node_id_j = e->node_id_j;
135 A_tri.emplace_back(edge_counter, node_id_i, w * dr_dxi);
136 A_tri.emplace_back(edge_counter, node_id_j, w * dr_dxj);
138 g[edge_counter] -= w * e->evaluateResidual();
146 Eigen::SparseMatrix<double>
A(m, n);
151 A.setFromTriplets(A_tri.begin(), A_tri.end());
157 Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>
163 solved_x_inc = solver.solve(g);
168 if (solved_variances)
172 solved_variances->
resize(n);
178 MRPT_TODO(
"Use compressed access instead of coeff() below");
180 Eigen::SparseMatrix<double> UT = solver.matrixR();
181 Eigen::SparseMatrix<double> solved_covariance(n, n);
182 solved_covariance.reserve(UT.nonZeros());
185 const int show_progress_steps = std::max(
int(20),
int(n / 20));
186 for (
int l = n - 1; l >= 0; l--)
188 if (!(l % show_progress_steps))
190 "Computing variance %6.02f%%... \r",
191 (100.0 * (n - l - 1)) / n);
194 double subSigmas = 0.0;
195 for (
size_t j = l + 1; j < n; j++)
197 if (UT.coeff(l, j) != 0)
203 for (
size_t i = l + 1; i <= j; i++)
205 if (UT.coeff(l, i) != 0)
208 UT.coeff(l, i) * solved_covariance.coeff(i, j);
212 for (
size_t i = j + 1; i < n; ++i)
214 if (UT.coeff(l, i) != 0)
217 UT.coeff(l, i) * solved_covariance.coeff(j, i);
221 solved_covariance.insert(l, j) = (-
sum / UT.coeff(l, l));
222 subSigmas += UT.coeff(l, j) * solved_covariance.coeff(l, j);
226 solved_covariance.insert(l, l) =
227 (1 / UT.coeff(l, l)) * (1 / UT.coeff(l, l) - subSigmas);
232 for (
unsigned int i = 0; i < n; i++)
234 const int idx = (int)solver.colsPermutation().indices().coeff(i);
235 const double variance = solved_covariance.coeff(i, i);
236 (*solved_variances)[idx] = variance;