bool Epipolar::isValidPair(vector<DMatch>& matches, vector<KeyPoint>& key1, vector<KeyPoint>& key2, Mat& cam, Mat& distor, Mat& ess, Mat& inliersMask, double inlierPercent){ vector<Point2f>pts1, pts2; inliersMask.deallocate(); Mat rot, trans; size_t n = matches.size(); Utility:: getPointMatches(key1, key2, matches, pts1, pts2); undistortPoints(pts1, pts1, cam, distor); undistortPoints(pts2, pts2, cam, distor); ess = findEssentialMat(pts1, pts2, 1.0, Point(0, 0), RANSAC, 0.999, 1.25, inliersMask); int inliers = recoverPose(ess, pts1, pts2, rot, trans, 1.0, Point(0, 0), inliersMask); return ((double)inliers / n) > inlierPercent; }
// Callback void imgcb(const sensor_msgs::Image::ConstPtr& msg) { oneTwo = ~oneTwo; if(oneTwo) { try { // Convert ROS images to cv::mat cv_bridge::CvImageConstPtr cv_ptr; cv_ptr = cv_bridge::toCvShare(msg); IplImage *img = new IplImage(cv_ptr->image); // Erosion dilation factor int erosion_size = 0; int dilation_size = 0; Mat elementE; Mat elementD; if(initial == FALSE) { prevImg = cv::Mat(cv_ptr->image.rows, cv_ptr->image.cols, cv_ptr->image.type()); currImg = cv::Mat(cv_ptr->image.rows, cv_ptr->image.cols, cv_ptr->image.type()); nbGreen = 0; nbOrange = 0; nbFrames = 1; initial = TRUE; } // Matrice used cv::Mat *prevImgBlur = new cv::Mat(cv_ptr->image.rows, cv_ptr->image.cols, cv_ptr->image.type()); cv::Mat *currImgBlur = new cv::Mat(cv_ptr->image.rows, cv_ptr->image.cols, cv_ptr->image.type()); cv::Mat *diffImg = new cv::Mat(cv_ptr->image.rows, cv_ptr->image.cols, CV_BGR2GRAY); cv::Mat *prevImgGrey = new cv::Mat(cv_ptr->image.rows, cv_ptr->image.cols, CV_BGR2GRAY); cv::Mat *currImgGrey = new cv::Mat(cv_ptr->image.rows, cv_ptr->image.cols, CV_BGR2GRAY); // Blur component cv::Point myAnchor =cv::Point(-1,-1); cv::Size sizeBlur = Size(10,10); cv::imshow("Initial", cv_ptr->image); cv::waitKey(1); // Update screen /*********************************/ /////////////////// // // // Tracker // // // /////////////////// // Get current image and update previous one currImg.copyTo(prevImg); cv_ptr->image.copyTo(currImg); cvtColor(prevImg, *prevImgGrey, CV_BGR2GRAY ); cvtColor(currImg, *currImgGrey, CV_BGR2GRAY ); // Filtre "blur" sur les deux images blur(*prevImgGrey, *prevImgBlur, sizeBlur, myAnchor, BORDER_DEFAULT ); blur(*currImgGrey, *currImgBlur, sizeBlur, myAnchor, BORDER_DEFAULT ); // Diff between both images absdiff(*prevImgBlur, *currImgBlur, *diffImg); // Threshold on the diff threshold( *diffImg, *diffImg, 3, 255, 0 ); //morphological opening (remove small objects from the foreground) erode(*diffImg, *diffImg, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) ); dilate( *diffImg, *diffImg, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)) ); //morphological closing (fill small holes in the foreground) dilate( *diffImg, *diffImg, getStructuringElement(MORPH_ELLIPSE, Size(20, 20)) ); erode(*diffImg, *diffImg, getStructuringElement(MORPH_ELLIPSE, Size(20, 20)) ); cv::imshow("diffImg", *diffImg); vector<vector<Point> > contours; vector<Vec4i> hierarchy; // Find contours findContours( *diffImg, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);//, Point(0, 0) ); // Approximate contours to polygons + get bounding rects and circles vector<vector<Point> > contours_poly( contours.size() ); vector<Rect> boundRect( contours.size() ); for( int i = 0; i < contours.size(); i++ ) { approxPolyDP( Mat(contours[i]), contours_poly[i], 3, true ); boundRect[i] = boundingRect( Mat(contours_poly[i]) ); } int j = 0; vector<Rect> lBoundRect( contours.size() ); for( int i = 0; i < contours.size(); i++ ) { if( (boundRect[i].area()) > MIN_AREA_RECTANGLE) { lBoundRect[j] = boundRect[i]; j++; } } // If there is some movement if(j != 0) { // Draw polygonal contour + bonding rects + circles Mat drawing = Mat::zeros( diffImg->size(), CV_8UC3 ); Mat andFilter = Mat::zeros( diffImg->size(), CV_8UC3 ); for( int i = 0; i< j; i++ ) { //Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) ); rectangle( drawing, lBoundRect[i].tl(), lBoundRect[i].br(), Scalar(255,255,255), CV_FILLED, 8, 0 ); } // AND filter on the initial image bitwise_and(drawing, currImg, andFilter, noArray()); // Find color bool isGreen = detect_color(Scalar(70,64,0), Scalar(100,139,255), &andFilter); bool isOrange = detect_color(Scalar(0,128,0), Scalar(20,200,255), &andFilter); if(isGreen) { nbGreen += 1; } if(isOrange) { nbOrange += 1; } // Send data /// Show in a window cv::imshow("final", andFilter); drawing.refcount = 0; drawing.deallocate(); } else { // Find color bool isGreen = detect_color(Scalar(70,64,0), Scalar(100,139,255), &currImg); bool isOrange = detect_color(Scalar(0,128,0), Scalar(20,200,255), &currImg); if(isGreen) { nbGreen += 1; } if(isOrange) { nbOrange += 1; } } // Destruct and free memory prevImgBlur->refcount = 0; prevImgBlur->deallocate(); currImgBlur->refcount = 0; currImgBlur->deallocate(); diffImg->refcount = 0; diffImg->deallocate(); currImgGrey->refcount = 0; currImgGrey->deallocate(); prevImgGrey->refcount = 0; prevImgGrey->deallocate(); if(nbFrames > FRAME_PER_SEC) { if(nbGreen > FRAME_PER_SEC_DIV_2) { if(nbOrange > FRAME_PER_SEC_DIV_2) { ROS_INFO("Green & Orange"); } else { ROS_INFO("Green"); } } else { if(nbOrange > FRAME_PER_SEC_DIV_2) { ROS_INFO("Orange"); } } nbGreen = 0; nbOrange = 0; nbFrames = 0; } else { nbFrames +=1; } } catch (const cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); } } }
int main(int argc, char **argv) { if (argc > 2) { std::cout << "Only the path of an image can be passed in arg" << std::endl; return -1; } Camera* zed; if (argc == 1) zed = new Camera(HD720); else zed = new Camera(argv[1]); // init computation mode of the zed ERRCODE err = zed->init(MODE::QUALITY, -1, true); //need quite a powerful graphic card in QUALITY // ERRCODE display cout << errcode2str(err) << endl; // Quit if an error occurred if (err != SUCCESS) { delete zed; return 1; } // print on screen the keys that can be used bool printHelp = false; std::string helpString = "[p] increase distance, [m] decrease distance, [q] quit"; // get the focale and the baseline of the zed float fx = zed->getParameters()->RightCam.fx; // here we work with the right camera float baseline = zed->getParameters()->baseline; // get width and height of the ZED images int width = zed->getImageSize().width; int height = zed->getImageSize().height; // create and alloc GPU memory for the disparity matrix Mat disparityRightGPU; disparityRightGPU.data = (unsigned char*) nppiMalloc_32f_C1(width, height, &disparityRightGPU.step); disparityRightGPU.setUp(width, height, 1, sl::zed::FLOAT, GPU); // create and alloc GPU memory for the depth matrix Mat depthRightGPU; depthRightGPU.data = (unsigned char*) nppiMalloc_32f_C1(width, height, &depthRightGPU.step); depthRightGPU.setUp(width, height, 1, sl::zed::FLOAT, GPU); // create and alloc GPU memory for the image matrix Mat imageDisplayGPU; imageDisplayGPU.data = (unsigned char*) nppiMalloc_8u_C4(width, height, &imageDisplayGPU.step); imageDisplayGPU.setUp(width, height, 4, sl::zed::UCHAR, GPU); // create a CPU image for display purpose cv::Mat imageDisplay(height, width, CV_8UC4); float depthMax = 6.; //Meter bool depthMaxAsChanged = true; char key = ' '; // launch a loop bool run = true; while (run) { // Grab the current images and compute the disparity bool res = zed->grab(RAW, 0, 1); // get the right image // !! WARNING !! this is not a copy, here we work with the data allocated by the zed object // this can be done ONLY if we call ONE time this methode before the next grab, make a copy if you want to get multiple IMAGE Mat imageRightGPU = zed->getView_gpu(STEREO_RIGHT); // get the disparity // !! WARNING !! this is not a copy, here we work with the data allocated by the zed object // this can be done ONLY if we call ONE time this methode before the next grab, make a copy if you want to get multiple MEASURE Mat disparityGPU = zed->retrieveMeasure_gpu(DISPARITY); // Call the cuda function that convert the disparity from left to right cuConvertDisparityLeft2Right(disparityGPU, disparityRightGPU); // Call the cuda function that convert disparity to depth cuConvertDisparity2Depth(disparityRightGPU, depthRightGPU, fx, baseline); // Call the cuda function that convert depth to color and merge it with the current right image cuOverlayImageAndDepth(depthRightGPU, imageRightGPU, imageDisplayGPU, depthMax); // Copy the processed image frome the GPU to the CPU for display cudaMemcpy2D((uchar*) imageDisplay.data, imageDisplay.step, (Npp8u*) imageDisplayGPU.data, imageDisplayGPU.step, imageDisplayGPU.getWidthByte(), imageDisplayGPU.height, cudaMemcpyDeviceToHost); if (printHelp) // write help text on the image if needed cv::putText(imageDisplay, helpString, cv::Point(20, 20), CV_FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(111, 111, 111, 255), 2); // display the result cv::imshow("Image right Overlay", imageDisplay); key = cv::waitKey(20); switch (key)// handle the pressed key { case 'q': // close the program case 'Q': run = false; break; case 'p': // increase the distance threshold case 'P': depthMax += 1; depthMaxAsChanged = true; break; case 'm': // decrease the distance threshold case 'M': depthMax = (depthMax > 1 ? depthMax - 1 : 1); depthMaxAsChanged = true; break; case 'h': // print help case 'H': printHelp = !printHelp; cout << helpString << endl; break; default: break; } if (depthMaxAsChanged) { cout << "New distance max " << depthMax << "m" << endl; depthMaxAsChanged = false; } } // free all the allocated memory before quit imageDisplay.release(); disparityRightGPU.deallocate(); depthRightGPU.deallocate(); imageDisplayGPU.deallocate(); delete zed; return 0; }