void MotionCore::init() { if (type_ == CORE_INIT) { FrameInfoBlock *frame_info; memory_.getBlockByName(frame_info,"frame_info"); if (frame_info->source == MEMORY_SIM) { std::cout << "MOTION CORE: SIM" << std::endl; type_ = CORE_SIM; } else if (frame_info->source == MEMORY_ROBOT) { std::cout << "MOTION CORE: ROBOT" << std::endl; type_ = CORE_ROBOT; } else { std::cerr << "Unknown memory type when init vision core" << std::endl; exit(1); } } inst_ = this; setMemoryVariables(); initMemory(); initModules(); fps_time_ = frame_info_->seconds_since_start; time_motion_started_ = frame_info_->seconds_since_start; walk_request_->walk_type_ = INVALID_WALK; loadCalibration(); }
int main(int argc, char** argv) { ros::init(argc, argv, "refill_checker"); ros::NodeHandle nh; pub_ = nh.advertise<std_msgs::Bool>("/needs_refill", 1); image_transport::ImageTransport it(nh); image_transport::Subscriber sub = it.subscribe("camera/rgb/image_raw", 1, imageCallback); std_msgs::Bool bee; bee.data = false; loadCalibration(); while(ros::ok()) { double ratio = bad / big_ol_size; if (ratio > EMPTY_THRESHOLD) { bee.data = true; pub_.publish(bee); } else { bee.data = false; pub_.publish(bee); } ros::spinOnce(); } return 0; }
//-------------------------------------------------------------- void testApp::setup() { //the pirmary listener can't yet be set by default, so even if you only have one window, you need to call this line ofxFensterManager::get()->getPrimaryWindow()->addListener(this); ofSetFrameRate(30); ofBackground(0, 0, 0); ofEnableAlphaBlending(); ofSetVerticalSync(true); ofSetWindowTitle("Control Panel"); vout.setup(); ofxFenster* win=ofxFensterManager::get()->createFenster(500,0,1024,768,OF_WINDOW); win->addListener(&vout); win->setWindowTitle("Output"); if(calibFile.open("calibration")){ printf("Calibration file found\n"); calibFile.close(); loadCalibration(); }else{ printf("Calibration file not found: using defaults\n"); skHratio = ofGetWidth()/640.0; skVratio = ofGetHeight()/480.0; skelHoffset = 0; skelVoffset = 0; spdWings = 0; hgtWings = 0; sclWings = 1; saveCalibration(); } bKinect = true; if(bKinect){ context.setupUsingXMLFile(ofToDataPath("openni/config/ofxopenni_config.xml",true)); context.getXnContext().SetGlobalMirror(false); depth.setup(&context); user.setup(&context, &depth); } wing.setup(); wLa.set(ofGetWidth()/2,ofGetHeight()/2); wRa.set(ofGetWidth()/2,ofGetHeight()/2); head.setup(); bDebug = true; bCalib = false; bMouseMode = false; bWingsGrown= false; trkEase = 0.6; scene = 0; growAmtR = 0; growAmtL = 0; oldHandL = 0; oldHandR = 0; cellEmitter.clear(); font.loadFont("verdana.ttf",60); curS = 0; }
int setCal(char *buf, int len) { int retVal = writeFile(CAL_FILE, buf, len); loadCalibration(); return retVal; }
//-------------------------------------------------------------- void calibrationManager::update(){ calibrationRectangle.x = 0; calibrationRectangle.y = 0; calibrationRectangle.width = ofGetWidth(); calibrationRectangle.height = ofGetHeight(); float widthPad = ofGetWidth() * 0.025f; float heightPad = ofGetHeight() * 0.025f; calibrationRectangle.x += widthPad; calibrationRectangle.y += heightPad; calibrationRectangle.width -= widthPad*2; calibrationRectangle.height -= heightPad*2; panel.update(); nDivisionsWidth = panel.getValueI("N_DIV_W"); nDivisionsHeight = panel.getValueI("N_DIV_H");; preTimePerDot = panel.getValueF("PRE_RECORD_TIME");; recordTimePerDot = panel.getValueF("RECORD_TIME");; smoothing = panel.getValueF("AMOUNT_SMOOTHING"); totalTimePerDot = preTimePerDot + recordTimePerDot; if (panel.getValueB("START_AUTO")){ bPreAutomatic = true; panel.setValueB("START_AUTO", false); } if (panel.getValueB("RESET_CALIB")){ clear(); panel.setValueB("RESET_CALIB", false); } if (panel.getValueB("SAVE_CALIB")){ if (bBeenFit){ saveCalibration(); } panel.setValueB("SAVE_CALIB", false); } if (panel.getValueB("LOAD_CALIB")){ loadCalibration(); panel.setValueB("LOAD_CALIB", false); } if ((bAutomatic == true && bAmInAutodrive == true) || bPreAutomatic){ menuEnergy = 0.94f * menuEnergy + 0.06f * 0.0f; } else { menuEnergy = 0.94f * menuEnergy + 0.06f * 1.0f; } // do the auto: if (bAutomatic == true && bAmInAutodrive == true){ int nPts = nDivisionsWidth * nDivisionsHeight; float totalTime = totalTimePerDot * nPts; if (ofGetElapsedTimef() - startTime > totalTime){ bAmInAutodrive = false; bInAutoRecording = false; calculate(); } else { float diffTime = ofGetElapsedTimef() - startTime ; int pt = (int)(diffTime / totalTimePerDot); float diffDotTime = diffTime - pt * totalTimePerDot; //cout << diffTime << " " << pt << " " << diffDotTime << endl; if (diffDotTime < preTimePerDot){ autoPct = (diffDotTime / preTimePerDot); bInAutoRecording = false; } else{ autoPct = (diffDotTime - preTimePerDot) / recordTimePerDot; bInAutoRecording = true; } pos = pt; } } inputEnergy *= 0.98f; int xx = (pos % nDivisionsWidth); int yy = (pos / nDivisionsWidth); bool bEven = false; if (yy % 2 == 0) bEven = true; //xp = ((float)ofGetWidth() / (float)(nDivisionsWidth-1)) * xx; //yp = ofGetHeight() - ((float)ofGetHeight() / (float)(nDivisionsHeight-1)) * yy; xp = bEven ? calibrationRectangle.x + ((float)calibrationRectangle.width / (float)(nDivisionsWidth-1)) * xx : calibrationRectangle.x + (calibrationRectangle.width - ((float)calibrationRectangle.width / (float)(nDivisionsWidth-1)) * xx); yp = calibrationRectangle.y + calibrationRectangle.height - ((float)calibrationRectangle.height / (float)(nDivisionsHeight-1)) * yy; }
bool MainController::setup() { pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS); Volume::get(ConfigArgs::get().volumeSize); Stopwatch::get().setCustomSignature(43543534); cudaSafeCall(cudaSetDevice(ConfigArgs::get().gpu)); loadCalibration(); std::cout << "Point resolution: " << ((int)((Volume::get().getVoxelSizeMeters().x * 1000.0f) * 10.0f)) / 10.0f << " millimetres" << std::endl; if(ConfigArgs::get().logFile.size()) { rawRead = new RawLogReader; logRead = static_cast<LogReader *>(rawRead); } else { liveRead = new LiveLogReader; logRead = static_cast<LogReader *>(liveRead); } ThreadDataPack::get(); trackerInterface = new TrackerInterface(logRead, depthIntrinsics); if(ConfigArgs::get().trajectoryFile.size()) { std::cout << "Load trajectory: " << ConfigArgs::get().trajectoryFile << std::endl; trackerInterface->loadTrajectory(ConfigArgs::get().trajectoryFile); } systemComponents.push_back(trackerInterface); ThreadDataPack::get().assignFrontend(trackerInterface->getFrontend()); cloudSliceProcessor = new CloudSliceProcessor(); systemComponents.push_back(cloudSliceProcessor); if(ConfigArgs::get().extractOverlap) { trackerInterface->enableOverlap(); } if(!ConfigArgs::get().incrementalMesh && ConfigArgs::get().enableMeshGenerator) { meshGenerator = new MeshGenerator(); systemComponents.push_back(meshGenerator); } else { ThreadDataPack::get().meshGeneratorFinished.assignValue(true); } if(ConfigArgs::get().vocabFile.size() && ConfigArgs::get().onlineDeformation) { deformation = new Deformation; placeRecognition = new PlaceRecognition(depthIntrinsics); systemComponents.push_back(deformation); systemComponents.push_back(placeRecognition); } else { ThreadDataPack::get().deformationFinished.assignValue(true); ThreadDataPack::get().placeRecognitionFinished.assignValue(true); } pangoVis = new PangoVis(depthIntrinsics); return true; }
// --------------------------------------------------- LOAD & SAVE bool ofxMapaMok::loadMesh(string _daeModel, int _textWidth, int _textHeight){ bool fileLoaded = false; // Cargamos el modelo // ofxAssimpModelLoader model; if (model.loadModel(_daeModel)){ textWidth = _textWidth; textHeight = _textHeight; fileLoaded = true; // store model name, will be used later to save xml settings // modelFile = _daeModel; //objectMesh = model.getMesh( model.getNumMeshes() - 1 ); for (int i=model.getNumMeshes()-1; i>=0; i--){ ofMesh m = model.getMesh(i); objectMesh.append( m ); } // Check to see if vertices match texture coordinates // int n = objectMesh.getNumVertices(); if ( n != objectMesh.getNumTexCoords() ){ ofLogError() << "[ofxMapamok]: not same amount of texCoords for all vertices"; } // Hm: this checks for normalized texcoords. Maybe if non pow2 don't multiply?? bool bNormalized = true; for(int i = 0; i < objectMesh.getNumVertices(); i++){ if ( objectMesh.getNumTexCoords() > i ){ float x = objectMesh.getTexCoords()[i].x; float y = objectMesh.getTexCoords()[i].y; if ( (x > 1) || (y > 1) || (x < -1) || (y < -1)){ ofLog(OF_LOG_ERROR,"TexCoord " + ofToString(i) + " it's out of normalized values"); ofLogError()<<x<<":"<<y<<endl; //bNormalized = false; } objectMesh.getTexCoords()[i] = ofVec2f( bNormalized ? x*textWidth : x, bNormalized ? y*textHeight : y); } else { objectMesh.addTexCoord(ofVec2f(0,0)); } } // Create & assign values to vectors based on model's points // objectPoints.resize(n); imagePoints.resize(n); referencePoints.resize(n, false); for(int i = 0; i < n; i++) { objectPoints[i] = ofxCv::toCv(objectMesh.getVertex(i)); } loadCalibration( modelFile.substr( 0, modelFile.length() - 4) + ".xml"); return true; } return false; }
MainController::MainController(int argc, char * argv[]) : good(true), eFusion(0), gui(0), groundTruthOdometry(0), logReader(0), framesToSkip(0), resetButton(false), resizeStream(0) { std::string empty; iclnuim = Parse::get().arg(argc, argv, "-icl", empty) > -1; std::string calibrationFile; Parse::get().arg(argc, argv, "-cal", calibrationFile); Resolution::getInstance(640, 480); if(calibrationFile.length()) { loadCalibration(calibrationFile); } else { Intrinsics::getInstance(528, 528, 320, 240); } Parse::get().arg(argc, argv, "-l", logFile); if(logFile.length()) { logReader = new RawLogReader(logFile, Parse::get().arg(argc, argv, "-f", empty) > -1); } else { logReader = new LiveLogReader(logFile, Parse::get().arg(argc, argv, "-f", empty) > -1); good = ((LiveLogReader *)logReader)->asus->ok(); } if(Parse::get().arg(argc, argv, "-p", poseFile) > 0) { groundTruthOdometry = new GroundTruthOdometry(poseFile); } confidence = 10.0f; depth = 3.0f; icp = 10.0f; icpErrThresh = 5e-05; covThresh = 1e-05; photoThresh = 115; fernThresh = 0.3095f; timeDelta = 200; icpCountThresh = 35000; start = 1; so3 = !(Parse::get().arg(argc, argv, "-nso", empty) > -1); end = std::numeric_limits<unsigned short>::max(); //Funny bound, since we predict times in this format really! Parse::get().arg(argc, argv, "-c", confidence); Parse::get().arg(argc, argv, "-d", depth); Parse::get().arg(argc, argv, "-i", icp); Parse::get().arg(argc, argv, "-ie", icpErrThresh); Parse::get().arg(argc, argv, "-cv", covThresh); Parse::get().arg(argc, argv, "-pt", photoThresh); Parse::get().arg(argc, argv, "-ft", fernThresh); Parse::get().arg(argc, argv, "-t", timeDelta); Parse::get().arg(argc, argv, "-ic", icpCountThresh); Parse::get().arg(argc, argv, "-s", start); Parse::get().arg(argc, argv, "-e", end); logReader->flipColors = Parse::get().arg(argc, argv, "-f", empty) > -1; openLoop = !groundTruthOdometry && Parse::get().arg(argc, argv, "-o", empty) > -1; reloc = Parse::get().arg(argc, argv, "-rl", empty) > -1; frameskip = Parse::get().arg(argc, argv, "-fs", empty) > -1; quiet = Parse::get().arg(argc, argv, "-q", empty) > -1; fastOdom = Parse::get().arg(argc, argv, "-fo", empty) > -1; rewind = Parse::get().arg(argc, argv, "-r", empty) > -1; frameToFrameRGB = Parse::get().arg(argc, argv, "-ftf", empty) > -1; gui = new GUI(logFile.length() == 0, Parse::get().arg(argc, argv, "-sc", empty) > -1); gui->flipColors->Ref().Set(logReader->flipColors); gui->rgbOnly->Ref().Set(false); gui->pyramid->Ref().Set(true); gui->fastOdom->Ref().Set(fastOdom); gui->confidenceThreshold->Ref().Set(confidence); gui->depthCutoff->Ref().Set(depth); gui->icpWeight->Ref().Set(icp); gui->so3->Ref().Set(so3); gui->frameToFrameRGB->Ref().Set(frameToFrameRGB); resizeStream = new Resize(Resolution::getInstance().width(), Resolution::getInstance().height(), Resolution::getInstance().width() / 2, Resolution::getInstance().height() / 2); }
Drive::Drive(PWM *pwm, Accelerometer *accel, Gyroscope *gyro, int frontleft, int frontright, int rearright, int rearleft, int update_rate, int smoothing) { mAccelerometer = accel; mGyroscope = gyro; mUpdateRate = update_rate; mTimerID = 0; mTimerEnabled = false; mSmoothing = smoothing; mAccelValue = new Vector3<float>[mSmoothing]; for (int i = 0; i < mSmoothing; ++i) { mAccelValue[i].x = 0.0f; mAccelValue[i].y = 0.0f; mAccelValue[i].z = 0.0f; } mGyroValue = new Vector3<float>[mSmoothing]; for (int i = 0; i < mSmoothing; ++i) { mGyroValue[i].x = 0.0f; mGyroValue[i].y = 0.0f; mGyroValue[i].z = 0.0f; } mRotate = 0.0f; mTranslate.x = 0.0f; mTranslate.y = 0.0f; mTranslate.z = 0.0f; mRoll = 0.0f; mPitch = 0.0f; mYaw = 0.0f; mTargetRoll = 0.0f; mTargetPitch = 0.0f; mTargetYaw = 0.0f; mPIDRollAngle = new PIDController(mTargetRoll, 0.00f, 0.00f, 0.00f, 5); mPIDPitchAngle = new PIDController(mTargetPitch, 0.00f, 0.00f, 0.00f, 5); mPIDYawAngle = new PIDController(mTargetYaw, 0.00f, 0.00f, 0.00f, 5); mPIDRollRate = new PIDController(0.0f, 0.00f, 0.00f, 0.00f, 5); mPIDPitchRate = new PIDController(0.0f, 0.00f, 0.00f, 0.00f, 5); mPIDYawRate = new PIDController(0.0f, 0.00f, 0.00f, 0.00f, 5); mMotors[0] = new Motor(pwm, frontleft, 1.26f, 1.6f); mMotors[1] = new Motor(pwm, frontright, 1.26f, 1.6f); mMotors[2] = new Motor(pwm, rearright, 1.26f, 1.6f); mMotors[3] = new Motor(pwm, rearleft, 1.26f, 1.6f); mAccelOffset.x = 0.0f; mAccelOffset.y = 0.0f; mAccelOffset.z = 0.0f; mGyroOffset.x = 0.0f; mGyroOffset.y = 0.0f; mGyroOffset.z = 0.0f; try { loadCalibration("calibration.ini"); } catch (CalibrationException &e) { std::cout << "WARNING: Continuimg without calibration" << std::endl; } // Wait for motors to prime usleep(3000000); gettimeofday(&mLastUpdate, NULL); // Pre-populate mAccelValue and mGyroValue arrays Vector3<float> aval; Vector3<float> gval; for (int i = 0; i < mSmoothing; ++i) { aval = mAccelerometer->read(); gval = mGyroscope->read(); mAccelValue[i] = aval; mGyroValue[i] = gval; usleep(10000); // 10,000us = 100Hz } mAccelValueCurrent = 0; mGyroValueCurrent = 0; Vector3<float> accelAvg = averageAccelerometer(); Vector3<float> gyroAvg = averageGyroscope(); calculateOrientation(0.0f, accelAvg, gyroAvg); // Make sure the motors are resting to start stop(); }