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 weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS)
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).
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
* 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++){
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,
* 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++){
knownOutliers.push_back(knownOut[i]);
}

8
gtsam/nonlinear/nonlinear.i
View File

@ -529,8 +529,8 @@ virtual class GncParams {
double relativeCostTol;
double weightsTol;
Verbosity verbosity;
std::vector<size_t> knownInliers;
std::vector<size_t> knownOutliers;
gtsam::KeyVector knownInliers;
gtsam::KeyVector knownOutliers;
void setLossType(const gtsam::GncLossType type);
void setMaxIterations(const size_t maxIter);
@ -538,8 +538,8 @@ virtual class GncParams {
void setRelativeCostTol(double value);
void setWeightsTol(double value);
void setVerbosityGNC(const gtsam::This::Verbosity value);
void setKnownInliers(const std::vector<size_t>& knownIn);
void setKnownOutliers(const std::vector<size_t>& knownOut);
void setKnownInliers(const gtsam::KeyVector& knownIn);
void setKnownOutliers(const gtsam::KeyVector& knownOut);
void print(const string& str = "GncParams: ") const;
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
* mutations on Python side will not be reflected on C++.
*/
#include <pybind11/stl.h>

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

@ -9,4 +9,4 @@
* interface, but without the `<pybind11/stl.h>` copying mechanism. Combined
* with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
* and saves one copy operation.
*/
*/