12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

addressed comments in PR

release/4.3a0
lcarlone 2022-07-02 16:02:42 -04:00
parent 7b0356cbea
commit f947b973cd
2 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/triangulation.h
View File

@ -107,7 +107,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal, const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, Key landmarkKey, const Point2Vector& measurements, Key landmarkKey,
const Point3& initialEstimate, const Point3& initialEstimate,
const SharedNoiseModel& model = nullptr) { const SharedNoiseModel& model = unit2) {
Values values; Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
@ -117,7 +117,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
typedef PinholePose<CALIBRATION> Camera; typedef PinholePose<CALIBRATION> Camera;
Camera camera_i(pose_i, sharedCal); Camera camera_i(pose_i, sharedCal);
graph.emplace_shared<TriangulationFactor<Camera> > // graph.emplace_shared<TriangulationFactor<Camera> > //
(camera_i, measurements[i], model? model : unit2, landmarkKey); (camera_i, measurements[i], model, landmarkKey);
} }
return std::make_pair(graph, values); return std::make_pair(graph, values);
} }

6
gtsam/nonlinear/GncOptimizer.h
View File

@ -299,9 +299,11 @@ class GTSAM_EXPORT GncOptimizer {
std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init; std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init;
} }
} }
if (mu_init >= 0 && mu_init < 1e-6) if (mu_init >= 0 && mu_init < 1e-6){
mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors, mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors,
// i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0 // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0
}
return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1, return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1,
// which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold
// and there is no need to robustify (TLS = least squares) // and there is no need to robustify (TLS = least squares)