diff --git a/cfg/StateestimationParams.cfg b/cfg/StateestimationParams.cfg index c30c9e8..a17cf74 100755 --- a/cfg/StateestimationParams.cfg +++ b/cfg/StateestimationParams.cfg @@ -27,6 +27,8 @@ gen.add("PTAMMinKFTimeDiff", double_t, 0, "Min. time bet gen.add("RescaleFixOrigin", bool_t, 0, "on scale reestimation: if TRUE, the map init pos remains fixed, if false, the current drone pos remains fixed.", True) +gen.add("MinTolerance", double_t, 0, "Minimum Tolerance Value", 0.1, 0.0, 10000) +gen.add("MaxTolerance", double_t, 0, "Maximum Tolerance Value", 30, 0.0, 10000) gen.add("c1", double_t, 0, "prediction model parameter", 0.58, 0.0, 50) diff --git a/src/stateestimation/EstimationNode.cpp b/src/stateestimation/EstimationNode.cpp index fcce315..eb9e90d 100644 --- a/src/stateestimation/EstimationNode.cpp +++ b/src/stateestimation/EstimationNode.cpp @@ -319,9 +319,8 @@ void EstimationNode::dynConfCb(tum_ardrone::StateestimationParamsConfig &config, ptamWrapper->mapLocked = config.PTAMMapLock; filter->allSyncLocked = config.PTAMSyncLock; - - ptamWrapper->setPTAMPars(config.PTAMMinKFTimeDiff, config.PTAMMinKFWiggleDist, config.PTAMMinKFDist); - + ptamWrapper->setPTAMPars(config.PTAMMinKFTimeDiff, config.PTAMMinKFWiggleDist, config.PTAMMinKFDist, + config.MinTolerance, config.MaxTolerance); filter->c1 = config.c1; filter->c2 = config.c2; diff --git a/src/stateestimation/PTAM/MapMaker.cc b/src/stateestimation/PTAM/MapMaker.cc index 204ee43..436422a 100644 --- a/src/stateestimation/PTAM/MapMaker.cc +++ b/src/stateestimation/PTAM/MapMaker.cc @@ -16,6 +16,7 @@ #include #include #include +#include "ros/ros.h" #ifdef WIN32 #define WIN32_LEAN_AND_MEAN @@ -337,7 +338,6 @@ bool MapMaker::InitFromStereo(KeyFrame &kF, RefreshSceneDepth(pkSecond); mdWiggleScaleDepthNormalized = mdWiggleScale / pkFirst->dSceneDepthMean; - AddSomeMapPoints(0); AddSomeMapPoints(3); AddSomeMapPoints(1); @@ -386,7 +386,12 @@ bool MapMaker::InitFromStereo(KeyFrame &kF, imuTrans = imuTrans / sqrt((double)(imuTrans*imuTrans)); double angle = 180*acos((double)(ptamTrans * imuTrans))/3.1415; - + if (initialScaleFactor < min_tol || initialScaleFactor > max_tol){ + printf("\nEstimated scale factor is not within the tolerance range (%.1f): try again!\n", + initialScaleFactor * 1.2); + return false; + } + if(angle > 6000) { printf("\nAngle between estimated Translation is too large (%.1f): try again!\n",angle); diff --git a/src/stateestimation/PTAM/MapMaker.h b/src/stateestimation/PTAM/MapMaker.h index bba4655..a2d95b7 100644 --- a/src/stateestimation/PTAM/MapMaker.h +++ b/src/stateestimation/PTAM/MapMaker.h @@ -60,6 +60,10 @@ class MapMaker : protected CVD::Thread bool NeedNewKeyFrame(KeyFrame &kCurrent); // Is it a good camera pose to add another KeyFrame? bool IsDistanceToNearestKeyFrameExcessive(KeyFrame &kCurrent); // Is the camera far away from the nearest KeyFrame (i.e. maybe lost?) + void SetMinTolerance(double min_tol){ + this->min_tol = min_tol; + } + double initialScaleFactor; double currentScaleFactor; // set exgternally for metric scale. double minKFWiggleDist; @@ -67,6 +71,10 @@ class MapMaker : protected CVD::Thread double lastMetricDist; double lastWiggleDist; + //Tolerance Interval + double min_tol; + double max_tol; + protected: Map &mMap; // The map @@ -136,8 +144,6 @@ class MapMaker : protected CVD::Thread bool mbBundleAbortRequested; // We should stop bundle adjustment bool mbBundleRunning; // Bundle adjustment is running bool mbBundleRunningIsRecent; // ... and it's a local bundle adjustment. - - }; #endif diff --git a/src/stateestimation/PTAMWrapper.cpp b/src/stateestimation/PTAMWrapper.cpp index 30fb118..abd22d7 100644 --- a/src/stateestimation/PTAMWrapper.cpp +++ b/src/stateestimation/PTAMWrapper.cpp @@ -117,7 +117,7 @@ void PTAMWrapper::ResetInternal() mpMapMaker = new MapMaker(*mpMap, *mpCamera); mpTracker = new Tracker(CVD::ImageRef(frameWidth, frameHeight), *mpCamera, *mpMap, *mpMapMaker); - setPTAMPars(minKFTimeDist, minKFWiggleDist, minKFDist); + setPTAMPars(minKFTimeDist, minKFWiggleDist, minKFDist, min_tol, max_tol); predConvert->setPosRPY(0,0,0,0,0,0); predIMUOnlyForScale->setPosRPY(0,0,0,0,0,0); @@ -135,7 +135,8 @@ void PTAMWrapper::ResetInternal() node->publishCommand("u l PTAM has been reset."); } -void PTAMWrapper::setPTAMPars(double minKFTimeDist, double minKFWiggleDist, double minKFDist) +void PTAMWrapper::setPTAMPars(double minKFTimeDist, double minKFWiggleDist, double minKFDist, + double min_tol, double max_tol) { if(mpMapMaker != 0) mpMapMaker->minKFDist = minKFDist; @@ -144,9 +145,16 @@ void PTAMWrapper::setPTAMPars(double minKFTimeDist, double minKFWiggleDist, doub if(mpTracker != 0) mpTracker->minKFTimeDist = minKFTimeDist; + if(mpMapMaker != 0) + mpMapMaker->min_tol = min_tol; + if(mpMapMaker != 0) + mpMapMaker->max_tol = max_tol; + this->minKFDist = minKFDist; this->minKFWiggleDist = minKFWiggleDist; this->minKFTimeDist = minKFTimeDist; + this->min_tol = min_tol; + this->max_tol = max_tol; } PTAMWrapper::~PTAMWrapper(void) diff --git a/src/stateestimation/PTAMWrapper.h b/src/stateestimation/PTAMWrapper.h index 198b005..7ddda2d 100644 --- a/src/stateestimation/PTAMWrapper.h +++ b/src/stateestimation/PTAMWrapper.h @@ -98,6 +98,9 @@ class PTAMWrapper : private CVD::Thread, private MouseKeyHandler double minKFWiggleDist; double minKFDist; + double min_tol; + double max_tol; + Predictor* imuOnlyPred; int lastScaleEKFtimestamp; @@ -154,7 +157,8 @@ class PTAMWrapper : private CVD::Thread, private MouseKeyHandler void newImage(sensor_msgs::ImageConstPtr img); void newNavdata(ardrone_autonomy::Navdata* nav); bool newImageAvailable; - void setPTAMPars(double minKFTimeDist, double minKFWiggleDist, double minKFDist); + void setPTAMPars(double minKFTimeDist, double minKFWiggleDist, double minKFDist, + double min_tol, double max_tol); bool handleCommand(std::string s); bool mapLocked; @@ -172,13 +176,10 @@ class PTAMWrapper : private CVD::Thread, private MouseKeyHandler // resets PTAM tracking inline void Reset() {resetPTAMRequested = true;}; - // start and stop system and respective thread. void startSystem(); void stopSystem(); - - enum {PTAM_IDLE = 0, PTAM_INITIALIZING = 1, PTAM_LOST = 2, PTAM_GOOD = 3, PTAM_BEST = 4, PTAM_TOOKKF = 5, PTAM_FALSEPOSITIVE = 6} PTAMStatus; TooN::SE3<> lastPTAMResultRaw; std::string lastPTAMMessage;