Use new GncParams::IndexVector
parent
42bab8f3e7
commit
1d51c4e646
|
@ -73,11 +73,13 @@ class GTSAM_EXPORT GncParams {
|
|||
double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS)
|
||||
Verbosity verbosity = SILENT; ///< Verbosity level
|
||||
|
||||
// *NOTE* We use KeyVector for inliers and outliers since it is fast + wrapping
|
||||
//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
|
||||
KeyVector knownInliers = KeyVector();
|
||||
IndexVector knownInliers = IndexVector();
|
||||
///< Slots in the factor graph corresponding to measurements that we know are outliers
|
||||
KeyVector knownOutliers = KeyVector();
|
||||
IndexVector knownOutliers = IndexVector();
|
||||
|
||||
/// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType).
|
||||
void setLossType(const GncLossType type) {
|
||||
|
@ -118,7 +120,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 KeyVector& knownIn) {
|
||||
void setKnownInliers(const IndexVector& knownIn) {
|
||||
for (size_t i = 0; i < knownIn.size(); i++){
|
||||
knownInliers.push_back(knownIn[i]);
|
||||
}
|
||||
|
@ -129,7 +131,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 KeyVector& knownOut) {
|
||||
void setKnownOutliers(const IndexVector& knownOut) {
|
||||
for (size_t i = 0; i < knownOut.size(); i++){
|
||||
knownOutliers.push_back(knownOut[i]);
|
||||
}
|
||||
|
|
|
@ -567,7 +567,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) {
|
|||
Values initial;
|
||||
initial.insert(X(1), p0);
|
||||
|
||||
std::vector<size_t> knownInliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownInliers;
|
||||
knownInliers.push_back(0);
|
||||
knownInliers.push_back(1);
|
||||
knownInliers.push_back(2);
|
||||
|
@ -644,7 +644,7 @@ TEST(GncOptimizer, barcsq) {
|
|||
Values initial;
|
||||
initial.insert(X(1), p0);
|
||||
|
||||
std::vector<size_t> knownInliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownInliers;
|
||||
knownInliers.push_back(0);
|
||||
knownInliers.push_back(1);
|
||||
knownInliers.push_back(2);
|
||||
|
@ -691,7 +691,7 @@ TEST(GncOptimizer, setInlierCostThresholds) {
|
|||
Values initial;
|
||||
initial.insert(X(1), p0);
|
||||
|
||||
std::vector<size_t> knownInliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownInliers;
|
||||
knownInliers.push_back(0);
|
||||
knownInliers.push_back(1);
|
||||
knownInliers.push_back(2);
|
||||
|
@ -763,7 +763,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) {
|
|||
// GNC
|
||||
// Note: in difficult instances, we set the odometry measurements to be
|
||||
// inliers, but this problem is simple enought to succeed even without that
|
||||
// assumption std::vector<size_t> knownInliers;
|
||||
// assumption GncParams<GaussNewtonParams>::IndexVector knownInliers;
|
||||
GncParams<GaussNewtonParams> gncParams;
|
||||
auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(*graph, *initial,
|
||||
gncParams);
|
||||
|
@ -784,12 +784,12 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
|
|||
// nonconvexity with known inliers and known outliers (check early stopping
|
||||
// when all measurements are known to be inliers or outliers)
|
||||
{
|
||||
std::vector<size_t> knownInliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownInliers;
|
||||
knownInliers.push_back(0);
|
||||
knownInliers.push_back(1);
|
||||
knownInliers.push_back(2);
|
||||
|
||||
std::vector<size_t> knownOutliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownOutliers;
|
||||
knownOutliers.push_back(3);
|
||||
|
||||
GncParams<GaussNewtonParams> gncParams;
|
||||
|
@ -813,11 +813,11 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
|
|||
|
||||
// nonconvexity with known inliers and known outliers
|
||||
{
|
||||
std::vector<size_t> knownInliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownInliers;
|
||||
knownInliers.push_back(2);
|
||||
knownInliers.push_back(0);
|
||||
|
||||
std::vector<size_t> knownOutliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownOutliers;
|
||||
knownOutliers.push_back(3);
|
||||
|
||||
GncParams<GaussNewtonParams> gncParams;
|
||||
|
@ -841,7 +841,7 @@ TEST(GncOptimizer, knownInliersAndOutliers) {
|
|||
|
||||
// only known outliers
|
||||
{
|
||||
std::vector<size_t> knownOutliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownOutliers;
|
||||
knownOutliers.push_back(3);
|
||||
|
||||
GncParams<GaussNewtonParams> gncParams;
|
||||
|
@ -916,11 +916,11 @@ TEST(GncOptimizer, setWeights) {
|
|||
// initialize weights and also set known inliers/outliers
|
||||
{
|
||||
GncParams<GaussNewtonParams> gncParams;
|
||||
std::vector<size_t> knownInliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownInliers;
|
||||
knownInliers.push_back(2);
|
||||
knownInliers.push_back(0);
|
||||
|
||||
std::vector<size_t> knownOutliers;
|
||||
GncParams<GaussNewtonParams>::IndexVector knownOutliers;
|
||||
knownOutliers.push_back(3);
|
||||
gncParams.setKnownInliers(knownInliers);
|
||||
gncParams.setKnownOutliers(knownOutliers);
|
||||
|
|
Loading…
Reference in New Issue