diff --git a/Cinder/LCTracker/include/Car.h b/Cinder/LCTracker/include/Car.h index 9bb94e2..ee19a83 100644 --- a/Cinder/LCTracker/include/Car.h +++ b/Cinder/LCTracker/include/Car.h @@ -23,7 +23,7 @@ class Car { Car(const Vec2f &initialPosition, const Vec2f &initialDirection); void setPositionAndDirection(const Vec2f &initialPosition, const Vec2f &initialDirection); void draw(); - void update(const Vec2f &posLaser, const float &relativeSpeed); + void update(const Vec2f &posLaser, const float &relativeSpeed, const Vec2f &windowSize); const Vec2f getTrackerA(){ return _posTrackerA; }; const Vec2f getTrackerB(){ return _posTrackerB; }; const Vec2f getCenter(){ return _center; }; diff --git a/Cinder/LCTracker/src/Car.cpp b/Cinder/LCTracker/src/Car.cpp index 223b28f..fdf8073 100644 --- a/Cinder/LCTracker/src/Car.cpp +++ b/Cinder/LCTracker/src/Car.cpp @@ -42,7 +42,7 @@ void Car::draw() glLineWidth(5); - gl::color(1,1,1); + gl::color(0.2,0.2,0.2); // Vec gl::drawLine(_posTrackerA, _posTrackerB); @@ -58,7 +58,6 @@ void Car::draw() gl::color(0, 0, 1); gl::drawSolidCircle(_posTrackerB, 10.0f); - } Vec2f Car::normalBetweenTrackingPoints() @@ -75,7 +74,9 @@ Vec2f Car::normalBetweenTrackingPoints() return Vec2f(normX, normY); } -void Car::update(const Vec2f &posLaser, const float &relativeSpeed) +void Car::update(const Vec2f &posLaser, + const float &relativeSpeed, + const Vec2f &windowSize) { // Whats the vector between the _center and the green laser? @@ -101,17 +102,24 @@ void Car::update(const Vec2f &posLaser, const float &relativeSpeed) Vec2f newA = RotatePointAroundCenter(_posTrackerA, _center, degrees); Vec2f newB = RotatePointAroundCenter(_posTrackerB, _center, degrees); - _posTrackerA = newA; - _posTrackerB = newB; // Then move forward a little - Vec2f newVec = _posTrackerA - _posTrackerB; //normalBetweenTrackingPoints(); + Vec2f newVec = newA - newB; //normalBetweenTrackingPoints(); newVec.normalize(); float goForwardDist = (_center.distance(posLaser) - (_size*0.5)) * relativeSpeed; Vec2f newCenter = _center + (newVec * goForwardDist); - setPositionAndDirection(newCenter, newVec); + if(Area(_size,_size,windowSize.x-(_size*2),windowSize.y-(_size*2)).contains(newCenter)){ + + /* + _posTrackerA = newA; + _posTrackerB = newB; + */ + + // Don't let the car leave the bounds, otherwise we cant track it + setPositionAndDirection(newCenter, newVec); + } } diff --git a/Cinder/LCTracker/src/LCTrackerApp.cpp b/Cinder/LCTracker/src/LCTrackerApp.cpp index dba6b9a..86d017e 100644 --- a/Cinder/LCTracker/src/LCTrackerApp.cpp +++ b/Cinder/LCTracker/src/LCTrackerApp.cpp @@ -85,13 +85,15 @@ class LCTrackerApp : public AppBasic { void mouseDown( MouseEvent event); void mouseUp( MouseEvent event); void mouseDrag( MouseEvent event); + void moveClosetTrackingPoint( MouseEvent event); void update(); void draw(); + void drawWithProjection(); void updateLabels(); Vec3f getLargestContour(vector< vector > *contours); void outputColors(); - void saveColorSettings(); - void readColorSettings(); + void saveSettings(); + void readSettings(); #if USE_SIM_CAR void resetCar(); #endif @@ -99,6 +101,9 @@ class LCTrackerApp : public AppBasic { private: gl::Texture _texTrack; + gl::Texture _texR; + gl::Texture _texG; + gl::Texture _texB; Surface8u _surfTracking; Vec3f _blobR; @@ -111,8 +116,9 @@ class LCTrackerApp : public AppBasic { SimpleGUI *gui; bool _isShowingGUI; LaserMode _laserMode; - string _colorPath; - bool _isMouseDown; + string _settingsPath; + bool _isDrawingProjection; + bool _isSkewed; Vec2f _posMouse; ColorConstraint _ccRed; @@ -123,6 +129,8 @@ class LCTrackerApp : public AppBasic { Car _car; + Vec2f _trackingRegion[4]; + }; LCTrackerApp::LCTrackerApp() : @@ -131,30 +139,35 @@ _blobG(0,0,0), _blobB(0,0,0), _labelFPS(0), _showTex(0), -_isMouseDown(0) +_isDrawingProjection(0), +_isSkewed(1) { }; void LCTrackerApp::shutdown() { - saveColorSettings(); + saveSettings(); outputColors(); } void LCTrackerApp::setup() { - _colorPath = ci::getDocumentsDirectory().string() + "lasercat_colors.csv"; - setWindowSize(640, 480); + _settingsPath = ci::getDocumentsDirectory().string() + "lasercat_settings.csv"; + + readSettings(); + + // Can this be in the init list? _surfTracking = Surface8u(640, 480, false); _texTrack = gl::Texture(_surfTracking); + _texR = gl::Texture(640,480); + _texG = gl::Texture(640,480); + _texB = gl::Texture(640,480); + _isShowingGUI = false; _laserMode = LaserModeGreen; - // HSV hue mapped to 0-255 (not 0-360) - readColorSettings(); - // GUI gui = new SimpleGUI(this); gui->lightColor = ColorA(1, 1, 0, 1); @@ -222,68 +235,114 @@ void LCTrackerApp::resetCar() { console() << "creating new car\n"; - _car = Car(getWindowSize() * 0.5, Vec2f(0.0,1.0)); - + _car = Car(Vec2f(320,240), Vec2f(0.0,1.0)); } #endif -void LCTrackerApp::saveColorSettings() +void LCTrackerApp::saveSettings() { // Get an ofstream which is what you'll use to write to your file. - std::ofstream oStream( _colorPath.c_str() ); + std::ofstream oStream( _settingsPath.c_str() ); - // write the string. + // write colors oStream << SerializedColorConstraints(_ccRed) << "\n"; // R oStream << SerializedColorConstraints(_ccGreen) << "\n"; // G oStream << SerializedColorConstraints(_ccBlue) << "\n"; // B + + // write tracking region + string ul = boost::lexical_cast((int)_trackingRegion[0].x) + "," + boost::lexical_cast((int)_trackingRegion[0].y) + "\n"; + string ur = boost::lexical_cast((int)_trackingRegion[1].x) + "," + boost::lexical_cast((int)_trackingRegion[1].y) + "\n"; + string ll = boost::lexical_cast((int)_trackingRegion[2].x) + "," + boost::lexical_cast((int)_trackingRegion[2].y) + "\n"; + string lr = boost::lexical_cast((int)_trackingRegion[3].x) + "," + boost::lexical_cast((int)_trackingRegion[3].y) + "\n"; + oStream << ul; + oStream << ur; + oStream << ll; + oStream << lr; + // Window position and size + oStream << string(boost::lexical_cast((int)getWindowPosX()) + "," + boost::lexical_cast((int)getWindowPosY()) + "," + boost::lexical_cast((int)getWindowWidth()) + "," + boost::lexical_cast((int)getWindowHeight())); + oStream.close(); } -void LCTrackerApp::readColorSettings() +void LCTrackerApp::readSettings() { - fs::path testPath( _colorPath ); + fs::path testPath( _settingsPath ); if( fs::exists( testPath ) ) { - string myString = loadString( loadFile( _colorPath ) ); - vector rgb = split(myString,"\n"); + string myString = loadString( loadFile( _settingsPath ) ); + vector settings = split(myString,"\n"); - for(int i=0;i colorVals = split(rgb[i],","); + for(int i=0;i tokens = split(settings[i],","); - if(colorVals.size() >= 6){ - int hueMin= boost::lexical_cast(colorVals[0]); - int hueMax= boost::lexical_cast(colorVals[1]); - int satMin= boost::lexical_cast(colorVals[2]); - int satMax= boost::lexical_cast(colorVals[3]); - int valMin= boost::lexical_cast(colorVals[4]); - int valMax= boost::lexical_cast(colorVals[5]); - ColorConstraint c = ColorConstraintMake(hueMin, hueMax, satMin, satMax, valMin, valMax); - switch(i){ - case 0: // R - _ccRed = c; - LogColorConstraint(_ccRed); - break; - case 1: // G - _ccGreen = c; - LogColorConstraint(_ccGreen); - break; - case 2: // B - _ccBlue = c; - LogColorConstraint(_ccBlue); - break; + if(i<3){ + // Colors + + if(tokens.size() >= 6){ + int hueMin= boost::lexical_cast(tokens[0]); + int hueMax= boost::lexical_cast(tokens[1]); + int satMin= boost::lexical_cast(tokens[2]); + int satMax= boost::lexical_cast(tokens[3]); + int valMin= boost::lexical_cast(tokens[4]); + int valMax= boost::lexical_cast(tokens[5]); + ColorConstraint c = ColorConstraintMake(hueMin, hueMax, satMin, satMax, valMin, valMax); + switch(i){ + case 0: // R + _ccRed = c; + LogColorConstraint(_ccRed); + break; + case 1: // G + _ccGreen = c; + LogColorConstraint(_ccGreen); + break; + case 2: // B + _ccBlue = c; + LogColorConstraint(_ccBlue); + break; + } + } + }else if(i<7){ + // Tracking region + if(tokens.size() == 2){ + int x = boost::lexical_cast(tokens[0]); + int y = boost::lexical_cast(tokens[1]); + int pointIdx = i-3; + _trackingRegion[pointIdx] = Vec2f(x,y); + console() << "trackingRegion " << pointIdx << " : " << _trackingRegion[pointIdx] << "\n"; + } + }else{ + // Window size and position + if(tokens.size() == 4){ + int x = boost::lexical_cast(tokens[0]); + int y = boost::lexical_cast(tokens[1]); + int w = boost::lexical_cast(tokens[2]); + int h = boost::lexical_cast(tokens[3]); + setWindowPos(x, y); + setWindowSize(w, h); } } } }else{ - // TODO: Use defaults - console() << "No file exists at " << _colorPath << ". Using default colors. \n"; + + // Use defaults + console() << "No file exists at " << _settingsPath << ". Using default settings. \n"; + _ccRed = ColorConstraintMake(0, 24, 108, 255, 201, 255); _ccGreen = ColorConstraintMake(106, 130, 0, 255, 222, 255); _ccBlue = ColorConstraintMake(89, 150, 167, 255, 179, 255); + + _trackingRegion[0] = Vec2f(0,0); + _trackingRegion[1] = Vec2f(640,0); + _trackingRegion[2] = Vec2f(0,480); + _trackingRegion[3] = Vec2f(640,480); + + setWindowSize(640, 480); + } } @@ -332,31 +391,93 @@ void LCTrackerApp::keyDown( KeyEvent event ) _showTex = event.getChar(); + }else if(c == KeyEvent::KEY_p){ + + _isDrawingProjection = !_isDrawingProjection; + + }else if(c==KeyEvent::KEY_s){ + _isSkewed = !_isSkewed; } } void LCTrackerApp::mouseDown( MouseEvent event) { - _isMouseDown = true; - _posMouse = event.getPos(); + moveClosetTrackingPoint(event); +} + +void LCTrackerApp::mouseDrag( MouseEvent event) +{ + moveClosetTrackingPoint(event); } void LCTrackerApp::mouseUp( MouseEvent event) { - _isMouseDown = false; - _posMouse = event.getPos(); + //... } -void LCTrackerApp::mouseDrag( MouseEvent event) +void LCTrackerApp::moveClosetTrackingPoint( MouseEvent event) { - _posMouse = event.getPos(); + Vec2f mousePos = event.getPos(); + + Vec2f ul = _trackingRegion[0]; + Vec2f ur = _trackingRegion[1]; + Vec2f ll = _trackingRegion[2]; + Vec2f lr = _trackingRegion[3]; + + int movePoint = 0; + float closestPointDist = event.getPos().distance(ul); + + if(mousePos.distance(ur) < closestPointDist){ + closestPointDist = mousePos.distance(ur); + movePoint = 1; + } + + if(mousePos.distance(ll) < closestPointDist){ + closestPointDist = mousePos.distance(ll); + movePoint = 2; + } + + if(mousePos.distance(lr) < closestPointDist){ + closestPointDist = mousePos.distance(lr); + movePoint = 3; + } + + // Move the closest point to the mouse position + _trackingRegion[movePoint] = mousePos; + } void LCTrackerApp::update() { if( _capture.checkNewFrame() ) { - _surfTracking = _capture.getSurface(); + + if(_isSkewed){ + + Surface surfCam = _capture.getSurface(); + // Perform perspective transform + cv::Mat src(toOcv(surfCam)), straigtened; + cv::Point2f src_vertices[4] = { + cv::Point(_trackingRegion[0].x, _trackingRegion[0].y), + cv::Point(_trackingRegion[1].x, _trackingRegion[1].y), + cv::Point(_trackingRegion[2].x, _trackingRegion[2].y), + cv::Point(_trackingRegion[3].x, _trackingRegion[3].y), + }; + cv::Point2f dst_vertices[4] = { + cv::Point(0,0), + cv::Point(640,0), + cv::Point(0,480), + cv::Point(640,480) + }; + cv::Mat warpMatrix = cv::getPerspectiveTransform(src_vertices, dst_vertices); + cv::warpPerspective(src, straigtened, warpMatrix, cv::Size(640,480)); + _surfTracking = fromOcv(straigtened); + + }else{ + + _surfTracking = _capture.getSurface(); + + } // Find the contours Vec2i imgSize = _surfTracking.getSize(); @@ -412,7 +533,7 @@ void LCTrackerApp::update() x=0; y++; } - + cv::Mat threshR(toOcv(chR)), threshG(toOcv(chG)), threshB(toOcv(chB)); vector > contoursR, contoursG, contoursB; @@ -450,32 +571,30 @@ void LCTrackerApp::update() }else{ _texTrack = gl::Texture(_surfTracking); } + + _texR = gl::Texture(chR); + _texG = gl::Texture(chG); + _texB = gl::Texture(chB); } Vec2f posLaser; - if(_isMouseDown){ - - posLaser = _posMouse; - - }else{ - switch(_laserMode){ - case LaserModeRed: - posLaser = _blobR.xy(); - break; - case LaserModeGreen: - posLaser = _blobG.xy(); - break; - case LaserModeBlue: - posLaser = _blobB.xy(); - break; - } + switch(_laserMode){ + case LaserModeRed: + posLaser = _blobR.xy(); + break; + case LaserModeGreen: + posLaser = _blobG.xy(); + break; + case LaserModeBlue: + posLaser = _blobB.xy(); + break; } if(posLaser != Vec2f::zero()){ float speed = 1 * (1.0/getAverageFps()); - _car.update(posLaser, speed); + _car.update(posLaser, speed, getWindowSize()); } @@ -510,17 +629,22 @@ Vec3f LCTrackerApp::getLargestContour(vector< vector > *contours) } - return largestContour; - + float widthRatio = getWindowWidth()/640.0f; + float heightRatio = getWindowHeight()/480.0f; + float avgScale = (widthRatio+heightRatio)/2; + + return Vec3f(largestContour.x * widthRatio, + largestContour.y * heightRatio, + largestContour.z * avgScale); + } -void LCTrackerApp::draw() +void LCTrackerApp::drawWithProjection() { - gl::clear( Color( 0.25, 0.25, 0.25 ) ); - - gl::color(1.0, 1.0, 1.0); - gl::draw(_texTrack); + // Black so no light is projected in the negative space + gl::clear( Color( 0,0,0 ) ); + // gui if(_isShowingGUI){ updateLabels(); gui->draw(); @@ -528,59 +652,102 @@ void LCTrackerApp::draw() gl::disableDepthWrite(); } - glLineWidth(2.0f); - - Vec2f posRed, posGreen, posBlue; - // Draw laser blob if(_blobG != Vec3f::zero()){ - gl::color(0.0, 1.0, 0.0); - posGreen = Vec2f(_blobG.x, _blobG.y); + gl::color(0.25, 0.25, 0.25); + Vec2f posGreen = Vec2f(_blobG.x, _blobG.y); gl::drawStrokedCircle(Vec2f(posGreen.x, posGreen.y), _blobG.z); } -#if !USE_SIM_CAR - - // Draw circles around the car blobs - + // Draw circles around the R & B if(_blobR != Vec3f::zero()){ - gl::color(1.0, 0.0, 0.0); - posRed = Vec2f(_blobR.x, _blobR.y); + gl::color(0.25, 0.25, 0.25); + Vec2f posRed = Vec2f(_blobR.x, _blobR.y); gl::drawStrokedCircle(posRed, _blobR.z); - }else{ drawVec = false; } + } if(_blobB != Vec3f::zero()){ - gl::color(0.0, 0.0, 1.0); - posBlue = Vec2f(_blobB.x, _blobB.y); + gl::color(0.25, 0.25, 0.25); + Vec2f posBlue = Vec2f(_blobB.x, _blobB.y); gl::drawStrokedCircle(Vec2f(posBlue.x, posBlue.y), _blobB.z); - }else{ drawVec = false; } - - // Drawing the car direction (a cross) - gl::color(1.0f, 1.0f, 1.0f); - - gl::drawLine(posRed, posBlue); - - Vec2f vecCar(posRed.x - posBlue.x, posRed.y - posBlue.y); - float vecCarLength = vecCar.length(); - vecCar.normalize(); - - float theta = DegreesToRadians(90.0f); - float normX = cos(theta) * vecCar.x - sin(theta) * vecCar.y; - float normY = sin(theta) * vecCar.x + cos(theta) * vecCar.y; - Vec2f normCar(normX, normY); - - normCar *= vecCarLength; - Vec2f halfVecCar = vecCar * (vecCarLength * 0.5); - - Vec2f normStart(posBlue.x + halfVecCar.x, posBlue.y + halfVecCar.y); - normStart -= (normCar * 0.5); - - // Draw the normal - gl::drawLine(normStart, normStart+normCar); - -#endif + } _car.draw(); + +} + +void LCTrackerApp::draw() +{ + if(_isDrawingProjection){ + + drawWithProjection(); + return; + + }else{ + + // Draw the camera image at the window size + gl::color(1.0, 1.0, 1.0); + gl::draw(_texTrack, Rectf(Vec2f(0,0), getWindowSize())); + + // Draw the tracking region + gl::color(0.25, 0.25, 0.25); + float widthRatio = getWindowWidth()/640.0f; + float heightRatio = getWindowHeight()/480.0f; + Vec2i ul(_trackingRegion[0].x * widthRatio, _trackingRegion[0].y * heightRatio); + Vec2i ur(_trackingRegion[1].x * widthRatio, _trackingRegion[1].y * heightRatio); + Vec2i ll(_trackingRegion[2].x * widthRatio, _trackingRegion[2].y * heightRatio); + Vec2i lr(_trackingRegion[3].x * widthRatio, _trackingRegion[3].y * heightRatio); + + gl::drawSolidCircle(ul, 5); + gl::drawSolidCircle(ur, 5); + gl::drawSolidCircle(ll, 5); + gl::drawSolidCircle(lr, 5); + + // Draw the blobs + if(_blobG != Vec3f::zero()){ + gl::color(0.0, 0.35, 0.0); + Vec2f posGreen = Vec2f(_blobG.x, _blobG.y); + gl::drawStrokedCircle(Vec2f(posGreen.x, posGreen.y), _blobG.z); + } + if(_blobR != Vec3f::zero()){ + gl::color(0.35, 0.0, 0.0); + Vec2f posRed = Vec2f(_blobR.x, _blobR.y); + gl::drawStrokedCircle(Vec2f(posRed.x, posRed.y), _blobR.z); + } + if(_blobB != Vec3f::zero()){ + gl::color(0.0, 0.0, 0.35); + Vec2f posBlue = Vec2f(_blobB.x, _blobB.y); + gl::drawStrokedCircle(Vec2f(posBlue.x, posBlue.y), _blobB.z); + } + + // Draw the RGB + Vec2f texSize = Vec2f(640,480)*0.2; + glLineWidth(1.0f); + + gl::color(0.5,0.5,0.5); + Rectf rRect = Rectf(Vec2f(getWindowWidth()-texSize.x, 0), Vec2f(getWindowWidth(), texSize.y)); + gl::draw(_texR, rRect); + gl::drawStrokedRect(rRect); + + Rectf gRect = Rectf(Vec2f(getWindowWidth()-texSize.x, texSize.y), Vec2f(getWindowWidth(), texSize.y*2)); + gl::draw(_texG, gRect); + gl::drawStrokedRect(gRect); + + Rectf bRect = Rectf(Vec2f(getWindowWidth()-texSize.x, texSize.y*2), Vec2f(getWindowWidth(), texSize.y*3)); + gl::draw(_texB, bRect); + gl::drawStrokedRect(bRect); + + if(_isShowingGUI){ + updateLabels(); + gui->draw(); + gl::disableDepthRead(); + gl::disableDepthWrite(); + } + + _car.draw(); + + return; + } }