/* * To work with Kinect or XtionPRO the user must install OpenNI library and PrimeSensorModule for OpenNI and * configure OpenCV with WITH_OPENNI flag is ON (using CMake). */ int main( int argc, char* argv[] ) { /* bool isColorizeDisp, isFixedMaxDisp; int imageMode; bool retrievedImageFlags[5]; string filename; bool isVideoReading; parseCommandLine( argc, argv, isColorizeDisp, isFixedMaxDisp, imageMode, retrievedImageFlags, filename, isVideoReading ); cout << "Device opening ..." << endl; VideoCapture capture; if( isVideoReading ) capture.open( filename ); else capture.open( CV_CAP_OPENNI ); cout << "done." << endl; if( !capture.isOpened() ) { cout << "Can not open a capture object." << endl; return -1; } if( !isVideoReading ) { bool modeRes=false; switch ( imageMode ) { case 0: modeRes = capture.set( CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_VGA_30HZ ); break; case 1: modeRes = capture.set( CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_SXGA_15HZ ); break; case 2: modeRes = capture.set( CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_SXGA_30HZ ); break; default: CV_Error( CV_StsBadArg, "Unsupported image mode property.\n"); } if (!modeRes) cout << "\nThis image mode is not supported by the device, the default value (CV_CAP_OPENNI_SXGA_15HZ) will be used.\n" << endl; } // Print some avalible device settings. cout << "\nDepth generator output mode:" << endl << "FRAME_WIDTH " << capture.get( CV_CAP_PROP_FRAME_WIDTH ) << endl << "FRAME_HEIGHT " << capture.get( CV_CAP_PROP_FRAME_HEIGHT ) << endl << "FRAME_MAX_DEPTH " << capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH ) << " mm" << endl << "FPS " << capture.get( CV_CAP_PROP_FPS ) << endl << "REGISTRATION " << capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ) << endl; if( capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) ) { cout << "\nImage generator output mode:" << endl << "FRAME_WIDTH " << capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_WIDTH ) << endl << "FRAME_HEIGHT " << capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_HEIGHT ) << endl << "FPS " << capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FPS ) << endl; } else { cout << "\nDevice doesn't contain image generator." << endl; if (!retrievedImageFlags[0] && !retrievedImageFlags[1] && !retrievedImageFlags[2]) return 0; } Mat lastbgr; for(;;) { Mat depthMap; Mat validDepthMap; Mat disparityMap; Mat bgrImage; Mat grayImage; if( !capture.grab() ) { cout << "Can not grab images." << endl; return -1; } else { if( retrievedImageFlags[0] && capture.retrieve( depthMap, CV_CAP_OPENNI_DEPTH_MAP ) ) { const float scaleFactor = 0.05f; Mat show; depthMap.convertTo( show, CV_8UC1, scaleFactor ); imshow( "depth map", show ); } if( retrievedImageFlags[1] && capture.retrieve( disparityMap, CV_CAP_OPENNI_DISPARITY_MAP ) ) { if( isColorizeDisp ) { Mat colorDisparityMap; colorizeDisparity( disparityMap, colorDisparityMap, isFixedMaxDisp ? getMaxDisparity(capture) : -1 ); Mat validColorDisparityMap; colorDisparityMap.copyTo( validColorDisparityMap, disparityMap != 0 ); imshow( "colorized disparity map", validColorDisparityMap ); } else { imshow( "original disparity map", disparityMap ); } } if( retrievedImageFlags[2] && capture.retrieve( validDepthMap, CV_CAP_OPENNI_VALID_DEPTH_MASK ) ) imshow( "valid depth mask", validDepthMap ); if( retrievedImageFlags[3] && capture.retrieve( bgrImage, CV_CAP_OPENNI_BGR_IMAGE ) ) imshow( "rgb image", bgrImage ); if( retrievedImageFlags[4] && capture.retrieve( grayImage, CV_CAP_OPENNI_GRAY_IMAGE ) ) imshow( "gray image", grayImage ); if( capture.retrieve( bgrImage, CV_CAP_OPENNI_BGR_IMAGE) && capture.retrieve( disparityMap, CV_CAP_OPENNI_DISPARITY_MAP ) ){ resize( disparityMap, disparityMap, bgrImage.size()); Mat colorDisparityMap; colorizeDisparity( disparityMap, colorDisparityMap, isFixedMaxDisp ? getMaxDisparity(capture) : -1 ); Mat validColorDisparityMap; colorDisparityMap.copyTo( validColorDisparityMap, disparityMap != 0 ); if( !lastbgr.empty() ) { SurfFeatureDetector detector(400); vector<KeyPoint> keypoint1, keypoint2; detector.detect(bgrImage, keypoint1); detector.detect(lastbgr, keypoint2); SurfDescriptorExtractor extractor; Mat des1, des2; extractor.compute(bgrImage, keypoint1, des1); extractor.compute(lastbgr, keypoint2, des2); BFMatcher matcher(NORM_L2); vector< vector<DMatch> > matches; matcher.radiusMatch(des1, des2, matches, 0.2); Mat img = lastbgr; for(vector< vector<DMatch> >::iterator iter = matches.begin(); iter != matches.end(); iter++) { if( iter -> size() > 0) { DMatch match = iter->at(0); line( img, keypoint1[match.queryIdx].pt, keypoint2[match.trainIdx].pt, CV_RGB(255,0,0), 1); } } imshow("vector", img); } bgrImage.copyTo(lastbgr); addWeighted( validColorDisparityMap, 0.5, bgrImage, 0.5, 0.0, bgrImage ); imshow( "mixed image", bgrImage); } } if( waitKey( 1 ) >= 0 ) break; } */ Capture capture; if( !capture.isOpened() ) { cout << "open fail" << endl; //return 0; } cout << capture.getInfo(); if(RoboMap::loadMap("map")) { cout << "failed opeing map" << endl; return 0; } vector<astar::ASPoint> vec = astar::asFindPath(astar::ASPoint(0, 0)); for(int i = 0; i < vec.size(); ++i) cout << "-" << vec[i].col << " " << vec[i].row << endl; return 0; Mat lastImg; Mat lastDepth; while(1) { if (! capture.grab()) break;; Mat img = capture.getBGRImage(); Mat depth = capture.getDepth(); if( !lastImg.empty() && !img.empty() && !lastDepth.empty() && !depth.empty()) { match(lastImg, img, lastDepth, depth); } img.copyTo(lastImg); depth.copyTo(lastDepth); if( waitKey(30) >= 0) break; } return 0; }