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;
}
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;
}
}
/**
* 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();
}
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);
}