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

Revert "switch from IndexVector to FastVector<size_t> now that pybind/stl.h is enabled" 

This reverts commit 64c28504ad.
release/4.3a0
Varun Agrawal 2023-07-10 13:40:14 -04:00
parent 3641dd1b34
commit 00f5596e70
2 changed files with 18 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/nonlinear/GncParams.h
View File

@ -73,10 +73,13 @@ 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 + wrapping
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
FastVector<size_t> knownInliers; IndexVector knownInliers = IndexVector();
///< 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
FastVector<size_t> knownOutliers; IndexVector knownOutliers = IndexVector();
/// 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) {
@ -117,7 +120,7 @@ class 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 FastVector<size_t>& knownIn) { void setKnownInliers(const IndexVector& 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]);
} }
@ -128,7 +131,7 @@ class 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 FastVector<size_t>& knownOut) { void setKnownOutliers(const IndexVector& 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]);
} }

22
tests/testGncOptimizer.cpp
View File

@ -567,7 +567,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
Values initial; Values initial;
initial.insert(X(1), p0); initial.insert(X(1), p0);
FastVector<size_t> knownInliers; GncParams<GaussNewtonParams>::IndexVector knownInliers;
knownInliers.push_back(0); knownInliers.push_back(0);
knownInliers.push_back(1); knownInliers.push_back(1);
knownInliers.push_back(2); knownInliers.push_back(2);
@ -644,7 +644,7 @@ TEST(GncOptimizer, barcsq) {
Values initial; Values initial;
initial.insert(X(1), p0); initial.insert(X(1), p0);
FastVector<size_t> knownInliers; GncParams<GaussNewtonParams>::IndexVector knownInliers;
knownInliers.push_back(0); knownInliers.push_back(0);
knownInliers.push_back(1); knownInliers.push_back(1);
knownInliers.push_back(2); knownInliers.push_back(2);
@ -691,7 +691,7 @@ TEST(GncOptimizer, setInlierCostThresholds) {
Values initial; Values initial;
initial.insert(X(1), p0); initial.insert(X(1), p0);
FastVector<size_t> knownInliers; GncParams<GaussNewtonParams>::IndexVector knownInliers;
knownInliers.push_back(0); knownInliers.push_back(0);
knownInliers.push_back(1); knownInliers.push_back(1);
knownInliers.push_back(2); knownInliers.push_back(2);
@ -761,7 +761,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) {
// 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 enought to succeed even without that
// assumption; // assumption GncParams<GaussNewtonParams>::IndexVector knownInliers;
GncParams<GaussNewtonParams> gncParams; GncParams<GaussNewtonParams> gncParams;
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(*graph, *initial, auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(*graph, *initial,
gncParams); gncParams);
@ -782,12 +782,12 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
// nonconvexity with known inliers and known outliers (check early stopping // nonconvexity with known inliers and known outliers (check early stopping
// when all measurements are known to be inliers or outliers) // when all measurements are known to be inliers or outliers)
{ {
FastVector<size_t> knownInliers; GncParams<GaussNewtonParams>::IndexVector knownInliers;
knownInliers.push_back(0); knownInliers.push_back(0);
knownInliers.push_back(1); knownInliers.push_back(1);
knownInliers.push_back(2); knownInliers.push_back(2);
FastVector<size_t> knownOutliers; GncParams<GaussNewtonParams>::IndexVector knownOutliers;
knownOutliers.push_back(3); knownOutliers.push_back(3);
GncParams<GaussNewtonParams> gncParams; GncParams<GaussNewtonParams> gncParams;
@ -811,11 +811,11 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
// nonconvexity with known inliers and known outliers // nonconvexity with known inliers and known outliers
{ {
FastVector<size_t> knownInliers; GncParams<GaussNewtonParams>::IndexVector knownInliers;
knownInliers.push_back(2); knownInliers.push_back(2);
knownInliers.push_back(0); knownInliers.push_back(0);
FastVector<size_t> knownOutliers; GncParams<GaussNewtonParams>::IndexVector knownOutliers;
knownOutliers.push_back(3); knownOutliers.push_back(3);
GncParams<GaussNewtonParams> gncParams; GncParams<GaussNewtonParams> gncParams;
@ -839,7 +839,7 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
// only known outliers // only known outliers
{ {
FastVector<size_t> knownOutliers; GncParams<GaussNewtonParams>::IndexVector knownOutliers;
knownOutliers.push_back(3); knownOutliers.push_back(3);
GncParams<GaussNewtonParams> gncParams; GncParams<GaussNewtonParams> gncParams;
@ -914,11 +914,11 @@ TEST(GncOptimizer, setWeights) {
// initialize weights and also set known inliers/outliers // initialize weights and also set known inliers/outliers
{ {
GncParams<GaussNewtonParams> gncParams; GncParams<GaussNewtonParams> gncParams;
FastVector<size_t> knownInliers; GncParams<GaussNewtonParams>::IndexVector knownInliers;
knownInliers.push_back(2); knownInliers.push_back(2);
knownInliers.push_back(0); knownInliers.push_back(0);
FastVector<size_t> knownOutliers; GncParams<GaussNewtonParams>::IndexVector knownOutliers;
knownOutliers.push_back(3); knownOutliers.push_back(3);
gncParams.setKnownInliers(knownInliers); gncParams.setKnownInliers(knownInliers);
gncParams.setKnownOutliers(knownOutliers); gncParams.setKnownOutliers(knownOutliers);