Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add initial scale tolerance #60

Open
wants to merge 2 commits into
base: indigo-devel
Choose a base branch
from
Open

Add initial scale tolerance #60

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6322b9a
Added min and max tolerance to the scale when initializing
Apr 6, 2015
faef9d1
Increase maximum tolerance
Jun 17, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions cfg/StateestimationParams.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
5 changes: 2 additions & 3 deletions src/stateestimation/EstimationNode.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
9 changes: 7 additions & 2 deletions src/stateestimation/PTAM/MapMaker.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#include <gvars3/instances.h>
#include <fstream>
#include <algorithm>
#include "ros/ros.h"

#ifdef WIN32
#define WIN32_LEAN_AND_MEAN
Expand Down Expand Up @@ -337,7 +338,6 @@ bool MapMaker::InitFromStereo(KeyFrame &kF,
RefreshSceneDepth(pkSecond);
mdWiggleScaleDepthNormalized = mdWiggleScale / pkFirst->dSceneDepthMean;


AddSomeMapPoints(0);
AddSomeMapPoints(3);
AddSomeMapPoints(1);
Expand Down Expand Up @@ -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);
Expand Down
10 changes: 8 additions & 2 deletions src/stateestimation/PTAM/MapMaker.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,13 +60,21 @@ 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;
double minKFDist;
double lastMetricDist;
double lastWiggleDist;

//Tolerance Interval
double min_tol;
double max_tol;

protected:

Map &mMap; // The map
Expand Down Expand Up @@ -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
Expand Down
12 changes: 10 additions & 2 deletions src/stateestimation/PTAMWrapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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;
Expand All @@ -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)
Expand Down
9 changes: 5 additions & 4 deletions src/stateestimation/PTAMWrapper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down