FeatureInitializer.cpp
Go to the documentation of this file.
1 /*
2  * OpenVINS: An Open Platform for Visual-Inertial Research
3  * Copyright (C) 2018-2023 Patrick Geneva
4  * Copyright (C) 2018-2023 Guoquan Huang
5  * Copyright (C) 2018-2023 OpenVINS Contributors
6  * Copyright (C) 2018-2019 Kevin Eckenhoff
7  *
8  * This program is free software: you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation, either version 3 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program. If not, see <https://www.gnu.org/licenses/>.
20  */
21 
22 #include "FeatureInitializer.h"
23 
24 #include "Feature.h"
25 #include "utils/print.h"
26 #include "utils/quat_ops.h"
27 
28 using namespace ov_core;
29 
30 bool FeatureInitializer::single_triangulation(std::shared_ptr<Feature> feat,
31  std::unordered_map<size_t, std::unordered_map<double, ClonePose>> &clonesCAM) {
32 
33  // Total number of measurements
34  // Also set the first measurement to be the anchor frame
35  int total_meas = 0;
36  size_t anchor_most_meas = 0;
37  size_t most_meas = 0;
38  for (auto const &pair : feat->timestamps) {
39  total_meas += (int)pair.second.size();
40  if (pair.second.size() > most_meas) {
41  anchor_most_meas = pair.first;
42  most_meas = pair.second.size();
43  }
44  }
45  feat->anchor_cam_id = anchor_most_meas;
46  feat->anchor_clone_timestamp = feat->timestamps.at(feat->anchor_cam_id).back();
47 
48  // Our linear system matrices
49  Eigen::Matrix3d A = Eigen::Matrix3d::Zero();
50  Eigen::Vector3d b = Eigen::Vector3d::Zero();
51 
52  // Get the position of the anchor pose
53  ClonePose anchorclone = clonesCAM.at(feat->anchor_cam_id).at(feat->anchor_clone_timestamp);
54  const Eigen::Matrix<double, 3, 3> &R_GtoA = anchorclone.Rot();
55  const Eigen::Matrix<double, 3, 1> &p_AinG = anchorclone.pos();
56 
57  // Loop through each camera for this feature
58  for (auto const &pair : feat->timestamps) {
59 
60  // Add CAM_I features
61  for (size_t m = 0; m < feat->timestamps.at(pair.first).size(); m++) {
62 
63  // Get the position of this clone in the global
64  const Eigen::Matrix<double, 3, 3> &R_GtoCi = clonesCAM.at(pair.first).at(feat->timestamps.at(pair.first).at(m)).Rot();
65  const Eigen::Matrix<double, 3, 1> &p_CiinG = clonesCAM.at(pair.first).at(feat->timestamps.at(pair.first).at(m)).pos();
66 
67  // Convert current position relative to anchor
68  Eigen::Matrix<double, 3, 3> R_AtoCi;
69  R_AtoCi.noalias() = R_GtoCi * R_GtoA.transpose();
70  Eigen::Matrix<double, 3, 1> p_CiinA;
71  p_CiinA.noalias() = R_GtoA * (p_CiinG - p_AinG);
72 
73  // Get the UV coordinate normal
74  Eigen::Matrix<double, 3, 1> b_i;
75  b_i << feat->uvs_norm.at(pair.first).at(m)(0), feat->uvs_norm.at(pair.first).at(m)(1), 1;
76  b_i = R_AtoCi.transpose() * b_i;
77  b_i = b_i / b_i.norm();
78  Eigen::Matrix3d Bperp = skew_x(b_i);
79 
80  // Append to our linear system
81  Eigen::Matrix3d Ai = Bperp.transpose() * Bperp;
82  A += Ai;
83  b += Ai * p_CiinA;
84  }
85  }
86 
87  // Solve the linear system
88  Eigen::MatrixXd p_f = A.colPivHouseholderQr().solve(b);
89 
90  // Check A and p_f
91  Eigen::JacobiSVD<Eigen::Matrix3d> svd(A);
92  Eigen::MatrixXd singularValues;
93  singularValues.resize(svd.singularValues().rows(), 1);
94  singularValues = svd.singularValues();
95  double condA = singularValues(0, 0) / singularValues(singularValues.rows() - 1, 0);
96 
97  // std::stringstream ss;
98  // ss << feat->featid << " - cond " << std::abs(condA) << " - z " << p_f(2, 0) << std::endl;
99  // PRINT_DEBUG(ss.str().c_str());
100 
101  // If we have a bad condition number, or it is too close
102  // Then set the flag for bad (i.e. set z-axis to nan)
103  if (std::abs(condA) > _options.max_cond_number || p_f(2, 0) < _options.min_dist || p_f(2, 0) > _options.max_dist ||
104  std::isnan(p_f.norm())) {
105  return false;
106  }
107 
108  // Store it in our feature object
109  feat->p_FinA = p_f;
110  feat->p_FinG = R_GtoA.transpose() * feat->p_FinA + p_AinG;
111  return true;
112 }
113 
114 bool FeatureInitializer::single_triangulation_1d(std::shared_ptr<Feature> feat,
115  std::unordered_map<size_t, std::unordered_map<double, ClonePose>> &clonesCAM) {
116 
117  // Total number of measurements
118  // Also set the first measurement to be the anchor frame
119  int total_meas = 0;
120  size_t anchor_most_meas = 0;
121  size_t most_meas = 0;
122  for (auto const &pair : feat->timestamps) {
123  total_meas += (int)pair.second.size();
124  if (pair.second.size() > most_meas) {
125  anchor_most_meas = pair.first;
126  most_meas = pair.second.size();
127  }
128  }
129  feat->anchor_cam_id = anchor_most_meas;
130  feat->anchor_clone_timestamp = feat->timestamps.at(feat->anchor_cam_id).back();
131  size_t idx_anchor_bearing = feat->timestamps.at(feat->anchor_cam_id).size() - 1;
132 
133  // Our linear system matrices
134  double A = 0.0;
135  double b = 0.0;
136 
137  // Get the position of the anchor pose
138  ClonePose anchorclone = clonesCAM.at(feat->anchor_cam_id).at(feat->anchor_clone_timestamp);
139  const Eigen::Matrix<double, 3, 3> &R_GtoA = anchorclone.Rot();
140  const Eigen::Matrix<double, 3, 1> &p_AinG = anchorclone.pos();
141 
142  // Get bearing in anchor frame
143  Eigen::Matrix<double, 3, 1> bearing_inA;
144  bearing_inA << feat->uvs_norm.at(feat->anchor_cam_id).at(idx_anchor_bearing)(0),
145  feat->uvs_norm.at(feat->anchor_cam_id).at(idx_anchor_bearing)(1), 1;
146  bearing_inA = bearing_inA / bearing_inA.norm();
147 
148  // Loop through each camera for this feature
149  for (auto const &pair : feat->timestamps) {
150 
151  // Add CAM_I features
152  for (size_t m = 0; m < feat->timestamps.at(pair.first).size(); m++) {
153 
154  // Skip the anchor bearing
155  if ((int)pair.first == feat->anchor_cam_id && m == idx_anchor_bearing)
156  continue;
157 
158  // Get the position of this clone in the global
159  const Eigen::Matrix<double, 3, 3> &R_GtoCi = clonesCAM.at(pair.first).at(feat->timestamps.at(pair.first).at(m)).Rot();
160  const Eigen::Matrix<double, 3, 1> &p_CiinG = clonesCAM.at(pair.first).at(feat->timestamps.at(pair.first).at(m)).pos();
161 
162  // Convert current position relative to anchor
163  Eigen::Matrix<double, 3, 3> R_AtoCi;
164  R_AtoCi.noalias() = R_GtoCi * R_GtoA.transpose();
165  Eigen::Matrix<double, 3, 1> p_CiinA;
166  p_CiinA.noalias() = R_GtoA * (p_CiinG - p_AinG);
167 
168  // Get the UV coordinate normal
169  Eigen::Matrix<double, 3, 1> b_i;
170  b_i << feat->uvs_norm.at(pair.first).at(m)(0), feat->uvs_norm.at(pair.first).at(m)(1), 1;
171  b_i = R_AtoCi.transpose() * b_i;
172  b_i = b_i / b_i.norm();
173  Eigen::Matrix3d Bperp = skew_x(b_i);
174 
175  // Append to our linear system
176  Eigen::Vector3d BperpBanchor = Bperp * bearing_inA;
177  A += BperpBanchor.dot(BperpBanchor);
178  b += BperpBanchor.dot(Bperp * p_CiinA);
179  }
180  }
181 
182  // Solve the linear system
183  double depth = b / A;
184  Eigen::MatrixXd p_f = depth * bearing_inA;
185 
186  // Then set the flag for bad (i.e. set z-axis to nan)
187  if (p_f(2, 0) < _options.min_dist || p_f(2, 0) > _options.max_dist || std::isnan(p_f.norm())) {
188  return false;
189  }
190 
191  // Store it in our feature object
192  feat->p_FinA = p_f;
193  feat->p_FinG = R_GtoA.transpose() * feat->p_FinA + p_AinG;
194  return true;
195 }
196 
197 bool FeatureInitializer::single_gaussnewton(std::shared_ptr<Feature> feat,
198  std::unordered_map<size_t, std::unordered_map<double, ClonePose>> &clonesCAM) {
199 
200  // Get into inverse depth
201  double rho = 1 / feat->p_FinA(2);
202  double alpha = feat->p_FinA(0) / feat->p_FinA(2);
203  double beta = feat->p_FinA(1) / feat->p_FinA(2);
204 
205  // Optimization parameters
206  double lam = _options.init_lamda;
207  double eps = 10000;
208  int runs = 0;
209 
210  // Variables used in the optimization
211  bool recompute = true;
212  Eigen::Matrix<double, 3, 3> Hess = Eigen::Matrix<double, 3, 3>::Zero();
213  Eigen::Matrix<double, 3, 1> grad = Eigen::Matrix<double, 3, 1>::Zero();
214 
215  // Cost at the last iteration
216  double cost_old = compute_error(clonesCAM, feat, alpha, beta, rho);
217 
218  // Get the position of the anchor pose
219  const Eigen::Matrix<double, 3, 3> &R_GtoA = clonesCAM.at(feat->anchor_cam_id).at(feat->anchor_clone_timestamp).Rot();
220  const Eigen::Matrix<double, 3, 1> &p_AinG = clonesCAM.at(feat->anchor_cam_id).at(feat->anchor_clone_timestamp).pos();
221 
222  // Loop till we have either
223  // 1. Reached our max iteration count
224  // 2. System is unstable
225  // 3. System has converged
226  while (runs < _options.max_runs && lam < _options.max_lamda && eps > _options.min_dx) {
227 
228  // Triggers a recomputation of jacobians/information/gradients
229  if (recompute) {
230 
231  Hess.setZero();
232  grad.setZero();
233 
234  double err = 0;
235 
236  // Loop through each camera for this feature
237  for (auto const &pair : feat->timestamps) {
238 
239  // Add CAM_I features
240  for (size_t m = 0; m < feat->timestamps.at(pair.first).size(); m++) {
241 
242  //=====================================================================================
243  //=====================================================================================
244 
245  // Get the position of this clone in the global
246  const Eigen::Matrix<double, 3, 3> &R_GtoCi = clonesCAM.at(pair.first).at(feat->timestamps[pair.first].at(m)).Rot();
247  const Eigen::Matrix<double, 3, 1> &p_CiinG = clonesCAM.at(pair.first).at(feat->timestamps[pair.first].at(m)).pos();
248  // Convert current position relative to anchor
249  Eigen::Matrix<double, 3, 3> R_AtoCi;
250  R_AtoCi.noalias() = R_GtoCi * R_GtoA.transpose();
251  Eigen::Matrix<double, 3, 1> p_CiinA;
252  p_CiinA.noalias() = R_GtoA * (p_CiinG - p_AinG);
253  Eigen::Matrix<double, 3, 1> p_AinCi;
254  p_AinCi.noalias() = -R_AtoCi * p_CiinA;
255 
256  //=====================================================================================
257  //=====================================================================================
258 
259  // Middle variables of the system
260  double hi1 = R_AtoCi(0, 0) * alpha + R_AtoCi(0, 1) * beta + R_AtoCi(0, 2) + rho * p_AinCi(0, 0);
261  double hi2 = R_AtoCi(1, 0) * alpha + R_AtoCi(1, 1) * beta + R_AtoCi(1, 2) + rho * p_AinCi(1, 0);
262  double hi3 = R_AtoCi(2, 0) * alpha + R_AtoCi(2, 1) * beta + R_AtoCi(2, 2) + rho * p_AinCi(2, 0);
263  // Calculate jacobian
264  double d_z1_d_alpha = (R_AtoCi(0, 0) * hi3 - hi1 * R_AtoCi(2, 0)) / (pow(hi3, 2));
265  double d_z1_d_beta = (R_AtoCi(0, 1) * hi3 - hi1 * R_AtoCi(2, 1)) / (pow(hi3, 2));
266  double d_z1_d_rho = (p_AinCi(0, 0) * hi3 - hi1 * p_AinCi(2, 0)) / (pow(hi3, 2));
267  double d_z2_d_alpha = (R_AtoCi(1, 0) * hi3 - hi2 * R_AtoCi(2, 0)) / (pow(hi3, 2));
268  double d_z2_d_beta = (R_AtoCi(1, 1) * hi3 - hi2 * R_AtoCi(2, 1)) / (pow(hi3, 2));
269  double d_z2_d_rho = (p_AinCi(1, 0) * hi3 - hi2 * p_AinCi(2, 0)) / (pow(hi3, 2));
270  Eigen::Matrix<double, 2, 3> H;
271  H << d_z1_d_alpha, d_z1_d_beta, d_z1_d_rho, d_z2_d_alpha, d_z2_d_beta, d_z2_d_rho;
272  // Calculate residual
273  Eigen::Matrix<float, 2, 1> z;
274  z << hi1 / hi3, hi2 / hi3;
275  Eigen::Matrix<float, 2, 1> res = feat->uvs_norm.at(pair.first).at(m) - z;
276 
277  //=====================================================================================
278  //=====================================================================================
279 
280  // Append to our summation variables
281  err += std::pow(res.norm(), 2);
282  grad.noalias() += H.transpose() * res.cast<double>();
283  Hess.noalias() += H.transpose() * H;
284  }
285  }
286  }
287 
288  // Solve Levenberg iteration
289  Eigen::Matrix<double, 3, 3> Hess_l = Hess;
290  for (size_t r = 0; r < (size_t)Hess.rows(); r++) {
291  Hess_l(r, r) *= (1.0 + lam);
292  }
293 
294  Eigen::Matrix<double, 3, 1> dx = Hess_l.colPivHouseholderQr().solve(grad);
295  // Eigen::Matrix<double,3,1> dx = (Hess+lam*Eigen::MatrixXd::Identity(Hess.rows(), Hess.rows())).colPivHouseholderQr().solve(grad);
296 
297  // Check if error has gone down
298  double cost = compute_error(clonesCAM, feat, alpha + dx(0, 0), beta + dx(1, 0), rho + dx(2, 0));
299 
300  // Debug print
301  // std::stringstream ss;
302  // ss << "run = " << runs << " | cost = " << dx.norm() << " | lamda = " << lam << " | depth = " << 1/rho << endl;
303  // PRINT_DEBUG(ss.str().c_str());
304 
305  // Check if converged
306  if (cost <= cost_old && (cost_old - cost) / cost_old < _options.min_dcost) {
307  alpha += dx(0, 0);
308  beta += dx(1, 0);
309  rho += dx(2, 0);
310  eps = 0;
311  break;
312  }
313 
314  // If cost is lowered, accept step
315  // Else inflate lambda (try to make more stable)
316  if (cost <= cost_old) {
317  recompute = true;
318  cost_old = cost;
319  alpha += dx(0, 0);
320  beta += dx(1, 0);
321  rho += dx(2, 0);
322  runs++;
323  lam = lam / _options.lam_mult;
324  eps = dx.norm();
325  } else {
326  recompute = false;
327  lam = lam * _options.lam_mult;
328  continue;
329  }
330  }
331 
332  // Revert to standard, and set to all
333  feat->p_FinA(0) = alpha / rho;
334  feat->p_FinA(1) = beta / rho;
335  feat->p_FinA(2) = 1 / rho;
336 
337  // Get tangent plane to x_hat
338  Eigen::HouseholderQR<Eigen::MatrixXd> qr(feat->p_FinA);
339  Eigen::MatrixXd Q = qr.householderQ();
340 
341  // Max baseline we have between poses
342  double base_line_max = 0.0;
343 
344  // Check maximum baseline
345  // Loop through each camera for this feature
346  for (auto const &pair : feat->timestamps) {
347  // Loop through the other clones to see what the max baseline is
348  for (size_t m = 0; m < feat->timestamps.at(pair.first).size(); m++) {
349  // Get the position of this clone in the global
350  const Eigen::Matrix<double, 3, 1> &p_CiinG = clonesCAM.at(pair.first).at(feat->timestamps.at(pair.first).at(m)).pos();
351  // Convert current position relative to anchor
352  Eigen::Matrix<double, 3, 1> p_CiinA = R_GtoA * (p_CiinG - p_AinG);
353  // Dot product camera pose and nullspace
354  double base_line = ((Q.block(0, 1, 3, 2)).transpose() * p_CiinA).norm();
355  if (base_line > base_line_max)
356  base_line_max = base_line;
357  }
358  }
359  // std::stringstream ss;
360  // ss << feat->featid << " - max base " << (feat->p_FinA.norm() / base_line_max) << " - z " << feat->p_FinA(2) << std::endl;
361  // PRINT_DEBUG(ss.str().c_str());
362 
363  // Check if this feature is bad or not
364  // 1. If the feature is too close
365  // 2. If the feature is invalid
366  // 3. If the baseline ratio is large
367  if (feat->p_FinA(2) < _options.min_dist || feat->p_FinA(2) > _options.max_dist ||
368  (feat->p_FinA.norm() / base_line_max) > _options.max_baseline || std::isnan(feat->p_FinA.norm())) {
369  return false;
370  }
371 
372  // Finally get position in global frame
373  feat->p_FinG = R_GtoA.transpose() * feat->p_FinA + p_AinG;
374  return true;
375 }
376 
377 double FeatureInitializer::compute_error(std::unordered_map<size_t, std::unordered_map<double, ClonePose>> &clonesCAM,
378  std::shared_ptr<Feature> feat, double alpha, double beta, double rho) {
379 
380  // Total error
381  double err = 0;
382 
383  // Get the position of the anchor pose
384  const Eigen::Matrix<double, 3, 3> &R_GtoA = clonesCAM.at(feat->anchor_cam_id).at(feat->anchor_clone_timestamp).Rot();
385  const Eigen::Matrix<double, 3, 1> &p_AinG = clonesCAM.at(feat->anchor_cam_id).at(feat->anchor_clone_timestamp).pos();
386 
387  // Loop through each camera for this feature
388  for (auto const &pair : feat->timestamps) {
389  // Add CAM_I features
390  for (size_t m = 0; m < feat->timestamps.at(pair.first).size(); m++) {
391 
392  //=====================================================================================
393  //=====================================================================================
394 
395  // Get the position of this clone in the global
396  const Eigen::Matrix<double, 3, 3> &R_GtoCi = clonesCAM.at(pair.first).at(feat->timestamps.at(pair.first).at(m)).Rot();
397  const Eigen::Matrix<double, 3, 1> &p_CiinG = clonesCAM.at(pair.first).at(feat->timestamps.at(pair.first).at(m)).pos();
398  // Convert current position relative to anchor
399  Eigen::Matrix<double, 3, 3> R_AtoCi;
400  R_AtoCi.noalias() = R_GtoCi * R_GtoA.transpose();
401  Eigen::Matrix<double, 3, 1> p_CiinA;
402  p_CiinA.noalias() = R_GtoA * (p_CiinG - p_AinG);
403  Eigen::Matrix<double, 3, 1> p_AinCi;
404  p_AinCi.noalias() = -R_AtoCi * p_CiinA;
405 
406  //=====================================================================================
407  //=====================================================================================
408 
409  // Middle variables of the system
410  double hi1 = R_AtoCi(0, 0) * alpha + R_AtoCi(0, 1) * beta + R_AtoCi(0, 2) + rho * p_AinCi(0, 0);
411  double hi2 = R_AtoCi(1, 0) * alpha + R_AtoCi(1, 1) * beta + R_AtoCi(1, 2) + rho * p_AinCi(1, 0);
412  double hi3 = R_AtoCi(2, 0) * alpha + R_AtoCi(2, 1) * beta + R_AtoCi(2, 2) + rho * p_AinCi(2, 0);
413  // Calculate residual
414  Eigen::Matrix<float, 2, 1> z;
415  z << hi1 / hi3, hi2 / hi3;
416  Eigen::Matrix<float, 2, 1> res = feat->uvs_norm.at(pair.first).at(m) - z;
417  // Append to our summation variables
418  err += pow(res.norm(), 2);
419  }
420  }
421 
422  return err;
423 }
ov_core::FeatureInitializerOptions::min_dist
double min_dist
Minimum distance to accept triangulated features.
Definition: FeatureInitializerOptions.h:60
ov_core::skew_x
Eigen::Matrix< double, 3, 3 > skew_x(const Eigen::Matrix< double, 3, 1 > &w)
Skew-symmetric matrix from a given 3x1 vector.
Definition: quat_ops.h:135
ov_core::FeatureInitializer::single_triangulation_1d
bool single_triangulation_1d(std::shared_ptr< Feature > feat, std::unordered_map< size_t, std::unordered_map< double, ClonePose >> &clonesCAM)
Uses a linear triangulation to get initial estimate for the feature, treating the anchor observation ...
Definition: FeatureInitializer.cpp:114
Feature.h
ov_core::FeatureInitializer::ClonePose::Rot
const Eigen::Matrix< double, 3, 3 > & Rot()
Accessor for rotation.
Definition: FeatureInitializer.h:78
ov_core::FeatureInitializerOptions::max_baseline
double max_baseline
Max baseline ratio to accept triangulated features.
Definition: FeatureInitializerOptions.h:66
ov_core::FeatureInitializer::ClonePose
Structure which stores pose estimates for use in triangulation.
Definition: FeatureInitializer.h:51
ov_core::FeatureInitializer::single_triangulation
bool single_triangulation(std::shared_ptr< Feature > feat, std::unordered_map< size_t, std::unordered_map< double, ClonePose >> &clonesCAM)
Uses a linear triangulation to get initial estimate for the feature.
Definition: FeatureInitializer.cpp:30
ov_core::FeatureInitializerOptions::max_cond_number
double max_cond_number
Max condition number of linear triangulation matrix accept triangulated features.
Definition: FeatureInitializerOptions.h:69
print.h
ov_core::FeatureInitializerOptions::lam_mult
double lam_mult
Multiplier to increase/decrease lambda.
Definition: FeatureInitializerOptions.h:57
ov_core::FeatureInitializerOptions::min_dx
double min_dx
Cutoff for dx increment to consider as converged.
Definition: FeatureInitializerOptions.h:51
ov_core::FeatureInitializerOptions::init_lamda
double init_lamda
Init lambda for Levenberg-Marquardt optimization.
Definition: FeatureInitializerOptions.h:45
FeatureInitializer.h
ov_core::FeatureInitializer::ClonePose::pos
const Eigen::Matrix< double, 3, 1 > & pos()
Accessor for position.
Definition: FeatureInitializer.h:81
ov_core::FeatureInitializerOptions::max_runs
int max_runs
Max runs for Levenberg-Marquardt.
Definition: FeatureInitializerOptions.h:42
ov_core::FeatureInitializer::_options
FeatureInitializerOptions _options
Contains options for the initializer process.
Definition: FeatureInitializer.h:132
ov_core::FeatureInitializerOptions::max_dist
double max_dist
Minimum distance to accept triangulated features.
Definition: FeatureInitializerOptions.h:63
ov_core::FeatureInitializerOptions::min_dcost
double min_dcost
Cutoff for cost decrement to consider as converged.
Definition: FeatureInitializerOptions.h:54
ov_core::FeatureInitializer::single_gaussnewton
bool single_gaussnewton(std::shared_ptr< Feature > feat, std::unordered_map< size_t, std::unordered_map< double, ClonePose >> &clonesCAM)
Uses a nonlinear triangulation to refine initial linear estimate of the feature.
Definition: FeatureInitializer.cpp:197
quat_ops.h
ov_core
Core algorithms for OpenVINS.
Definition: CamBase.h:30
ov_core::FeatureInitializer::compute_error
double compute_error(std::unordered_map< size_t, std::unordered_map< double, ClonePose >> &clonesCAM, std::shared_ptr< Feature > feat, double alpha, double beta, double rho)
Helper function for the gauss newton method that computes error of the given estimate.
Definition: FeatureInitializer.cpp:377

ov_core
Author(s): Patrick Geneva , Kevin Eckenhoff , Guoquan Huang
autogenerated on Mon Jan 22 2024 03:08:17