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

small update to GNC IndexVector

release/4.3a0
Varun Agrawal 2023-07-10 13:43:44 -04:00
parent 00f5596e70
commit 7c935d9e43
2 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/nonlinear/GncParams.h
View File

@ -73,13 +73,12 @@ class GncParams {
double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS)
Verbosity verbosity = SILENT; ///< Verbosity level Verbosity verbosity = SILENT; ///< Verbosity level
//TODO(Varun) replace IndexVector with vector<size_t> once pybind11/stl.h is globally enabled. /// Use IndexVector for inliers and outliers since it is fast
/// Use IndexVector for inliers and outliers since it is fast + wrapping
using IndexVector = FastVector<uint64_t>; using IndexVector = FastVector<uint64_t>;
///< Slots in the factor graph corresponding to measurements that we know are inliers ///< Slots in the factor graph corresponding to measurements that we know are inliers
IndexVector knownInliers = IndexVector(); IndexVector knownInliers;
///< Slots in the factor graph corresponding to measurements that we know are outliers ///< Slots in the factor graph corresponding to measurements that we know are outliers
IndexVector knownOutliers = IndexVector(); IndexVector knownOutliers;
/// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
void setLossType(const GncLossType type) { void setLossType(const GncLossType type) {

9
tests/testGncOptimizer.cpp
View File

@ -750,7 +750,8 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) {
// add a few outliers // add a few outliers
SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas(
Vector3(0.1, 0.1, 0.01)); Vector3(0.1, 0.1, 0.01));
graph->push_back(BetweenFactor<Pose2>(90, 50, Pose2(), betweenNoise)); // some arbitrary and incorrect between factor // some arbitrary and incorrect between factor
graph->push_back(BetweenFactor<Pose2>(90, 50, Pose2(), betweenNoise));
/// get expected values by optimizing outlier-free graph /// get expected values by optimizing outlier-free graph
Values expectedWithOutliers = LevenbergMarquardtOptimizer(*graph, *initial) Values expectedWithOutliers = LevenbergMarquardtOptimizer(*graph, *initial)
@ -759,9 +760,9 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) {
// CHECK(assert_equal(expected, expectedWithOutliers, 1e-3)); // CHECK(assert_equal(expected, expectedWithOutliers, 1e-3));
// GNC // GNC
// Note: in difficult instances, we set the odometry measurements to be // NOTE: in difficult instances, we set the odometry measurements to be
// inliers, but this problem is simple enought to succeed even without that // inliers, but this problem is simple enough to succeed even without that
// assumption GncParams<GaussNewtonParams>::IndexVector knownInliers; // assumption.
GncParams<GaussNewtonParams> gncParams; GncParams<GaussNewtonParams> gncParams;
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(*graph, *initial, auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(*graph, *initial,
gncParams); gncParams);