KDvoid CornerSubPix ( KDint nIdx )
{
	string	sMsg;
	KDchar	szStr [ 256 ];

	Mat		tSrc;
	Mat		tDst;
	Mat		tGray;
	KDint	nThresh;
	RNG		tRng ( 12345 );

	nThresh = 205;

	// Load source image and convert it to gray
	tSrc = imread ( "/res/image/apple.png" );
	cvtColor ( tSrc, tGray, CV_BGR2GRAY );

	//
	// Apply Shi-Tomasi corner detector
	//
	// Parameters for Shi-Tomasi algorithm
	vector<Point2f>		aCorners;

	KDdouble	dQualityLevel		= 0.01;
	KDdouble	dMinDistance		= 10;
	KDint		nMaxCorners			= 4;
	KDint		nBlockSize			= 3;
	bool		bUseHarrisDetector	= false;
	KDdouble	dK					= 0.04;

	// Copy the source image
	tDst = tSrc.clone ( );

	// Apply corner detection
	goodFeaturesToTrack ( tGray, aCorners, nMaxCorners, dQualityLevel, dMinDistance, Mat ( ), nBlockSize, bUseHarrisDetector, dK ); 

	// Draw corners detected
	kdSprintfKHR ( szStr, "** Number of corners detected: %d\n", aCorners.size ( ) );
	sMsg = szStr;

	KDint		nR = 4;
	for ( KDuint i = 0; i < aCorners.size ( ); i++ )
	{
		circle ( tDst, aCorners [ i ], nR, Scalar ( tRng.uniform ( 0, 255 ), tRng.uniform ( 0, 255 ), tRng.uniform ( 0, 255 ) ), -1, 8, 0 );
	}

	// Set the neeed parameters to find the refined corners
	Size		tWinSize  = Size ( 5, 5 );
	Size		tZeroZone = Size ( -1, -1 );

	TermCriteria  tCriteria = TermCriteria ( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 40, 0.001 );

	// Calculate the refined corner locations
	cornerSubPix ( tGray, aCorners, tWinSize, tZeroZone, tCriteria );

	// Write them down
	for ( KDuint i = 0; i < aCorners.size ( ); i++ )
	{
		kdSprintfKHR ( szStr, " -- Refined Corner [%d] ( %.3f, %.3f )\n", i, aCorners [ i ].x, aCorners [ i ].y );
		sMsg += szStr;
	}	

	g_pController->setFrame ( 1, tSrc );
	g_pController->setFrame ( 2, tDst );
	g_pController->setMessage ( sMsg.c_str ( ) );
}
int main( int argc, char** argv ){

	/// Receive parameters about the template to track
	char *x, *y, *w, *h;
	int xpos = (int)strtol(argv[1],&x,10);
	int ypos = (int)strtol(argv[2],&y,10);
	int width = (int)strtol(argv[3],&w,10);
	int height = (int)strtol(argv[4],&h,10);
	std::cout << xpos << " "  << ypos << " " << width << " " << height << std::endl;

	///Read image and extract template
	std::cout << "Showing template" << std::endl;
	cv::Mat orig = cv::imread("examples/hole1/frame0003.jpg");
	cv::Mat orig_cp = orig.clone();
	cv::Rect patch_roi(xpos,ypos,width,height);
	cv::Mat img_patch = orig(patch_roi);
	cv::Point patch_center((int)(xpos+width/2),(int)(ypos+height/2));
	cv::ellipse(orig_cp, patch_center, cv::Size( width/2, height/2), 0, 0, 360, cv::Scalar( 255, 0, 0 ), 2, 8, 0);
	cv::imshow(disp_window,orig_cp);
	cv::waitKey();

	///Compute features to track
	std::cout << "Calculating features to track" << std::endl;
	cv::Mat orig_gray;
	cvtColor(orig, orig_gray, cv::COLOR_BGR2GRAY);
	const int MAX_CORNERS = 100;
	std::vector<cv::Point2f>corners[2];
	cv::Mat mask = cv::Mat::zeros(orig_gray.rows, orig_gray.cols, CV_8UC1);
	cv::Mat patch_mask = cv::Mat::ones(img_patch.rows, img_patch.cols, CV_8UC1);
	patch_mask.copyTo(mask(patch_roi));
	goodFeaturesToTrack(orig_gray,corners[0],MAX_CORNERS,0.01,10,mask,3,1,0.04);

	/// Set the neeed parameters to find the refined corners
  	cv::Size subPixWinSize = cv::Size( 5, 5 );
  	cv::Size zeroZone = cv::Size( -1, -1 );
  	cv::TermCriteria termcrit = cv::TermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 200, 0.003 );
  	cornerSubPix(orig_gray, corners[0], subPixWinSize, zeroZone, termcrit);
  	for(int i = 0; i < corners[0].size(); i++){
  		cv::circle(orig_cp, corners[0][i], 3, cv::Scalar(0,255,0), -1, 8);
  		//corners[1].push_back(corners[0][i]);
  	}
 	cv::imshow(disp_window,orig_cp);
	cv::waitKey(); 

	/// Calculate optical flow
	std::cout << "Calculating optical flow" << std::endl;
	std::vector<uchar> status;
    std::vector<float> err;
    cv::Size optFlowWinSize = cv::Size(31,31);
    cv::Mat next_frame = cv::imread("examples/hole1/frame0005.jpg");
    cv::Mat next_frame_cp = next_frame.clone();
    cv::Mat next_frame_gray;
    cvtColor(next_frame,next_frame_gray, cv::COLOR_BGR2GRAY);
    calcOpticalFlowPyrLK(orig_gray, next_frame_gray, corners[0], corners[1], status, err, optFlowWinSize, 5, termcrit, 
    	0, 0.01);
    for(int i = 0; i < corners[1].size(); i++){
    	if(!status[i]){
    		if(corners[1][i].x > 0 && corners[1][i].x < orig_gray.cols && corners[1][i].y > 0 && corners[1][i].y < orig_gray.rows ){
	    		std::cout << "good status " << corners[1][i].x << " " << corners[1][i].y << std::endl;
	    		cv::circle(next_frame_cp, corners[0][i], 5, cv::Scalar(0,255,0), -1, 8);
	  			cv::circle(next_frame_cp, corners[1][i], 3, cv::Scalar(255,0,255), -1, 8);
	  			cv::line(next_frame_cp, corners[0][i], corners[1][i], cv::Scalar(255, 0,0),1,8,0);
	  		}
    	}
  	}
  	std::cout << "Drawing" << std::endl;
  	cv::imshow(disp_window,next_frame_cp);
	cv::waitKey();



	return 0;

}
Exemple #3
0
void StereoCalib(const vector<string>& imagelist, Size boardSize, bool useCalibrated=true, bool showRectified=true)
{
    if( imagelist.size() % 2 != 0 )
    {
        cout << "Error: the image list contains odd (non-even) number of elements\n";
        return;
    }
    printf("board size: %d x %d", boardSize.width, boardSize.height);
    bool displayCorners = true;
    const int maxScale = 2;
    const float squareSize = 1.f;  // Set this to your actual square size
    // ARRAY AND VECTOR STORAGE:

    vector<vector<Point2f> > imagePoints[2];
    vector<vector<Point3f> > objectPoints;
    Size imageSize;

    int i, j, k, nimages = (int)imagelist.size()/2;

    imagePoints[0].resize(nimages);
    imagePoints[1].resize(nimages);
    vector<string> goodImageList;

    for( i = j = 0; i < nimages; i++ )
    {
        for( k = 0; k < 2; k++ )
        {
            const string& filename = imagelist[i*2+k];
            Mat img = imread(filename, 0);
            if(img.empty())
                break;
            if( imageSize == Size() )
                imageSize = img.size();
            else if( img.size() != imageSize )
            {
                cout << "The image " << filename << " has the size different from the first image size. Skipping the pair\n";
                break;
            }
            bool found = false;
            vector<Point2f>& corners = imagePoints[k][j];
            for( int scale = 1; scale <= maxScale; scale++ )
            {
                Mat timg;
                if( scale == 1 )
                    timg = img;
                else
                    resize(img, timg, Size(), scale, scale);
                found = findChessboardCorners(timg, boardSize, corners,
                    CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);
                if( found )
                {
                    if( scale > 1 )
                    {
                        Mat cornersMat(corners);
                        cornersMat *= 1./scale;
                    }
                    break;
                }
            }
            if( displayCorners )
            {
                cout << filename << endl;
                Mat cimg, cimg1;
                cvtColor(img, cimg, CV_GRAY2BGR);
                drawChessboardCorners(cimg, boardSize, corners, found);
                double sf = 640./MAX(img.rows, img.cols);
                resize(cimg, cimg1, Size(), sf, sf);
                imshow("corners", cimg1);
                char c = (char)waitKey(500);
                if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit
                    exit(-1);
            }
            else
                putchar('.');
            if( !found )
                break;
            cornerSubPix(img, corners, Size(11,11), Size(-1,-1),
                         TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,
                                      30, 0.01));
        }
        if( k == 2 )
        {
            goodImageList.push_back(imagelist[i*2]);
            goodImageList.push_back(imagelist[i*2+1]);
            j++;
        }
    }
    cout << j << " pairs have been successfully detected.\n";
    nimages = j;
    if( nimages < 2 )
    {
        cout << "Error: too little pairs to run the calibration\n";
        return;
    }

    imagePoints[0].resize(nimages);
    imagePoints[1].resize(nimages);
    objectPoints.resize(nimages);

    for( i = 0; i < nimages; i++ )
    {
        for( j = 0; j < boardSize.height; j++ )
            for( k = 0; k < boardSize.width; k++ )
                objectPoints[i].push_back(Point3f(j*squareSize, k*squareSize, 0));
    }

    cout << "Running stereo calibration ...\n";

    Mat cameraMatrix[2], distCoeffs[2];
    cameraMatrix[0] = Mat::eye(3, 3, CV_64F);
    cameraMatrix[1] = Mat::eye(3, 3, CV_64F);
    Mat R, T, E, F;

    double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
                    cameraMatrix[0], distCoeffs[0],
                    cameraMatrix[1], distCoeffs[1],
                    imageSize, R, T, E, F,
                    TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5),
                    CV_CALIB_FIX_ASPECT_RATIO +
                    CV_CALIB_ZERO_TANGENT_DIST +
                    //CV_CALIB_SAME_FOCAL_LENGTH +
                    CV_CALIB_RATIONAL_MODEL +
                    CV_CALIB_FIX_K3 + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K5);
    cout << "done with RMS error=" << rms << endl;

// CALIBRATION QUALITY CHECK
// because the output fundamental matrix implicitly
// includes all the output information,
// we can check the quality of calibration using the
// epipolar geometry constraint: m2^t*F*m1=0
    double err = 0;
    int npoints = 0;
    vector<Vec3f> lines[2];
    for( i = 0; i < nimages; i++ )
    {
        int npt = (int)imagePoints[0][i].size();
        Mat imgpt[2];
        for( k = 0; k < 2; k++ )
        {
            imgpt[k] = Mat(imagePoints[k][i]);
            undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], Mat(), cameraMatrix[k]);
            computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]);
        }
        for( j = 0; j < npt; j++ )
        {
            double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] +
                                imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) +
                           fabs(imagePoints[1][i][j].x*lines[0][j][0] +
                                imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]);
            err += errij;
        }
        npoints += npt;
    }
    cout << "average reprojection err = " <<  err/npoints << endl;

    // save intrinsic parameters
    FileStorage fs("calib/intrinsics.yml", CV_STORAGE_WRITE);
    if( fs.isOpened() )
    {
        fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] <<
            "M2" << cameraMatrix[1] << "D2" << distCoeffs[1];
        fs.release();
    }
    else
        cout << "Error: can not save the intrinsic parameters\n";

    Mat R1, R2, P1, P2, Q;
    Rect validRoi[2];

    stereoRectify(cameraMatrix[0], distCoeffs[0],
                  cameraMatrix[1], distCoeffs[1],
                  imageSize, R, T, R1, R2, P1, P2, Q,
                  CALIB_ZERO_DISPARITY, 1, imageSize, &validRoi[0], &validRoi[1]);

    fs.open("calib/extrinsics.yml", CV_STORAGE_WRITE);
    if( fs.isOpened() )
    {
        fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q;
        fs.release();
    }
    else
        cout << "Error: can not save the intrinsic parameters\n";

    // OpenCV can handle left-right
    // or up-down camera arrangements
    bool isVerticalStereo = fabs(P2.at<double>(1, 3)) > fabs(P2.at<double>(0, 3));

// COMPUTE AND DISPLAY RECTIFICATION
    if( !showRectified )
        return;

    Mat rmap[2][2];
// IF BY CALIBRATED (BOUGUET'S METHOD)
    if( useCalibrated )
    {
        // we already computed everything
    }
// OR ELSE HARTLEY'S METHOD
    else
 // use intrinsic parameters of each camera, but
 // compute the rectification transformation directly
 // from the fundamental matrix
    {
        vector<Point2f> allimgpt[2];
        for( k = 0; k < 2; k++ )
        {
            for( i = 0; i < nimages; i++ )
                std::copy(imagePoints[k][i].begin(), imagePoints[k][i].end(), back_inserter(allimgpt[k]));
        }
        F = findFundamentalMat(Mat(allimgpt[0]), Mat(allimgpt[1]), FM_8POINT, 0, 0);
        Mat H1, H2;
        stereoRectifyUncalibrated(Mat(allimgpt[0]), Mat(allimgpt[1]), F, imageSize, H1, H2, 3);

        R1 = cameraMatrix[0].inv()*H1*cameraMatrix[0];
        R2 = cameraMatrix[1].inv()*H2*cameraMatrix[1];
        P1 = cameraMatrix[0];
        P2 = cameraMatrix[1];
    }

    //Precompute maps for cv::remap()
    initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]);
    initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]);

    Mat canvas;
    double sf;
    int w, h;
    if( !isVerticalStereo )
    {
        sf = 600./MAX(imageSize.width, imageSize.height);
        w = cvRound(imageSize.width*sf);
        h = cvRound(imageSize.height*sf);
        canvas.create(h, w*2, CV_8UC3);
    }
    else
    {
        sf = 300./MAX(imageSize.width, imageSize.height);
        w = cvRound(imageSize.width*sf);
        h = cvRound(imageSize.height*sf);
        canvas.create(h*2, w, CV_8UC3);
    }

    for( i = 0; i < nimages; i++ )
    {
        for( k = 0; k < 2; k++ )
        {
            Mat img = imread(goodImageList[i*2+k], 0), rimg, cimg;
            remap(img, rimg, rmap[k][0], rmap[k][1], CV_INTER_LINEAR);
            cvtColor(rimg, cimg, CV_GRAY2BGR);
            Mat canvasPart = !isVerticalStereo ? canvas(Rect(w*k, 0, w, h)) : canvas(Rect(0, h*k, w, h));
            resize(cimg, canvasPart, canvasPart.size(), 0, 0, CV_INTER_AREA);
            if( useCalibrated )
            {
                Rect vroi(cvRound(validRoi[k].x*sf), cvRound(validRoi[k].y*sf),
                          cvRound(validRoi[k].width*sf), cvRound(validRoi[k].height*sf));
                rectangle(canvasPart, vroi, Scalar(0,0,255), 3, 8);
            }
        }

        if( !isVerticalStereo )
            for( j = 0; j < canvas.rows; j += 16 )
                line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8);
        else
            for( j = 0; j < canvas.cols; j += 16 )
                line(canvas, Point(j, 0), Point(j, canvas.rows), Scalar(0, 255, 0), 1, 8);
        imshow("rectified", canvas);
        char c = (char)waitKey();
        if( c == 27 || c == 'q' || c == 'Q' )
            break;
    }
}
Exemple #4
0
/**
 * Find a list of candidate marker from a given scene
 *
 * @param current frame, in grayscale 8UC1 format
 * @return a list of marker candidates
 **/
vector<Marker> MarkerDetector::findMarkerCandidates( Mat& frame ) {
    vector<Marker> candidates;
    
    /* Do some thresholding, in fact you should tune the parameters here a bit */
    Mat thresholded;
    threshold( frame, thresholded, 50.0, 255.0, CV_THRESH_BINARY );
    
    /* Find contours */
    vector<vector<Point>> contours;
    findContours( thresholded.clone(), contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE );
    
    for( vector<Point> contour: contours ) {
        /* Approximate polygons out of these contours */
        vector<Point> approxed;
        approxPolyDP( contour, approxed, contour.size() * 0.05, true );
        
        /* Make sure it passes our first candidate check */
        if( !checkPoints( approxed ) )
            continue;
        
        /* Do some perspective transformation on the candidate marker to a predetermined square */
        Marker marker;
        marker.matrix = Mat( markerHeight, markerWidth, CV_8UC1 );
        std::copy( approxed.begin(), approxed.end(), back_inserter( marker.poly ) );
        
        /* Apply sub pixel search */
        cornerSubPix( thresholded, marker.poly, Size(5, 5), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 40, 0.001) );
        
        /* Projection target */
        const static vector<Point2f> target_corners = {
            Point2f( -0.5f, -0.5f ),
            Point2f( +5.5f, -0.5f ),
            Point2f( +5.5f, +5.5f ),
            Point2f( -0.5f, +5.5f ),
        };
        
        /* Apply perspective transformation, to project our 3D marker to a predefined 2D coords */
        Mat projection = getPerspectiveTransform( marker.poly, target_corners );
        warpPerspective( thresholded, marker.matrix, projection, marker.matrix.size() );
        
        /* Ignore those region that's fully black, or not surrounded by black bars */
        if( sum(marker.matrix) == Scalar(0) ||
           countNonZero( marker.matrix.row(0)) != 0 ||
           countNonZero( marker.matrix.row(markerHeight - 1)) != 0 ||
           countNonZero( marker.matrix.col(0)) != 0 ||
           countNonZero( marker.matrix.col(markerWidth - 1)) != 0 ) {
            continue;
        }
        
        
        /* Find the rotation that has the smallest hex value */
        pair<unsigned int, unsigned int> minimum = { numeric_limits<unsigned int>::max(), 0 };
        vector<unsigned int> codes(markerHeight);
        unsigned int power = 1 << (markerWidth - 3);
        
        /* Rotate the marker 4 times, store the hex code upon each rotation */
        for( int rotation = 0; rotation < 4; rotation++ ) {
            stringstream ss;
            codes[rotation] = 0;
            
            for( int i = 1; i < markerHeight - 1; i++ ) {
                unsigned int code = 0;
                for ( int j = 1; j < markerWidth - 1; j++ ){
                    int value = static_cast<int>(marker.matrix.at<uchar>(i, j));
                    if( value == 0 )
                        code = code + ( power >> j );
                }
                
                ss << hex << code;
            }
            ss >> codes[rotation];
            
            if( minimum.first > codes[rotation] ) {
                minimum.first  = codes[rotation];
                minimum.second = rotation;
            }
            
            flip( marker.matrix, marker.matrix, 1 );
            marker.matrix = marker.matrix.t();
        }
        
        
        rotate( marker.poly.begin(), marker.poly.begin() + ((minimum.second + 2) % 4), marker.poly.end() );
        for( int i = 0; i < minimum.second; i++ ) {
            flip( marker.matrix, marker.matrix, 1 );
            marker.matrix = marker.matrix.t();
        }
        
        marker.code = minimum.first;
        
        candidates.push_back( marker );
    }
    
    return candidates;
}
JNIEXPORT void JNICALL
Java_ph_edu_dlsu_opticalflow_CameraActivity_process(JNIEnv *env, jobject instance,
                                                            jobject pTarget, jbyteArray pSource, jint levels) {
    //long t;
    cv::Mat srcBGR;
    std::vector<std::vector<cv::Point> > contours;
    std::vector<cv::Vec4i> hierarchy;

    cv::RNG randnum(12345); // for random color
    cv::Scalar color;

    AndroidBitmapInfo bitmapInfo;
    uint32_t* bitmapContent;

    if(AndroidBitmap_getInfo(env, pTarget, &bitmapInfo) < 0) abort();
    if(bitmapInfo.format != ANDROID_BITMAP_FORMAT_RGBA_8888) abort();
    if(AndroidBitmap_lockPixels(env, pTarget, (void**)&bitmapContent) < 0) abort();

    /// Access source array data... OK
    jbyte* source = (jbyte*)env->GetPrimitiveArrayCritical(pSource, 0);
    if (source == NULL) abort();

    /// cv::Mat for YUV420sp source and output BGRA
    cv::Mat srcGray(bitmapInfo.height, bitmapInfo.width, CV_8UC1, (unsigned char *)source);
    cv::Mat src(bitmapInfo.height + bitmapInfo.height/2, bitmapInfo.width, CV_8UC1, (unsigned char *)source);
    cv::Mat mbgra(bitmapInfo.height, bitmapInfo.width, CV_8UC4, (unsigned char *)bitmapContent);


/***********************************************************************************************/
    /// Native Image Processing HERE...

    //t = cv::getTickCount();

    if(srcBGR.empty())
        srcBGR = cv::Mat(bitmapInfo.height, bitmapInfo.width, CV_8UC3);

    // RGB frame input
    cv::cvtColor(src, srcBGR, CV_YUV420sp2RGB);

    /// If previous frame doesn't exist yet, initialize to srcGray
    if(previous_gray_frame.empty()){
        srcGray.copyTo(previous_gray_frame);
        LOGI("Initializing previous frame...");
    }

    // Detect the strong corners on an image.
    cv::goodFeaturesToTrack (previous_gray_frame, previous_features,
                             MAX_CORNERS, 0.05, 5, cv::noArray(), 3, false, 0.04);

   // Refines the corner locations
    cornerSubPix (previous_gray_frame, previous_features,
                  cv::Size(win_size, win_size), cv::Size(-1,-1),
                  cv::TermCriteria (CV_TERMCRIT_ITER |CV_TERMCRIT_EPS ,20,0.03));

    std::vector<uchar> features_found;
    // The output status vector. Each element of the vector is set to 1 if the flow for
    // the corresponding features has been found, 0 otherwise

    // Calculates the optical flow for a sparse feature set using the iterative Lucas-Kanade method with
    // pyramids
    cv::calcOpticalFlowPyrLK (previous_gray_frame, srcGray,
                          previous_features, current_features, features_found,
                          cv::noArray(),cv::Size(win_size*4+1,win_size*4+1), 0,
            cv::TermCriteria (CV_TERMCRIT_ITER |CV_TERMCRIT_EPS ,20,0.3));

    for( int i = 0; i < (int)previous_features.size(); i++ )
    {
        if(features_found[i]) {
            // Randomize color and display the velocity vectors
            color = cv::Scalar(randnum.uniform(0, 255), randnum.uniform(0,255), randnum.uniform(0,255) );
            line(srcBGR, previous_features[i], current_features[i], color);
        }
    }


    //LOGI("Processing took %0.2f ms.", 1000*(cv::getTickCount() - t)/(float)cv::getTickFrequency());

    cvtColor(srcBGR, mbgra, CV_BGR2BGRA);

    // Copy the current gray frame into previous_gray_frame
    srcGray.copyTo(previous_gray_frame);

/************************************************************************************************/

    /// Release Java byte buffer and unlock backing bitmap
    //env-> ReleasePrimitiveArrayCritical(pSource,source,0);
    /*
     * If 0, then JNI should copy the modified array back into the initial Java
     * array and tell JNI to release its temporary memory buffer.
     *
     * */

    env-> ReleasePrimitiveArrayCritical(pSource, source, JNI_COMMIT);
    /*
 * If JNI_COMMIT, then JNI should copy the modified array back into the
 * initial array but without releasing the memory. That way, the client code
 * can transmit the result back to Java while still pursuing its work on the
 * memory buffer
 *
 * */

    /*
     * Get<Primitive>ArrayCritical() and Release<Primitive>ArrayCritical()
     * are similar to Get<Primitive>ArrayElements() and Release<Primitive>ArrayElements()
     * but are only available to provide a direct access to the target array
     * (instead of a copy). In exchange, the caller must not perform blocking
     * or JNI calls and should not hold the array for a long time
     *
     */


    if (AndroidBitmap_unlockPixels(env, pTarget) < 0) abort();
}
Exemple #6
0
Calib::Calib()
{
    Size boardSize(6,5); // Chessboard' size in corners with both color (nb of squares -1)
    int widthSquare = 40; // Width of a square in mm
    int heightSquare = 27;
    vector <Mat> images;

    // Getting the four images of the chessboard
    string imageFileName = "../src/mire1.jpg";
    images.push_back(imread(imageFileName, 1));

    imageFileName = "../src/mire2.jpg";
    images.push_back(imread(imageFileName, 1));

    imageFileName = "../src/mire3.jpg";
    images.push_back(imread(imageFileName, 1));

    imageFileName = "../src/mire4.jpg";
    images.push_back(imread(imageFileName, 1));

    Size imageSize = images.at(0).size();

    // Find chessboard's corners in the scene for the 4 images
    vector<vector<Point2f> > cornersScene(1);
    vector<Mat> imagesGray;

    imagesGray.resize(4);

    for (int i=0; i<4; i++)
    {
        if(images.at(i).empty())
        {
            cerr << "Image not read correctly!" << endl;
            exit(-1);
        }

        bool patternFound = findChessboardCorners(images.at(i), boardSize, cornersScene[0]);
        if(!patternFound)
        {
            cerr << "Could not find chess board!" << endl;
            exit(-1);
        }

        // Improve corner's coordinate accuracy
        cvtColor(images.at(i), imagesGray.at(i), CV_RGB2GRAY);
        cornerSubPix(imagesGray.at(i), cornersScene[0], Size(3,2), Size(-1,-1), TermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1));

        // Drawing the corners
        drawChessboardCorners(images.at(i), boardSize, Mat(cornersScene[0]), patternFound );

        imshow("Corners find", images.at(i));

        int keyPressed;
        /*do
        {
            keyPressed = waitKey(0);
        } while (keyPressed != 27);*/
    }

    // Getting the chessboard's corners on the mire's image
    vector<vector<Point3f> > cornersMire(1);

    for( int y = 0; y < boardSize.height; y++ )
    {
        for( int x = 0; x < boardSize.width; x++ )
        {
            cornersMire[0].push_back(cv::Point3f(float(x*widthSquare),
                                                 float(y*heightSquare), 0));
        }
    }

    // Getting the camera's parameters

    Mat distortionCoefficients = Mat::zeros(8, 1, CV_64F);
    Mat cameraMatrix = Mat::eye(3, 3, CV_64F);

    calibrateCamera(cornersMire, cornersScene, imageSize, cameraMatrix,
                    distortionCoefficients, rotationVectors, translationVectors);


    //cout << "Camera matrix: " << cameraMatrix << endl;
    //cout << "Distortion _coefficients: " << distortionCoefficients << endl;
    cout << rotationVectors.at(0) << endl;
    cout << translationVectors.at(0) << endl;

}
///////////////////////////////////////////////////////
// Panel::CalibrateCamera() Description
///////////////////////////////////////////////////////
void Panel::CalibrateCamera(string sFilePath)
{
	help();

	//! [file_read]
	Settings s;
	const string inputSettingsFile = sFilePath;
	FileStorage fs(inputSettingsFile, FileStorage::READ); // Read the settings
	if (!fs.isOpened())
	{
		cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << endl;
//		return -1;
	}
	fs["Settings"] >> s;
	fs.release();                                         // close Settings file
	//! [file_read]

	//FileStorage fout("settings.yml", FileStorage::WRITE); // write config as YAML
	//fout << "Settings" << s;

	if (!s.goodInput)
	{
		cout << "Invalid input detected. Application stopping. " << endl;
//		return -1;
	}

	vector<vector<Point2f> > imagePoints;
	Mat cameraMatrix, distCoeffs;
	Size imageSize;
	int mode = s.inputType == Settings::IMAGE_LIST ? CAPTURING : DETECTION;
	clock_t prevTimestamp = 0;
	const Scalar RED(0, 0, 255), GREEN(0, 255, 0);
	const char ESC_KEY = 27;
	int counter = 1;

	//! [get_input]
	for (;;)
	{
		Mat view;
		bool blinkOutput = false;

		view = s.nextImage();

		//-----  If no more image, or got enough, then stop calibration and show result -------------
		if (mode == CAPTURING && imagePoints.size() >= (size_t)s.nrFrames)
		{
			if (runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints))
				mode = CALIBRATED;
			else
				mode = DETECTION;
		}
		if (view.empty())          // If there are no more images stop the loop
		{
			// if calibration threshold was not reached yet, calibrate now
			if (mode != CALIBRATED && !imagePoints.empty())
				runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints);
			break;
		}
		//! [get_input]

		imageSize = view.size();  // Format input image.
		if (s.flipVertical)    flip(view, view, 0);

		//! [find_pattern]
		vector<Point2f> pointBuf;

		bool found;
		switch (s.calibrationPattern) // Find feature points on the input format
		{
		case Settings::CHESSBOARD:
			found = findChessboardCorners(view, s.boardSize, pointBuf,
				CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);
			break;
		case Settings::CIRCLES_GRID:
			found = findCirclesGrid(view, s.boardSize, pointBuf);
			break;
		case Settings::ASYMMETRIC_CIRCLES_GRID:
			found = findCirclesGrid(view, s.boardSize, pointBuf, CALIB_CB_ASYMMETRIC_GRID);
			break;
		default:
			found = false;
			break;
		}
		//! [find_pattern]
		//! [pattern_found]
		if (found)                // If done with success,
		{
			// improve the found corners' coordinate accuracy for chessboard
			if (s.calibrationPattern == Settings::CHESSBOARD)
			{
				Mat viewGray;
				cvtColor(view, viewGray, COLOR_BGR2GRAY);
				cornerSubPix(viewGray, pointBuf, Size(11, 11),
					Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.1));
			}

			if (mode == CAPTURING &&  // For camera only take new samples after delay time
				(!s.inputCapture.isOpened() || clock() - prevTimestamp > s.delay*1e-3*CLOCKS_PER_SEC))
			{
				imagePoints.push_back(pointBuf);
				prevTimestamp = clock();
				blinkOutput = s.inputCapture.isOpened();
			}

			// Draw the corners.
			drawChessboardCorners(view, s.boardSize, Mat(pointBuf), found);
		}
		//! [pattern_found]
		//----------------------------- Output Text ------------------------------------------------
		//! [output_text]
		string msg = (mode == CAPTURING) ? "100/100" :
			mode == CALIBRATED ? "Calibrated" : "Press 'g' to start";
		int baseLine = 0;
		Size textSize = getTextSize(msg, 1, 1, 1, &baseLine);
		Point textOrigin(view.cols - 2 * textSize.width - 10, view.rows - 2 * baseLine - 10);

		if (mode == CAPTURING)
		{
			if (s.showUndistorsed)
				msg = format("%d/%d Undist", (int)imagePoints.size(), s.nrFrames);
			else
				msg = format("%d/%d", (int)imagePoints.size(), s.nrFrames);
		}

		putText(view, msg, textOrigin, 1, 1, mode == CALIBRATED ? GREEN : RED);

		if (blinkOutput)
			bitwise_not(view, view);
		//! [output_text]
		//------------------------- Video capture  output  undistorted ------------------------------
		//! [output_undistorted]
		if (mode == CALIBRATED && s.showUndistorsed)
		{
			Mat temp = view.clone();
			undistort(temp, view, cameraMatrix, distCoeffs);
		}
		//! [output_undistorted]
		//------------------------------ Show image and check for input commands -------------------
		//! [await_input]
		
		namedWindow("Image View" + to_string(counter), WINDOW_NORMAL);
		resizeWindow("Image View" + to_string(counter), 640, 480);
		imshow("Image View" + to_string(counter), view);
		char key = (char)waitKey(s.inputCapture.isOpened() ? 50 : s.delay);

		cout << "Image " << to_string(counter) << " Completed" << endl;
		counter++;

		if (key == ESC_KEY)
			break;

		if (key == 'u' && mode == CALIBRATED)
			s.showUndistorsed = !s.showUndistorsed;

		if (s.inputCapture.isOpened() && key == 'g')
		{
			mode = CAPTURING;
			imagePoints.clear();
		}
		//! [await_input]
	}

	// -----------------------Show the undistorted image for the image list ------------------------
	//! [show_results]
	if (s.inputType == Settings::IMAGE_LIST && s.showUndistorsed)
	{
		Mat view, rview, map1, map2;
		initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
			getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0),
			imageSize, CV_16SC2, map1, map2);

		m_mainMap1 = map1;
		m_mainMap2 = map2;

		for (size_t i = 0; i < s.imageList.size(); i++)
		{
			view = imread(s.imageList[i], 1);
			if (view.empty())
				continue;
			remap(view, rview, map1, map2, INTER_LINEAR);
			imshow("Image View", rview);
			char c = (char)waitKey();
			if (c == ESC_KEY || c == 'q' || c == 'Q')
				break;
		}
	}
	//! [show_results]

//	return 0;

}
///////////////////////////////////////////////////////
// Panel::PixelsToLength() 
//  Description: Not currently used: Calculates the
// ratio of cm to pixels from one checkerboard image
///////////////////////////////////////////////////////
void Panel::PixelsToLength(string sImgPath)
{
	cout << "Pixels to Length" << endl << endl;
	m_pPanel->m_Image = imread(sImgPath);

	Point2f corner1;
	Point2f corner2;
	Point2f corner3;
	Point2f corner4;

	bool found = false;

	found = findChessboardCorners(m_pPanel->m_Image, Size(9, 6), m_pPanel->corners,
		CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);

	Mat imgGray;
	cvtColor(m_pPanel->m_Image, imgGray, COLOR_BGR2GRAY);
	cornerSubPix(imgGray, m_pPanel->corners, Size(11, 11), Size(-1, -1),
		TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

	corner1.x = m_pPanel->corners[45].x;
	corner1.y = m_pPanel->corners[45].y;

	corner2.x = m_pPanel->corners[0].x;
	corner2.y = m_pPanel->corners[0].y;

	corner3.x = m_pPanel->corners[8].x;
	corner3.y = m_pPanel->corners[8].y;

	corner4.x = m_pPanel->corners[53].x;
	corner4.y = m_pPanel->corners[53].y;

	// Draw rectangle around checkerboard

	line(m_pPanel->m_Image, corner1, corner2, CV_RGB(0, 0, 255), 2);
	line(m_pPanel->m_Image, corner2, corner3, CV_RGB(0, 0, 255), 2);
	line(m_pPanel->m_Image, corner3, corner4, CV_RGB(0, 0, 255), 2);
	line(m_pPanel->m_Image, corner4, corner1, CV_RGB(0, 0, 255), 2);

	circle(m_pPanel->m_Image, corner1, 10, CV_RGB(0, 0, 255), 2);
	circle(m_pPanel->m_Image, corner2, 10, CV_RGB(0, 255, 0), 2);
	circle(m_pPanel->m_Image, corner3, 10, CV_RGB(255, 0, 0), 2);
	circle(m_pPanel->m_Image, corner4, 10, CV_RGB(100, 100, 100), 2);

	double pixel_length = 0;
	double pixel_width = 0;

	double pixel_width1 = norm(corner1 - corner2);
	double pixel_length1 = norm(corner2 - corner3);
	double pixel_width2 = norm(corner3 - corner4);
	double pixel_length2 = norm(corner4 - corner1);

	if (pixel_length1 >= pixel_length2)
		pixel_length = pixel_length1;
	else 
		pixel_length = pixel_length2;

	if (pixel_width1 >= pixel_width2)
		pixel_width = pixel_width1;
	else
		pixel_width = pixel_width2;


	double ratio = (m_pPanel->m_boardLength - 1.0) / (m_pPanel->m_boardWidth - 1.0);

	if (pixel_length >= (pixel_width * ratio)){
		m_pPanel->m_cmPerPixel = (m_pPanel->m_squareSize * (float)(m_pPanel->m_boardLength - 1)) / pixel_length;
	}
	else
		m_pPanel->m_cmPerPixel = (m_pPanel->m_squareSize * (float)(m_pPanel->m_boardWidth - 1)) / pixel_width;

	cout << "cm per pixel : " << m_pPanel->m_cmPerPixel << endl;

	// Perspective Transform
	//	double ratio = 8.0 / 5.0;
	//	double length = ratio * pixel_width;
	//
	//	vector<Point2f> panel_pts;
	//	vector<Point2f> rect_pts;
	//	panel_pts.push_back(corner1);
	//	panel_pts.push_back(corner2);
	//	panel_pts.push_back(corner3);
	//	panel_pts.push_back(corner4);
	//	rect_pts.push_back(Point2f(0, 0));
	//	rect_pts.push_back(Point2f((float)width, 0));
	//	rect_pts.push_back(Point2f((float)width, (float)length));
	//	rect_pts.push_back(Point2f(0, (float)length));
	//
	//	// Draw new rectangle
	//	line(m_pPanel->m_Image, rect_pts[0], rect_pts[1], CV_RGB(255, 0, 0), 2);
	//	line(m_pPanel->m_Image, rect_pts[1], rect_pts[2], CV_RGB(255, 0, 0), 2);
	//	line(m_pPanel->m_Image, rect_pts[2], rect_pts[3], CV_RGB(255, 0, 0), 2);
	//	line(m_pPanel->m_Image, rect_pts[3], rect_pts[0], CV_RGB(255, 0, 0), 2);
	//
	//	// Perspective Transorm
	//	Mat transmtx = getPerspectiveTransform(panel_pts, rect_pts);
	//	int offsetSize = 500;
	//	Mat transformed = Mat::zeros(m_pPanel->m_Image.cols + offsetSize, m_pPanel->m_Image.rows + offsetSize, CV_8UC3);
	//	warpPerspective(m_pPanel->m_Image, transformed, transmtx, transformed.size());
	//
	////	Mat subImg(transformed, Rect(corner1.x, corner1.y, width, length));
	//
	//	namedWindow("Original", WINDOW_NORMAL);
	//	imshow("Original", m_pPanel->m_Image);
	//	namedWindow("Warped", WINDOW_AUTOSIZE);
	//	imshow("Warped", transformed);
}