void LSDWrapper::ResetInternal()
{
// read camera calibration (yes, its done here)
std::string calibFile;
while(node->arDroneVersion == 0)
{
std::cout << "Waiting for first navdata to determine drone version!" << std::endl;
usleep(250000);
}
if(file.size()==0)
{
if(node->arDroneVersion == 1)
file = node->packagePath + "/camcalib/ardrone1_default.txt";
else if(node->arDroneVersion == 2)
file = node->packagePath + "/camcalib/ardrone2_default.txt";
}
outFileName = node->packagePath+"estimated_poses.txt";
//Specify width and height for the monoOdometry module
if(monoOdometry != nullptr)
{
delete monoOdometry;
printf("Deleted SlamSystem Object!\n");
K_sophus << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0;
monoOdometry = new SlamSystem(frameWidth, frameWidth, K_sophus, doSlam);
monoOdometry->setVisualization(outputWrapper);
}
//Setting the filestream
std::ifstream fleH (file.c_str());
inputStream->setCalibration(fileH);
fleH.close();
inputStream->run();
outputWrapper = new ROSOutput3DWrapper(inputStream->width(), inputStream->height());
LiveSLAMWrapper lsdTracker(inputStream, outputWrapper);
predConvert->setPosRPY(0,0,0,0,0,0);
predIMUOnlyForScale->setPosRPY(0,0,0,0,0,0);
resetLSDRequested = false;
isGoodCount = 0;
lastAnimSentClock = 0;
LSDInitializedClock = 0;
lastLSDMessage = "";
flushMapKeypoints = false;
node->publishCommand("u l LSD has been reset.");
imageSeqNumber = 0;
isInitialized = false;
Util::closeAllWindows();
}
void PTAMWrapper::ResetInternal()
{
mimFrameBW.resize(CVD::ImageRef(frameWidth, frameHeight));
mimFrameBW_workingCopy.resize(CVD::ImageRef(frameWidth, frameHeight));
if(mpMapMaker != 0) delete mpMapMaker;
if(mpMap != 0) delete mpMap;
if(mpTracker != 0) delete mpTracker;
if(mpCamera != 0) delete mpCamera;
// read camera calibration (yes, its done here)
std::string file = node->calibFile;
while(node->arDroneVersion == 0)
{
std::cout << "Waiting for first navdata to determine drone version!" << std::endl;
usleep(250000);
}
if(file.size()==0)
{
if(node->arDroneVersion == 1)
file = node->packagePath + "/camcalib/ardrone1_default.txt";
else if(node->arDroneVersion == 2)
file = node->packagePath + "/camcalib/ardrone2_default.txt";
}
std::ifstream fleH (file.c_str());
TooN::Vector<5> camPar;
fleH >> camPar[0] >> camPar[1] >> camPar[2] >> camPar[3] >> camPar[4];
fleH.close();
std::cout<< "Set Camera Paramerer to: " << camPar[0] << " " << camPar[1] << " " << camPar[2] << " " << camPar[3] << " " << camPar[4] << std::endl;
mpMap = new Map;
mpCamera = new ATANCamera(camPar);
mpMapMaker = new MapMaker(*mpMap, *mpCamera);
mpTracker = new Tracker(CVD::ImageRef(frameWidth, frameHeight), *mpCamera, *mpMap, *mpMapMaker);
setPTAMPars(minKFTimeDist, minKFWiggleDist, minKFDist);
predConvert->setPosRPY(0,0,0,0,0,0);
predIMUOnlyForScale->setPosRPY(0,0,0,0,0,0);
resetPTAMRequested = false;
forceKF = false;
isGoodCount = 0;
lastAnimSentClock = 0;
lockNextFrame = false;
PTAMInitializedClock = 0;
lastPTAMMessage = "";
flushMapKeypoints = false;
node->publishCommand("u l PTAM has been reset.");
}
LSDWrapper::LSDWrapper(DroneKalmanFilter* f, EstimationNode* nde, Output3DWrapper* outputWrapper)
{
filter = f;
node = nde;
predConvert = 0;
predIMUOnlyForScale = 0;
mpCamera = 0;
newImageAvailable = false;
mapPointsTransformed = std::vector<tvec3>();
keyFramesTransformed = std::vector<tse3>();
predConvert = new Predictor();
predIMUOnlyForScale = new Predictor();
imuOnlyPred = new Predictor();
drawUI = UI_PRES;
frameWidth = frameHeight = 0;
logfileScalePairs = 0;
this->outputWrapper = outputWrapper;
isInitialized = false;
/***********************************************************relok this*/
std::ifstream fleH (file.c_str());
Sophus::Matrix3f K_sophus;
TooN::Vector<5> camPar;
fleH >> camPar[0] >> camPar[1] >> camPar[2] >> camPar[3] >> camPar[4];
fleH.close();
std::cout<< "Set Camera Paramerer to: " << camPar[0] << " " << camPar[1] << " " << camPar[2] << " " << camPar[3] << " " << camPar[4] << camPar[5] << " " << camPar[6] << " " << camPar[7] << std::endl;
K_sophus << camPar[0], 0.0, camPar[1], 0.0, camPar[2], camPar[3], 0.0, 0.0, 1.0;
outFile = nullptr;
frameWidth = camPar[4];
frameHeight = camPar[5];
// make Odometry
monoOdometry = new SlamSystem(frameWidth, frameHeight, K_sophus, doSlam);
monoOdometry->setVisualization(outputWrapper);
imageSeqNumber = 0;
}
