void Idle() { flag_found_pinch_data=false; KeyboardChanges(); std::unique_ptr<PXCPointF32[]> uvmap(new PXCPointF32[640*480]); //+ uvmap.get() as argument passing //PXCPointF32 *uvmap=uvmap= // new PXCPointF32[constants::COLOR_WIDTH*constants::COLOR_HEIGHT]; //std::vector<PXCPointF32> uvmap(640*480); //-------CAPTURE NEW IMAGE FRAME--------------------// PXCImage::ImageData data; pxcStatus sts=psm->AcquireFrame(true); if (sts==PXC_STATUS_NO_ERROR) { PXCCapture::Sample *sample = psm->QuerySample(); if (sample) { PXCImage *colorIm=0,*depthIm=0,*blobIm=0; colorIm = sample->color; depthIm = sample->depth; if(colorIm && depthIm) { CopyColorStreamToColorMat(colorIm,data,ColorMat); CopyDepthStreamToDepthMatAndGetUVMap(depthIm,data,uvmap.get(),DepthMat); CopyDepthImageMappedToColor_ToMat(colorIm,depthIm,data,MappedDepthMat); //After this function I have a UVMap available! //************PINCH GESTURE DETECTION ALGORITHM*****************************************// // Get extracted blobs blobData->Update(); // update to the current blob data int iBlobsNum = blobData->QueryNumberOfBlobs(); PXCBlobData::IBlob *pBlob=nullptr; //Get the nearest blob-only 1 if( (blobData->QueryBlobByAccessOrder(0, PXCBlobData::ACCESS_ORDER_NEAR_TO_FAR, pBlob)) ==PXC_STATUS_NO_ERROR) { CopyBlobToBlobMat(pBlob,blobIm,data); //Edo exo to BlobMat based on depth data only int nContours = pBlob->QueryNumberOfContours(); //if there is a hole if(nContours==2) { vector<PXCPointI32> outerContourPoints; vector<PXCPointI32> innerContourPoints; //for each contour int outerContourSize = pBlob->QueryContourSize(0); int innerContourSize = pBlob->QueryContourSize(1); outerContourPoints.resize(outerContourSize); innerContourPoints.resize(innerContourSize); //cout<<innerContourSize<<endl; if( innerContourSize >= constants::MIN_INNER_CONTOUR_POINTS_TO_VALID_PINCH && outerContourSize > 1 ) { //fill vectors with the points of each contour pBlob->QueryContourPoints(0,outerContourSize,&outerContourPoints[0]); pBlob->QueryContourPoints(1,innerContourSize,&innerContourPoints[0]); //FIND THE LEFT-MOST AND RIGHT-MOST INNER CONTOUR POINTS //given the contour points of inner contour FindLeftMostAndRightMostPointsOfInnerContour( innerContourPoints, leftMostPoint, rightMostPoint); //Create 3 neighbor points left to the left-most point of inner contour //-6,-10,-14 distance in pixels from left-most point //Create also 2 neighbor points above and underneath the middle previous neighbor //MAKE A CLASS WHICH WILL HAVE 5 POINTS AS PROPERTIES vector<cv::Point> target_points_neighborhood_vec; //pass target_points_neighborhood_vec to fill it with neighborhood points //10 means 8 for horizontal and 10-8=2 for verticals CreatePinchNeighborhoodPoints(leftMostPoint,rightMostPoint,target_points_neighborhood_vec,8); //target_points_neighborhood_vec is filled with Points if(target_points_neighborhood_vec.size()>0) { float average_z_of_valid_target_points_mm=GetAverageZValueOfNeighborhoodPoints(target_points_neighborhood_vec); //FIND THE AVERAGE X AND Y OF TARGET POINTS IN COLOR IMAGE MAPPED TO DEPTH int average_x_of_valid_target_points_pixels=0; int average_y_of_valid_target_points_pixels=0; //pass average_x and average_y as reference and also vector as reference GetAverageXandYofMappedPoints(average_x_of_valid_target_points_pixels,average_y_of_valid_target_points_pixels, target_points_neighborhood_vec,uvmap.get()); //if(average_x_of_valid_target_points_pixels>=0 && average_y_of_valid_target_points_pixels>=0 && average_x_of_valid_target_points_pixels<640 && average_x_of_valid_target_points_pixels<480) if(average_x_of_valid_target_points_pixels>=0 && average_y_of_valid_target_points_pixels>=0) { PXCPoint3DF32 pinch_data_color_PIXELS; pinch_data_color_PIXELS.x=average_x_of_valid_target_points_pixels; pinch_data_color_PIXELS.y=average_y_of_valid_target_points_pixels; pinch_data_color_PIXELS.z=average_z_of_valid_target_points_mm; if(projection->ProjectColorToCamera( 1,&pinch_data_color_PIXELS,&pinch_data_world_MM) == PXC_STATUS_NO_ERROR) { flag_found_pinch_data=true; cout.precision(2); //pinch_data_world_MM is the 3D Point of the target pinch in camera world coordinates //cout<<fixed<<"World X(cm): "<<pinch_data_world_MM.x/10<<" World Y(cm): "<<pinch_data_world_MM.y/10<<" World.Z(cm)"<<pinch_data_world_MM.z/10<<endl; cv::Point avgpt(average_x_of_valid_target_points_pixels,average_y_of_valid_target_points_pixels); MyFilledCircle(ColorMat,avgpt,cv::Scalar( 0,0,255 ),4); } } } } } else { InitializeLeftandRightMostPoints(leftMostPoint,rightMostPoint); } } } } } cv::undistort(ColorMat,TheInputImage_Undistorted, TheDistCameraParameters.CameraMatrix, TheDistCameraParameters.Distorsion); //Method for detecting Black/White Chessboard probDetect=TheBoardDetector.detect(TheInputImage_Undistorted); frameCnt++; if(probDetect>0.06) { //blobConfig->SetMaxDistance(TheBoardDetector.getDetectedBoard().Tvec.at<float>(0,2)*1000); // … call more configuration methods // Apply the new configuration values //blobConfig->ApplyChanges(); draw_augmented_content=true; //********************CHANGE THE STATE OF SYSTEM BASED ON PINCH DETECTION********************// //Based on whether or not a pinch gesture has been detected in the current frame, //we change the current state of our system if(flag_found_pinch_data) { //pinch+chessboard detected //An egine detect pinch eno itan free, pigaine se pinch-in state if(game.getPinchState() == PinchState::FREE ) game.setPinchState(PinchState::PINCH_IN); //an itan se pinch-in kai egine detect pali pinch, pigaine se pinch continuous else if(game.getPinchState()==PinchState::PINCH_IN) game.setPinchState(PinchState::PINCH_CONTINUOUS); else if(game.getPinchState()==PinchState::PINCH_CONTINUOUS) game.setPinchState(PinchState::PINCH_CONTINUOUS); else if(game.getPinchState()==PinchState::PINCH_OUT ) { if(bValidEngineMove) game.setPinchState(PinchState::ENEMY_MOVE); else game.setPinchState(PinchState::FREE); } else if(game.getPinchState()==PinchState::ENEMY_MOVE) { if(bEndOfEnemyMoveFlag==true) game.setPinchState(PinchState::FREE); else game.setPinchState(PinchState::ENEMY_MOVE); } } else{ if(game.getPinchState()==PinchState::PINCH_IN) game.setPinchState(PinchState::PINCH_OUT); else if(game.getPinchState()==PinchState::PINCH_OUT) { if(bValidEngineMove==true) game.setPinchState(PinchState::ENEMY_MOVE); else game.setPinchState(PinchState::FREE); } else if(game.getPinchState()==PinchState::ENEMY_MOVE) { if(bEndOfEnemyMoveFlag==true ) game.setPinchState(PinchState::FREE); else game.setPinchState(PinchState::ENEMY_MOVE); } else if(game.getPinchState()==PinchState::PINCH_CONTINUOUS) { boolVec[frameCnt%constants::numFrames]=1; cout<<"VECTOR FULL: "<<std::accumulate(boolVec.begin(), boolVec.end(), 0)<<" / "<<boolVec.size()<<endl; if(std::accumulate(boolVec.begin(), boolVec.end(), 0)==boolVec.size()) { game.setPinchState(PinchState::PINCH_OUT); frameCnt=0; for(int i=0;i<boolVec.size();i++) { boolVec[i]=0; } } } else if(game.getPinchState()==PinchState::FREE) game.setPinchState(PinchState::FREE); } //***************************************************************************************// } else draw_augmented_content=false; //*****************************ACTIONS BASED ON THE CURRENT STATE OF THE SYSTEM************************// //After setting the new State of our system, we can now proceed and make use of algorithms for //the detection of the 3D position of pinch and the correct rendering of objects. if(game.getPinchState()==PinchState::FREE) { bRenderStaticBoardFlag=true; bRenderMovingPawnFlag=false; } else if(game.getPinchState()==PinchState::PINCH_IN) //bRenderStaticBoardFlag=false; { pinch_point_wrt_chessboard_M=GetPinchPointwrtChessboard(pinch_data_world_MM); float dist= CalculateDistanceBetweenPinchPointAndCentersOfSquaresWithWhitePiece (pinch_point_wrt_chessboard_M,SelectedStartSquare); //cout<<"Distance: "<<dist<<endl; //cout<<"Closest square: "<<SelectedStartSquare.x<<" , "<<SelectedStartSquare.y<<endl; if(dist>constants::DISTANCE_THRESHOLD_FOR_PIECE_SELECTION) { //Way too far away for selecting a piece cout<<"PINCH TOO FAR AWAY FROM A PIECE"<<endl; SelectedStartSquare.x=-1; SelectedStartSquare.y=-1; bRenderStaticBoardFlag=true; bRenderMovingPawnFlag=false; } else { bRenderStaticBoardFlag=false; bRenderMovingPawnFlag=true; } } else if(game.getPinchState()==PinchState::PINCH_CONTINUOUS) { if(SelectedStartSquare.x!=-1 && SelectedStartSquare.y!=-1) { pinch_point_wrt_chessboard_M=GetPinchPointwrtChessboard(pinch_data_world_MM); //A piece was selected during pinch-in bRenderStaticBoardFlag=false; bRenderMovingPawnFlag=true; } else { cout<<"No piece has been selected!"<<endl; cout<<"Please PINCH-IN AGAIN"<<endl; bRenderStaticBoardFlag=true; bRenderMovingPawnFlag=false; } } else if(game.getPinchState()==PinchState::PINCH_OUT) { if(SelectedStartSquare.x==-1 && SelectedStartSquare.y==-1) { cout<<"No piece has been selected!"<<endl; cout<<"Please PINCH-IN AGAIN"<<endl; bValidEngineMove=false; bRenderStaticBoardFlag=true; bRenderMovingPawnFlag=false; //valid engine move=false } else { float dist2=CalculateDistanceBetweenPinchPointAndPossibleEndSquare (pinch_point_wrt_chessboard_M,SelectedEndSquare); bValidEngineMove= game.CallPlayerMove(SelectedStartSquare,SelectedEndSquare); bRenderStaticBoardFlag=true; bRenderMovingPawnFlag=false; } } else if(game.getPinchState()==PinchState::ENEMY_MOVE) { if(GameResult!=GameWinner::ENEMY && GameResult!=GameWinner::USER) { if(bEnemyMovingPawn==false) { //pare apo to AI tin Move (arxiko-teliko) game.CallEnemyMove(); bEnemyMovingPawn=true; changingStartPoint=Start3DPositionOfEnemyPiece; diffvec.x=End3DPositionOfEnemyPiece.x-Start3DPositionOfEnemyPiece.x; diffvec.y=End3DPositionOfEnemyPiece.y-Start3DPositionOfEnemyPiece.y; diffvec.z=End3DPositionOfEnemyPiece.z-Start3DPositionOfEnemyPiece.z; diffvec.x/=Calculate3DDistanceOf2Points(Start3DPositionOfEnemyPiece,End3DPositionOfEnemyPiece); diffvec.y/=Calculate3DDistanceOf2Points(Start3DPositionOfEnemyPiece,End3DPositionOfEnemyPiece); bRenderStaticBoardFlag=false; bRenderMovingPawnFlag=false; bEndOfEnemyMoveFlag=false; } else { bFlagTimer=true; } } } //*****************************************************************************************************// //cout<<game.getPinchState()<<endl; //Change the inpur BGR frame to RGB for OpenGL cv::cvtColor(TheInputImage_Undistorted,TheInputImage_RGB,CV_BGR2RGB); //TheInputImage_RGB becomes new_frame_image cv::resize(TheInputImage_RGB,new_frame_image,TheGlWindowSize); psm->ReleaseFrame(); glutPostRedisplay(); }
bool GoBoardDetector::calculateCameraIntrinsix() { //cv::showAndSave("marker_board_pre", undistortImage); std::vector<cv::Point3f> m_markerCorners3d; cv::Mat test; undistortImage.copyTo(test); m_markerCorners3d.clear(); std::vector<cv::Point2f> imgPoints; bool hasBoardMarkers = false; for(size_t i=0; i<m_detectedMarkers.size(); i++) { Marker& m = m_detectedMarkers[i]; //m.draw(undistortImage,cv::Scalar(0,0,255),2); for(size_t j=0; j<boardMarkerID.size(); j++) { //if the marker is one of the board marker ids if(m.id == boardMarkerID[j]) { bool CorrectMarker = false; float botL_x; float botL_y; //bottom row if(j>= botLeft && j<= botRight) { CorrectMarker = true; hasBoardMarkers = true; int factor = j- botLeft; botL_x = gap*factor; botL_y = 0; imgPoints.push_back(m.points[3]); imgPoints.push_back(m.points[0]); imgPoints.push_back(m.points[1]); imgPoints.push_back(m.points[2]); m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y,0)); m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y+1,0)); m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y+1,0)); m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y,0)); }//top row else if(j>=0 && j<=topRight) { CorrectMarker = true; hasBoardMarkers = true; int factor = j; botL_x = gap*factor; botL_y = (yMarkerNumber-1)*gap; imgPoints.push_back(m.points[3]); imgPoints.push_back(m.points[0]); imgPoints.push_back(m.points[1]); imgPoints.push_back(m.points[2]); m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y,0)); m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y+1,0)); m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y+1,0)); m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y,0)); }//left row else if((j-topRight)%2 == 1) { CorrectMarker = true; hasBoardMarkers = true; int factor = (botLeft-j)/2; botL_x = 0; botL_y = gap*factor; imgPoints.push_back(m.points[3]); imgPoints.push_back(m.points[0]); imgPoints.push_back(m.points[1]); imgPoints.push_back(m.points[2]); m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y,0)); m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y+1,0)); m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y+1,0)); m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y,0)); }//right row else if((j-topRight)%2 == 0) { CorrectMarker = true; hasBoardMarkers = true; int factor = (botLeft-(j-1))/2; botL_x = (xMarkerNumber-1)*gap; botL_y = gap*factor; imgPoints.push_back(m.points[3]); imgPoints.push_back(m.points[0]); imgPoints.push_back(m.points[1]); imgPoints.push_back(m.points[2]); m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y,0)); m_markerCorners3d.push_back(cv::Point3f(botL_x,botL_y+1,0)); m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y+1,0)); m_markerCorners3d.push_back(cv::Point3f(botL_x+1,botL_y,0)); } if(CorrectMarker){ if(showMarkers){ m.draw(undistortImage,cv::Scalar(0,0,255),2); } } } } } if(hasBoardMarkers == false){ GoBoardRaux = cv::Mat(); GoBoardTaux = cv::Mat(); return false; } cv::Mat transform_r; BoardImagePoint.clear(); //two methods if(m_PoseMethod == SOLVEPNP){ cv::Mat iprts; cv::Mat raux, taux; cv::solvePnP(m_markerCorners3d, imgPoints, camMatrix, distCoeff,transform_r,taux); transform_r.convertTo(GoBoardRaux,CV_32F); taux.convertTo(GoBoardTaux ,CV_32F); cv::Rodrigues(transform_r, GoBoardRaux); }else{ //test cv::Mat m_markerCorners3d_RPP = cv::Mat::zeros(3, m_markerCorners3d.size(), CV_64F); // 3D points, z is zero cv::Mat imgPoints_RPP = cv::Mat::ones(3, m_markerCorners3d.size(), CV_64F); // 2D points, homogenous points cv::Mat_<float> rotMat(3,3); int iterations; double obj_err; double img_err; //cv::Mat iprts; std::vector<cv::Point2f> iprts; cv::undistortPoints(cv::Mat(imgPoints),iprts,camMatrix, distCoeff); for(size_t i=0; i<iprts.size();i++) { m_markerCorners3d_RPP.at<double>(0,i) = m_markerCorners3d[i].x; m_markerCorners3d_RPP.at<double>(1,i) = m_markerCorners3d[i].y; imgPoints_RPP.at<double>(0,i) = iprts[i].x; imgPoints_RPP.at<double>(1,i) = iprts[i].y; } if(!RPP::Rpp(m_markerCorners3d_RPP, imgPoints_RPP, GoBoardRaux, GoBoardTaux, iterations, obj_err, img_err)) { fprintf(stderr, "Error with RPP\n"); return 1; } cv::solvePnP(m_markerCorners3d, imgPoints, camMatrix, distCoeff,transform_r,GoBoardTaux); cv::Rodrigues(GoBoardRaux, transform_r); } //project 3d points on the image plane //std::cout<<transform_r<<std::endl; cv::projectPoints(Board3DPoint, transform_r, GoBoardTaux, camMatrix, distCoeff, BoardImagePoint); //cv::showAndSave("marker_board", undistortImage); if(showMarkers){ for(size_t c=0;c<BoardImagePoint.size() ; c++){ //MyFilledCircle(undistortImage, m_detectedMarkers[i].points[c],cv::Scalar(0,255,0)); //if(c<4) // cv::line(undistortImage, BoardImagePoint[c], BoardImagePoint[(c+1)%4], cv::Scalar(255,0,0), 2, CV_AA); MyFilledCircle(undistortImage, BoardImagePoint[c], cv::Scalar(0,0,255)); } } fullBoardInScene = true; for(int i=0; i<4;i++){ if(BoardImagePoint[i].x<0 || BoardImagePoint[i].x >frameWidth || BoardImagePoint[i].y<0 || BoardImagePoint[i].y >frameHeight) fullBoardInScene = false; } //boardImagePoint[0-3] are the four corners //if(BoardImagePoint[0].x //cv::showAndSave("marker_board_with_board_pts", undistortImage); if(showMarkers){ cv::imshow("markers",undistortImage); cv::waitKey(1); } return true; }