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

use KeyVector for GNC inliers & outliers

release/4.3a0
Varun Agrawal 2022-07-25 22:19:29 -04:00
parent 4edcb41ad3
commit 42bab8f3e7
4 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/nonlinear/GncParams.h
View File

@ -72,8 +72,12 @@ class GTSAM_EXPORT GncParams {
double relativeCostTol = 1e-5; ///< If relative cost change is below this threshold, stop iterating double relativeCostTol = 1e-5; ///< If relative cost change is below this threshold, stop iterating
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
std::vector<size_t> knownInliers = std::vector<size_t>(); ///< Slots in the factor graph corresponding to measurements that we know are inliers
std::vector<size_t> knownOutliers = std::vector<size_t>(); ///< Slots in the factor graph corresponding to measurements that we know are outliers // *NOTE* We use KeyVector for inliers and outliers since it is fast + wrapping
///< Slots in the factor graph corresponding to measurements that we know are inliers
KeyVector knownInliers = KeyVector();
///< Slots in the factor graph corresponding to measurements that we know are outliers
KeyVector knownOutliers = KeyVector();
/// 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) {
@ -114,7 +118,7 @@ class GTSAM_EXPORT GncParams {
* This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and
* only apply GNC to prune outliers from the loop closures. * only apply GNC to prune outliers from the loop closures.
* */ * */
void setKnownInliers(const std::vector<size_t>& knownIn) { void setKnownInliers(const KeyVector& knownIn) {
for (size_t i = 0; i < knownIn.size(); i++){ for (size_t i = 0; i < knownIn.size(); i++){
knownInliers.push_back(knownIn[i]); knownInliers.push_back(knownIn[i]);
} }
@ -125,7 +129,7 @@ class GTSAM_EXPORT GncParams {
* corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg,
* and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. * and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15].
* */ * */
void setKnownOutliers(const std::vector<size_t>& knownOut) { void setKnownOutliers(const KeyVector& knownOut) {
for (size_t i = 0; i < knownOut.size(); i++){ for (size_t i = 0; i < knownOut.size(); i++){
knownOutliers.push_back(knownOut[i]); knownOutliers.push_back(knownOut[i]);
} }

8
gtsam/nonlinear/nonlinear.i
View File

@ -529,8 +529,8 @@ virtual class GncParams {
double relativeCostTol; double relativeCostTol;
double weightsTol; double weightsTol;
Verbosity verbosity; Verbosity verbosity;
std::vector<size_t> knownInliers; gtsam::KeyVector knownInliers;
std::vector<size_t> knownOutliers; gtsam::KeyVector knownOutliers;
void setLossType(const gtsam::GncLossType type); void setLossType(const gtsam::GncLossType type);
void setMaxIterations(const size_t maxIter); void setMaxIterations(const size_t maxIter);
@ -538,8 +538,8 @@ virtual class GncParams {
void setRelativeCostTol(double value); void setRelativeCostTol(double value);
void setWeightsTol(double value); void setWeightsTol(double value);
void setVerbosityGNC(const gtsam::This::Verbosity value); void setVerbosityGNC(const gtsam::This::Verbosity value);
void setKnownInliers(const std::vector<size_t>& knownIn); void setKnownInliers(const gtsam::KeyVector& knownIn);
void setKnownOutliers(const std::vector<size_t>& knownOut); void setKnownOutliers(const gtsam::KeyVector& knownOut);
void print(const string& str = "GncParams: ") const; void print(const string& str = "GncParams: ") const;
enum Verbosity { enum Verbosity {

2
python/gtsam/preamble/nonlinear.h
View File

@ -10,5 +10,3 @@
* Without this they will be automatically converted to a Python object, and all * Without this they will be automatically converted to a Python object, and all
* mutations on Python side will not be reflected on C++. * mutations on Python side will not be reflected on C++.
*/ */
#include <pybind11/stl.h>