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; }