void mapDepthRGB (IplImage *rawDepth, IplImage *rawRGB, IplImage *undistortedDepth, IplImage *undistortedRGB, double *rgbToWorld, int *worldToRGB) {
int u, v, uRGB, vRGB, i;
double x, y, z, x2, y2, z2;
IplImage *newColorFrame = cvCreateImage(cvSize(640, 480), 8, 3);
cvCvtColor(rawRGB, newColorFrame, CV_RGB2BGR);
cvUndistort2(newColorFrame, undistortedRGB, &rgbIntrinsic, &rgbDistortion);
cvUndistort2(rawDepth, undistortedDepth, &dIntrinsic, &dDistortion);
for(i=0; i < KINECT_RES; i++) {
worldToRGB[i*2]=0;
worldToRGB[i*2+1]=0;
rgbToWorld[i*3]=0;
rgbToWorld[i*3+1]=0;
rgbToWorld[i*3+2]=0;
}
for (v=0; v < 480; v++) {
for (u=0; u < 640; u++) {
i=v*640+u;
if (u >=10 && u < 630 && v >= 10 && v < 470) {
z = depthTable[((short*)undistortedDepth->imageData)[i]];
if ((z < 0.5) || (z >10)) {
x2=0.0; y2=0.0; z2=0;
uRGB=0, vRGB=0;
}
else {
x = (u - dIntrinsicVals[2])*z / dIntrinsicVals[0];
y = (v - dIntrinsicVals[5])*z / dIntrinsicVals[4];
x2 = (x*rotationVals[0] + y*rotationVals[1] + z*rotationVals[2]) + transFudgedVals[0];
y2 = (x*rotationVals[3] + y*rotationVals[4] + z*rotationVals[5]) + transFudgedVals[1];
z2 = (x*rotationVals[6] + y*rotationVals[7] + z*rotationVals[8]) + transFudgedVals[2];
uRGB = (int)((x2*rgbIntrinsicVals[0]/z2) + rgbIntrinsicVals[2]);
vRGB = (int)((y2*rgbIntrinsicVals[4]/z2) + rgbIntrinsicVals[5]);
if ((uRGB < 0) || (vRGB < 0) || (uRGB >= 640) || (vRGB >= 480)) {
x2=0; y2=0; z2=0;
uRGB=0, vRGB=0;
}
}
worldToRGB[i*2]=uRGB;
worldToRGB[i*2+1]=vRGB;
rgbToWorld[(vRGB*640+uRGB)*3]=x2;
rgbToWorld[(vRGB*640+uRGB)*3+1]=y2;
rgbToWorld[(vRGB*640+uRGB)*3+2]=z2;
}
}
}
cvReleaseImage(&newColorFrame);
}
void cvUnDistortOnce( const CvArr* src, CvArr* dst,
const float* intrinsic_matrix,
const float* distortion_coeffs,
int )
{
CvMat _a = cvMat( 3, 3, CV_32F, (void*)intrinsic_matrix );
CvMat _k = cvMat( 4, 1, CV_32F, (void*)distortion_coeffs );
cvUndistort2( src, dst, &_a, &_k, 0 );
}
void CV_UndistortBadArgTest::run_func()
{
if (useCPlus)
{
cv::undistort(src,dst,camera_mat,distortion_coeffs,new_camera_mat);
}
else
{
cvUndistort2(_src,_dst,_camera_mat,_distortion_coeffs,_new_camera_mat);
}
}
Image* AbstractCamera::previewUndistort(string distortedImagePath) {
Image* returnImages = new Image[2];
returnImages[0] = Image(distortedImagePath.c_str());
if (returnImages[0].getIplImage() != NULL) {
returnImages[1] = returnImages[0].getCopy();
//TODO Check performance related to different undistortion approaches (cvUndistortInit+cvRemap)
cvUndistort2(returnImages[0].getIplImage(), returnImages[1].getIplImage(), _intrinsicParams,
_distortionCoefficients);
}
return returnImages;
}
//--------------------------------------------------------------------------------
void ofxCvImage::undistort( float radialDistX, float radialDistY,
float tangentDistX, float tangentDistY,
float focalX, float focalY,
float centerX, float centerY ){
if( !bAllocated ){
ofLogError("ofxCvImage") << "undistort(): image not allocated";
return;
}
float camIntrinsics[] = { focalX, 0, centerX, 0, focalY, centerY, 0, 0, 1 };
float distortionCoeffs[] = { radialDistX, radialDistY, tangentDistX, tangentDistY };
CvMat _a = cvMat( 3, 3, CV_32F, (void*)camIntrinsics );
CvMat _k = cvMat( 4, 1, CV_32F, (void*)distortionCoeffs );
cvUndistort2( cvImage, cvImageTemp, &_a, &_k, 0 );
swapTemp();
flagImageChanged();
}
void mitk::UndistortCameraImage::UndistortImage(IplImage *src, IplImage *dst)
{
// init intrinsic camera matrix [fx 0 cx; 0 fy cy; 0 0 1].
m_intrinsicMatrixData[0] = (double)m_fcX;
m_intrinsicMatrixData[1] = 0.0;
m_intrinsicMatrixData[2] = (double)m_ccX;
m_intrinsicMatrixData[3] = 0.0;
m_intrinsicMatrixData[4] = (double)m_fcY;
m_intrinsicMatrixData[5] = (double)m_ccY;
m_intrinsicMatrixData[6] = 0.0;
m_intrinsicMatrixData[7] = 0.0;
m_intrinsicMatrixData[8] = 1.0;
m_intrinsicMatrix = cvMat(3, 3, CV_32FC1, m_intrinsicMatrixData);
// init distortion matrix
m_distortionMatrix = cvMat(1, 4, CV_32F, m_distortionMatrixData);
// undistort
cvUndistort2(src,dst, &m_intrinsicMatrix, &m_distortionMatrix);
}
bool CvCalibFilter::Undistort( CvMat** srcarr, CvMat** dstarr )
{
int i;
if( !srcarr || !dstarr )
{
assert(0);
return false;
}
if( isCalibrated )
{
for( i = 0; i < cameraCount; i++ )
{
if( srcarr[i] && dstarr[i] )
{
CvMat src_stub, *src;
CvMat dst_stub, *dst;
src = cvGetMat( srcarr[i], &src_stub );
dst = cvGetMat( dstarr[i], &dst_stub );
if( src->data.ptr == dst->data.ptr )
{
if( !undistImg || undistImg->width != src->width ||
undistImg->height != src->height ||
CV_ARE_TYPES_EQ( undistImg, src ))
{
cvReleaseMat( &undistImg );
undistImg = cvCreateMat( src->height, src->width, src->type );
}
cvCopy( src, undistImg );
src = undistImg;
}
#if 1
{
CvMat A = cvMat( 3, 3, CV_32FC1, cameraParams[i].matrix );
CvMat k = cvMat( 1, 4, CV_32FC1, cameraParams[i].distortion );
if( !undistMap[i][0] || undistMap[i][0]->width != src->width ||
undistMap[i][0]->height != src->height )
{
cvReleaseMat( &undistMap[i][0] );
cvReleaseMat( &undistMap[i][1] );
undistMap[i][0] = cvCreateMat( src->height, src->width, CV_32FC1 );
undistMap[i][1] = cvCreateMat( src->height, src->width, CV_32FC1 );
cvInitUndistortMap( &A, &k, undistMap[i][0], undistMap[i][1] );
}
cvRemap( src, dst, undistMap[i][0], undistMap[i][1] );
#else
cvUndistort2( src, dst, &A, &k );
#endif
}
}
}
}
else
{
for( i = 0; i < cameraCount; i++ )
{
if( srcarr[i] != dstarr[i] )
cvCopy( srcarr[i], dstarr[i] );
}
}
return true;
}
int main( int argc, char** argv )
{
CvSize board_size = {0,0};
float square_size = 1.f, aspect_ratio = 1.f;
const char* out_filename = "out_camera_data.yml";
const char* input_filename = 0;
int i, image_count = 10;
int write_extrinsics = 0, write_points = 0;
int flags = 0;
CvCapture* capture = 0;
FILE* f = 0;
char imagename[1024];
CvMemStorage* storage;
CvSeq* image_points_seq = 0;
int elem_size, flip_vertical = 0;
int delay = 1000;
clock_t prev_timestamp = 0;
CvPoint2D32f* image_points_buf = 0;
CvFont font = cvFont( 1, 1 );
double _camera[9], _dist_coeffs[4];
CvMat camera = cvMat( 3, 3, CV_64F, _camera );
CvMat dist_coeffs = cvMat( 1, 4, CV_64F, _dist_coeffs );
CvMat *extr_params = 0, *reproj_errs = 0;
double avg_reproj_err = 0;
int mode = DETECTION;
int undistort_image = 0;
CvSize img_size = {0,0};
const char* live_capture_help =
"When the live video from camera is used as input, the following hot-keys may be used:\n"
" <ESC>, 'q' - quit the program\n"
" 'g' - start capturing images\n"
" 'u' - switch undistortion on/off\n";
if( argc < 2 )
{
// calibration -w 6 -h 8 -s 2 -n 10 -o camera.yml -op -oe [<list_of_views.txt>]
printf( "This is a camera calibration sample.\n"
"Usage: calibration\n"
" -w <board_width> # the number of inner corners per one of board dimension\n"
" -h <board_height> # the number of inner corners per another board dimension\n"
" [-n <number_of_frames>] # the number of frames to use for calibration\n"
" # (if not specified, it will be set to the number\n"
" # of board views actually available)\n"
" [-di <disk_images> # Number of disk images before triggering undistortion\n"
" [-d <delay>] # a minimum delay in ms between subsequent attempts to capture a next view\n"
" # (used only for video capturing)\n"
" [-s <square_size>] # square size in some user-defined units (1 by default)\n"
" [-o <out_camera_params>] # the output filename for intrinsic [and extrinsic] parameters\n"
" [-op] # write detected feature points\n"
" [-oe] # write extrinsic parameters\n"
" [-zt] # assume zero tangential distortion\n"
" [-a <aspect_ratio>] # fix aspect ratio (fx/fy)\n"
" [-p] # fix the principal point at the center\n"
" [-v] # flip the captured images around the horizontal axis\n"
" [input_data] # input data, one of the following:\n"
" # - text file with a list of the images of the board\n"
" # - name of video file with a video of the board\n"
" # if input_data not specified, a live view from the camera is used\n"
"\n" );
printf( "%s", live_capture_help );
return 0;
}
for( i = 1; i < argc; i++ )
{
const char* s = argv[i];
if( strcmp( s, "-w" ) == 0 )
{
if( sscanf( argv[++i], "%u", &board_size.width ) != 1 || board_size.width <= 0 )
return fprintf( stderr, "Invalid board width\n" ), -1;
}
else if( strcmp( s, "-h" ) == 0 )
{
if( sscanf( argv[++i], "%u", &board_size.height ) != 1 || board_size.height <= 0 )
return fprintf( stderr, "Invalid board height\n" ), -1;
}
else if( strcmp( s, "-s" ) == 0 )
{
if( sscanf( argv[++i], "%f", &square_size ) != 1 || square_size <= 0 )
return fprintf( stderr, "Invalid board square width\n" ), -1;
}
else if( strcmp( s, "-n" ) == 0 )
{
if( sscanf( argv[++i], "%u", &image_count ) != 1 || image_count <= 3 )
return printf("Invalid number of images\n" ), -1;
}
else if( strcmp( s, "-di") == 0)
{
if( sscanf( argv[++i], "%d", &images_from_file) != 1 || images_from_file < 3)
return printf("Invalid di, must be >= 3\n"), -1;
}
else if( strcmp( s, "-a" ) == 0 )
{
if( sscanf( argv[++i], "%f", &aspect_ratio ) != 1 || aspect_ratio <= 0 )
return printf("Invalid aspect ratio\n" ), -1;
}
else if( strcmp( s, "-d" ) == 0 )
{
if( sscanf( argv[++i], "%u", &delay ) != 1 || delay <= 0 )
return printf("Invalid delay\n" ), -1;
}
else if( strcmp( s, "-op" ) == 0 )
{
write_points = 1;
}
else if( strcmp( s, "-oe" ) == 0 )
{
write_extrinsics = 1;
}
else if( strcmp( s, "-zt" ) == 0 )
{
flags |= CV_CALIB_ZERO_TANGENT_DIST;
}
else if( strcmp( s, "-p" ) == 0 )
{
flags |= CV_CALIB_FIX_PRINCIPAL_POINT;
}
else if( strcmp( s, "-v" ) == 0 )
{
flip_vertical = 1;
}
else if( strcmp( s, "-o" ) == 0 )
{
out_filename = argv[++i];
}
else if( s[0] != '-' )
input_filename = s;
else
return fprintf( stderr, "Unknown option %s", s ), -1;
}
if( input_filename )
{
capture = cvCreateFileCapture( input_filename );
if( !capture )
{
f = fopen( input_filename, "rt" );
if( !f )
return fprintf( stderr, "The input file could not be opened\n" ), -1;
image_count = -1;
}
mode = CAPTURING;
}
else
capture = cvCreateCameraCapture(0);
if( !capture && !f )
return fprintf( stderr, "Could not initialize video capture\n" ), -2;
if( capture )
printf( "%s", live_capture_help );
elem_size = board_size.width*board_size.height*sizeof(image_points_buf[0]);
storage = cvCreateMemStorage( MAX( elem_size*4, 1 << 16 ));
image_points_buf = (CvPoint2D32f*)cvAlloc( elem_size );
image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage );
cvNamedWindow( "Image View", 1 );
cvNamedWindow( "Undistort",1);
int disk_image_cnt = 0;
for(;;)
{
IplImage *view = 0, *view_gray = 0;
int count = 0, found, blink = 0;
CvPoint text_origin;
CvSize text_size = {0,0};
int base_line = 0;
char s[100];
int key;
if( f && fgets( imagename, sizeof(imagename)-2, f ))
{
int l = strlen(imagename);
if( l > 0 && imagename[l-1] == '\n' )
imagename[--l] = '\0';
if( l > 0 )
{
if( imagename[0] == '#' )
continue;
view = cvLoadImage( imagename, 1 );
disk_image_cnt++;
}
}
else if( capture )
{
IplImage* view0 = cvQueryFrame( capture );
if( view0 )
{
view = cvCreateImage( cvGetSize(view0), IPL_DEPTH_8U, view0->nChannels );
if( view0->origin == IPL_ORIGIN_BL )
cvFlip( view0, view, 0 );
else
cvCopy( view0, view );
}
}
if( !view || (disk_image_cnt == images_from_file))
{
if( image_points_seq->total > 0 )
{
image_count = image_points_seq->total;
goto calibrate;
}
break;
}
if( flip_vertical )
cvFlip( view, view, 0 );
img_size = cvGetSize(view);
found = cvFindChessboardCorners( view, board_size,
image_points_buf, &count, CV_CALIB_CB_ADAPTIVE_THRESH );
#if 1
// improve the found corners' coordinate accuracy
view_gray = cvCreateImage( cvGetSize(view), 8, 1 );
cvCvtColor( view, view_gray, CV_BGR2GRAY );
cvFindCornerSubPix( view_gray, image_points_buf, count, cvSize(11,11),
cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
cvReleaseImage( &view_gray );
#endif
if( mode == CAPTURING && found && (f || clock() - prev_timestamp > delay*1e-3*CLOCKS_PER_SEC) )
{
cvSeqPush( image_points_seq, image_points_buf );
prev_timestamp = clock();
blink = !f;
#if 1
if( capture )
{
sprintf( imagename, "view%03d.png", image_points_seq->total - 1 );
cvSaveImage( imagename, view );
}
#endif
}
cvDrawChessboardCorners( view, board_size, image_points_buf, count, found );
cvGetTextSize( "100/100", &font, &text_size, &base_line );
text_origin.x = view->width - text_size.width - 10;
text_origin.y = view->height - base_line - 10;
if( mode == CAPTURING )
{
if( image_count > 0 )
sprintf( s, "%d/%d", image_points_seq ? image_points_seq->total : 0, image_count );
else
sprintf( s, "%d/?", image_points_seq ? image_points_seq->total : 0 );
}
else if( mode == CALIBRATED )
sprintf( s, "Calibrated" );
else
sprintf( s, "Press 'g' to start" );
cvPutText( view, s, text_origin, &font, mode != CALIBRATED ?
CV_RGB(255,0,0) : CV_RGB(0,255,0));
if( blink )
cvNot( view, view );
//Rectify or Undistort the image
if( mode == CALIBRATED && undistort_image )
{
IplImage* t = cvCloneImage( view );
cvShowImage("Image View", view);
cvUndistort2( t, view, &camera, &dist_coeffs );
cvReleaseImage( &t );
cvShowImage( "Undistort", view );
cvWaitKey(0);
}
else{
cvShowImage( "Image View", view );
key = cvWaitKey(capture ? 50 : 500);
}
if( key == 27 )
break;
if( key == 'u' && mode == CALIBRATED ){
undistort_image = !undistort_image;
}
if( capture && key == 'g' )
{
mode = CAPTURING;
cvClearMemStorage( storage );
image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage );
}
if( mode == CAPTURING && (unsigned)image_points_seq->total >= (unsigned)image_count )
{
calibrate:
if(disk_image_cnt == images_from_file)
undistort_image = !undistort_image;
cvReleaseMat( &extr_params );
cvReleaseMat( &reproj_errs );
int code = run_calibration( image_points_seq, img_size, board_size,
square_size, aspect_ratio, flags, &camera, &dist_coeffs, &extr_params,
&reproj_errs, &avg_reproj_err );
// save camera parameters in any case, to catch Inf's/NaN's
save_camera_params( out_filename, image_count, img_size,
board_size, square_size, aspect_ratio, flags,
&camera, &dist_coeffs, write_extrinsics ? extr_params : 0,
write_points ? image_points_seq : 0, reproj_errs, avg_reproj_err );
if( code )
mode = CALIBRATED;
else
mode = DETECTION;
}
if( !view )
break;
cvReleaseImage( &view );
}
if( capture )
cvReleaseCapture( &capture );
return 0;
}
RTC::ReturnCode_t ImageCalibration::onExecute(RTC::UniqueId ec_id)
{
board_sz = cvSize(m_board_w, m_board_h);
//Calibrationパターンを計算する。
if (m_inputImageIn.isNew()) {
m_inputImageIn.read();
if(m_keyIn.isNew()){
m_keyIn.read();
key = (int)m_key.data;
}
if(g_temp_w != m_inputImage.width || g_temp_h != m_inputImage.height){
inputImage_buff = cvCreateImage(cvSize(m_inputImage.width, m_inputImage.height), 8, 3);
outputImage_buff = cvCreateImage(cvSize(m_inputImage.width, m_inputImage.height), 8, 3);
tempImage_buff = cvCreateImage(cvSize(m_inputImage.width, m_inputImage.height), 8, 3);
undistortionImage = cvCreateImage(cvSize(m_inputImage.width, m_inputImage.height), 8, 3);
birds_image = cvCreateImage(cvSize(m_inputImage.width, m_inputImage.height), 8, 3);
intrinsicMatrix = cvCreateMat(3,3,CV_64FC1);
distortionCoefficient = cvCreateMat(4,1,CV_64FC1);
captureCount = 0;
findFlag = 0;
mapx = cvCreateImage( cvSize(m_inputImage.width, m_inputImage.height), IPL_DEPTH_32F, 1);
mapy = cvCreateImage( cvSize(m_inputImage.width, m_inputImage.height), IPL_DEPTH_32F, 1);
corners = new CvPoint2D32f[m_board_w * m_board_h];
g_temp_w = m_inputImage.width;
g_temp_h = m_inputImage.height;
}
//Capture開始する。
memcpy(inputImage_buff->imageData,(void *)&(m_inputImage.pixels[0]), m_inputImage.pixels.length());
// tempImage_buff = cvCloneImage(inputImage_buff);
//OutPortに出力する。
int len = inputImage_buff->nChannels * inputImage_buff->width * inputImage_buff->height;
m_origImage.pixels.length(len);
memcpy((void *)&(m_origImage.pixels[0]), inputImage_buff->imageData, len);
m_origImage.width = inputImage_buff->width;
m_origImage.height = inputImage_buff->height;
m_origImageOut.write();
//Capture確認用のWindowの生成
//cvShowImage("Capture", inputImage_buff);
cvWaitKey(1);
//SpaceBarを押すとサンプル映像5枚を撮る
if (key == ' ') {
tempImage_buff = cvCloneImage(inputImage_buff);
//映像を生成する
IplImage *grayImage = cvCreateImage(cvGetSize(tempImage_buff), 8, 1);
//行列の生成
CvMat *worldCoordinates = cvCreateMat((m_board_w * m_board_h) * NUM_OF_BACKGROUND_FRAMES, 3, CV_64FC1); //世界座標用行列
CvMat *imageCoordinates = cvCreateMat((m_board_w * m_board_h) * NUM_OF_BACKGROUND_FRAMES ,2, CV_64FC1); //画像座標用行列
CvMat *pointCounts = cvCreateMat(NUM_OF_BACKGROUND_FRAMES, 1, CV_32SC1); //コーナー数の行列
CvMat *rotationVectors = cvCreateMat(NUM_OF_BACKGROUND_FRAMES, 3, CV_64FC1); //回転ベクトル
CvMat *translationVectors = cvCreateMat(NUM_OF_BACKGROUND_FRAMES, 3, CV_64FC1);
//世界座標を設定する
for (int i = 0; i < NUM_OF_BACKGROUND_FRAMES; i++){
for ( int j = 0; j < (m_board_w * m_board_h); j++) {
cvSetReal2D(worldCoordinates, i * (m_board_w * m_board_h) + j, 0, (j % m_board_w) * UNIT);
cvSetReal2D(worldCoordinates, i * (m_board_w * m_board_h) + j, 1, (j / m_board_w) * UNIT);
cvSetReal2D(worldCoordinates, i * (m_board_w * m_board_h) + j, 2, 0.0);
}
}
//コーナー数を設定
for(int i = 0; i < NUM_OF_BACKGROUND_FRAMES; i++){
cvSetReal2D(pointCounts, i, 0, (m_board_w * m_board_h));
}
//コーナーを検出する。
findFlag = findCorners(tempImage_buff, grayImage, corners);
if (findFlag != 0) {
//コーナーをすべて検出した場合
//映像座標を設定する。
for (;;){
for (int i = 0; i < (m_board_w * m_board_h); i++){
cvSetReal2D(imageCoordinates, captureCount * (m_board_w * m_board_h) + i, 0, corners[i].x);
cvSetReal2D(imageCoordinates, captureCount * (m_board_w * m_board_h) + i, 1, corners[i].y);
}
captureCount++;
printf("%d枚目キャプチャしました\n", captureCount);
if (captureCount == NUM_OF_BACKGROUND_FRAMES) {
//設定した回数チェックパターンを撮った場合
//カメラパラメータを推定する。
cvCalibrateCamera2(
worldCoordinates,
imageCoordinates,
pointCounts,
cvGetSize(inputImage_buff),
intrinsicMatrix,
distortionCoefficient,
rotationVectors,
translationVectors,
CALIBRATE_CAMERA_FLAG
);
//情報をTextとして出力
printf("\nレンズ歪み係数\n");
saveRenseMatrix(distortionCoefficient);
printMatrix("%lf", distortionCoefficient);
//m_renseParameter.data = renseParameters;
printf("\n内部パラメータ\n");
saveInternalParameterMatrix(intrinsicMatrix);
printMatrix("%lf ", intrinsicMatrix);
//m_internalParameter.data = internalParameter;
captureCount = 0;
break;
}
}
}
if (findFlag != 0){
InParameter = 1;
}else if (findFlag == 0) {
InParameter = 0;
}
//メモリ解除
cvReleaseMat(&worldCoordinates);
cvReleaseMat(&imageCoordinates);
cvReleaseMat(&pointCounts);
cvReleaseMat(&rotationVectors);
cvReleaseMat(&translationVectors);
cvReleaseImage(&grayImage);
}
g_temp_w = m_inputImage.width;
g_temp_h = m_inputImage.height;
}
//外部パターンを取得
if (key == ' ' && m_inputImageIn.isNew() && InParameter == 1) {
//行列の生成
CvMat *worldCoordinates = cvCreateMat((m_board_w * m_board_h), 3, CV_64FC1); //世界座標用行列
CvMat *imageCoordinates = cvCreateMat((m_board_w * m_board_h), 2, CV_64FC1); //画像座標用行列
CvMat *rotationVectors = cvCreateMat(1, 3, CV_64FC1); //回転ベクトル
CvMat *rotationMatrix = cvCreateMat(3, 3, CV_64FC1); //回転行列
CvMat *translationVectors = cvCreateMat(1, 3, CV_64FC1);
//世界座標を設定する
for (int i = 0; i < (m_board_w * m_board_h); i++){
cvSetReal2D(worldCoordinates, i, 0, (i % m_board_w) * UNIT);
cvSetReal2D(worldCoordinates, i, 1, (i / m_board_w) * UNIT);
cvSetReal2D(worldCoordinates, i, 2, 0.0);
}
cvWaitKey( 1 );
// スペースキーが押されたら
if ( findFlag != 0 ) {
// コーナーがすべて検出された場合
// 画像座標を設定する
for ( int i = 0; i < (m_board_w * m_board_h); i++ ){
cvSetReal2D( imageCoordinates, i, 0, corners[i].x);
cvSetReal2D( imageCoordinates, i, 1, corners[i].y);
}
// 外部パラメータを推定する
cvFindExtrinsicCameraParams2(
worldCoordinates,
imageCoordinates,
intrinsicMatrix,
distortionCoefficient,
rotationVectors,
translationVectors
);
// 回転ベクトルを回転行列に変換する
cvRodrigues2( rotationVectors, rotationMatrix, NULL );
printf( "\n外部パラメータ\n" );
printExtrinsicMatrix( rotationMatrix, translationVectors );
saveExternalParameterMatrix(rotationMatrix, translationVectors);
m_externalParameter.data = CORBA::string_dup(externalParameter);
m_renseParameter.data = CORBA::string_dup(renseParameters);
m_internalParameter.data = CORBA::string_dup(internalParameter);
}
//メモリを解放
cvReleaseMat( &worldCoordinates );
cvReleaseMat( &imageCoordinates );
cvReleaseMat( &rotationVectors );
cvReleaseMat( &rotationMatrix );
cvReleaseMat( &translationVectors );
//X,Y初期化
cvInitUndistortMap(
intrinsicMatrix,
distortionCoefficient,
mapx,
mapy
);
//外部パラメータ確認フラグ
outParameter = 1;
key = 0;
}
//内部外部パラメータの出力に成功したら
if (InParameter == 1 && outParameter == 1) {
// レンズ歪みを補正した画像を生成する
cvUndistort2(
inputImage_buff,
undistortionImage,
intrinsicMatrix,
distortionCoefficient
);
//cvShowImage("歪み補正", undistortionImage);
//OutPortに補正映像を出力する。
//int len = undistortionImage->nChannels * undistortionImage->width * undistortionImage->height;
//m_calbImage.pixels.length(len);
//歪み補正映像をOutPortとしてメモリコピーする。
//memcpy((void *)&(m_calbImage.pixels[0]), undistortionImage->imageData, len);
//m_calbImageOut.write();
//鳥瞰図の座標設定
objPts[0].x = 0; objPts[0].y = 0;
objPts[1].x = m_board_w-1; objPts[1].y = 0;
objPts[2].x = 0; objPts[2].y = m_board_h-1;
objPts[3].x = m_board_w-1; objPts[3].y = m_board_h-1;
//取得するCornerを設定
imgPts[0] = corners[0];
imgPts[1] = corners[m_board_w - 1];
imgPts[2] = corners[(m_board_h - 1) * m_board_w];
imgPts[3] = corners[(m_board_h - 1) * m_board_w + m_board_w - 1];
//指定したCornerに○を作成する
cvCircle(tempImage_buff, cvPointFrom32f(imgPts[0]), 9, CV_RGB(0,0,255), 3);
cvCircle(tempImage_buff, cvPointFrom32f(imgPts[1]), 9, CV_RGB(0,255,0), 3);
cvCircle(tempImage_buff, cvPointFrom32f(imgPts[2]), 9, CV_RGB(255,0,0), 3);
cvCircle(tempImage_buff, cvPointFrom32f(imgPts[3]), 9, CV_RGB(255,255,0), 3);
CvMat *H = cvCreateMat(3, 3, CV_32F);
cvGetPerspectiveTransform(objPts, imgPts, H);
//高さを設定する。
CV_MAT_ELEM(*H, float, 2, 2) = m_camera_Height;
//Warppingを実行
cvWarpPerspective(inputImage_buff, birds_image, H, CV_INTER_LINEAR | CV_WARP_INVERSE_MAP | CV_WARP_FILL_OUTLIERS);
//鳥瞰図をOutPortに出力する。
int len = birds_image->nChannels * birds_image->width * birds_image->height;
m_birdImage.pixels.length(len);
memcpy((void *)&(m_birdImage.pixels[0]), birds_image->imageData, len);
m_birdImage.width = inputImage_buff->width;
m_birdImage.height = inputImage_buff->height;
m_birdImageOut.write();
cvWaitKey(10);
//cvShowImage("Bird_Eye", birds_image);
cvReleaseMat(&H);
g_temp_w = m_inputImage.width;
g_temp_h = m_inputImage.height;
key = 0;
}
int main()
{
if(run_tests_only)
{
MyLine3D::runTest();
return 0;
}
//CvMat *camera_inner_calibration_matrix;
bool show_surf_example=false;
bool show_calibration_from_camera_and_undistortion=false;
if(show_calibration_from_camera_and_undistortion)
{
CvMat *object_points_all=0;
CvMat *image_points_all=0;
CvMat *points_count_all=0;
CvMat *camera_matr=0;
CvMat *distor_coefs=0;
CvMat *rotation_vecs=0;
CvMat *transpose_vecs=0;
vector<CvPoint2D32f> qu_calibr_points;
IplImage* frameCam1;
cvNamedWindow("WindowCam1",CV_WINDOW_KEEPRATIO);
CvCapture *captureCam1=cvCreateCameraCapture(0);
IplImage *quarterFrame;
CvPoint2D32f *cornersFounded= new CvPoint2D32f[100];
int cornersCount=0;
int result_Found=0;
// getting snapshots for inner camera calibration from video camera
bool capture_flag=false;
while(true)
{
frameCam1=cvQueryFrame(captureCam1);
quarterFrame=cvCreateImage(cvSize((frameCam1->width),(frameCam1->height)),IPL_DEPTH_8U,3);
cvCopy(frameCam1,quarterFrame);
if(capture_flag)
{
result_Found=cvFindChessboardCorners(quarterFrame,cvSize(chess_b_szW,chess_b_szH),cornersFounded,&cornersCount);//,CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS |CV_CALIB_CB_FAST_CHECK);
cvDrawChessboardCorners(quarterFrame,cvSize(chess_b_szW,chess_b_szH),cornersFounded,cornersCount,result_Found);
if(result_Found>0)
AddPointsToInnerCalibrate(qu_calibr_points,cornersFounded,cornersCount);
capture_flag=false;
cvShowImage("WindowCam1",quarterFrame);
if(result_Found>0)
cvWaitKey(0);
}
char c=cvWaitKey(33);
if(c==27)
break;
if(c==32 || c=='y' || c=='Y')
capture_flag=true;
cvShowImage("WindowCam1",quarterFrame);
cvReleaseImage(&quarterFrame);
}
cvReleaseImage(&quarterFrame);
cvReleaseCapture(&captureCam1);
cvDestroyWindow("WindowCam1");
PrintAllPointsForInnerCalibrate(qu_calibr_points,chess_b_szW*chess_b_szH);
InitCvMatPointsParametersForInnerCallibration_part1(qu_calibr_points,chess_b_szW*chess_b_szH,object_points_all,image_points_all,points_count_all,chess_b_szW,chess_b_szH);
InitOtherCameraParametersForInnerCallibration_part2(qu_calibr_points.size()/(chess_b_szW*chess_b_szH),camera_matr,distor_coefs,rotation_vecs,transpose_vecs);
double calibration_error_result=cvCalibrateCamera2(object_points_all,
image_points_all,
points_count_all,
cvSize(imgW,imgH),
camera_matr,
distor_coefs,
rotation_vecs,
transpose_vecs,
CV_CALIB_FIX_PRINCIPAL_POINT|CV_CALIB_FIX_ASPECT_RATIO|CV_CALIB_ZERO_TANGENT_DIST
);
WriteMatrixCoef(camera_matr);
WriteMatrixCoef(distor_coefs);
//camera_inner_calibration_matrix=cvCreateMat(3,3,CV_32FC1);
//cvCopy(camera_matr,camera_inner_calibration_matrix);
cvSave("camera_calibration_inner.txt",camera_matr,"camera_inner_calibration_matrix");
cvSave("camera_calibration_dist.txt",distor_coefs,"distor_coefs","coeficients of distortions");
cout<<"Total Error:"<<calibration_error_result<<endl;
cout<<"Average Calibration Error :"<<(calibration_error_result)/qu_calibr_points.size()<<endl;
//undistortion example
IplImage *frame_cur;
IplImage *undistor_image;
cvNamedWindow("cameraUndistor",CV_WINDOW_KEEPRATIO);
CvCapture *captureCam2=cvCreateCameraCapture(0);
bool undist_flag=false;
while(true)
{
frame_cur= cvQueryFrame(captureCam2);
undistor_image=cvCreateImage(cvSize((frame_cur->width),(frame_cur->height)),IPL_DEPTH_8U,3);
if(undist_flag)
{
cvUndistort2(frame_cur,undistor_image,camera_matr,distor_coefs);
}
else
{
cvCopy(frame_cur,undistor_image);
}
cvShowImage("cameraUndistor",undistor_image);
char c=cvWaitKey(33);
if(c==27)
break;
if(c=='u'||c=='U')
undist_flag=!undist_flag;
cvReleaseImage(&undistor_image);
}
cvReleaseImage(&undistor_image);
cvReleaseCapture(&captureCam2);
cvDestroyWindow("cameraUndistor");
}//ending undistortion_example
if(show_surf_example)
{
//using SURF
initModule_nonfree();// added at 16.04.2013
CvCapture* capture_cam_3=cvCreateCameraCapture(0);
cvNamedWindow("SURF from Cam",CV_WINDOW_KEEPRATIO);
cvCreateTrackbar("Hessian Level","SURF from Cam",0,1000,onTrackbarSlide1);
IplImage* buf_frame_3=0;
IplImage* gray_copy=0;
IplImage* buf_frame_3_copy=0;
CvSeq *kp1,*descr1;
CvMemStorage *storage=cvCreateMemStorage(0);
CvSURFPoint *surf_pt;
bool surf_flag=false;
while(true)
{
buf_frame_3=cvQueryFrame(capture_cam_3);
if(surf_flag)
{
surf_flag=false;
gray_copy=cvCreateImage(cvSize((buf_frame_3->width),(buf_frame_3->height)),IPL_DEPTH_8U,1);
buf_frame_3_copy=cvCreateImage(cvSize((buf_frame_3->width),(buf_frame_3->height)),IPL_DEPTH_8U,3);
cvCvtColor(buf_frame_3,gray_copy,CV_RGB2GRAY);
//cvSetImageROI(gray_copy,cvRect(280,200,40,40));
cvExtractSURF(gray_copy,NULL,&kp1,&descr1,storage,cvSURFParams(0.0,0));
cvReleaseImage(&gray_copy);
re_draw=true;
while(true)
{
if(re_draw)
{
cvCopy(buf_frame_3,buf_frame_3_copy);
double pi=acos(-1.0);
for(int i=0;i<kp1->total;i++)
{
surf_pt=(CvSURFPoint*)cvGetSeqElem(kp1,i);
if(surf_pt->hessian<min_hessian)
continue;
int pt_x,pt_y;
pt_x=(int)(surf_pt->pt.x);
pt_y=(int)(surf_pt->pt.y);
int sz=surf_pt->size;
int rad_angle=(surf_pt->dir*pi)/180;
cvCircle(buf_frame_3_copy,cvPoint(pt_x,pt_y),1/*sz*/,CV_RGB(0,255,0));
cvLine(buf_frame_3_copy,cvPoint(pt_x,pt_y),cvPoint(pt_x+sz*cosl(rad_angle),pt_y-sz*sinl(rad_angle)),CV_RGB(0,0,255));
}
cvShowImage("SURF from Cam",buf_frame_3_copy);
}
char c=cvWaitKey(33);
if(c==27)
{
break;
}
}
cvReleaseImage(&buf_frame_3_copy);
}
cvShowImage("SURF from Cam",buf_frame_3);
char ch=cvWaitKey(33);
if(ch==27)
break;
if(ch==32)
surf_flag=true;
}
if(gray_copy!=0)
cvReleaseImage(&gray_copy);
cvReleaseCapture(&capture_cam_3);
cvDestroyWindow("SURF from Cam");
}//ending SURF_example
CvFont my_font=cvFont(1,1);
cvInitFont(&my_font,CV_FONT_HERSHEY_SIMPLEX,1.0,1.0);
cvNamedWindow("twoSnapshots",CV_WINDOW_KEEPRATIO);
cvCreateTrackbar("Select LLine","twoSnapshots",0,1000,onTrackbarSlideSelectLine);
CvCapture *capture_4 = 0;
IplImage* left_img=0;
IplImage* right_img=0;
IplImage* cur_frame_buf=0;
IplImage* gray_img_left=0;
IplImage* gray_img_right=0;
IplImage* merged_images=0;
IplImage* merged_images_copy=0;
CvMat *fundamentalMatrix = 0;
vector<KeyPoint> key_points_left;
Mat descriptors_left;
vector<KeyPoint> key_points_right;
Mat descriptors_right;
//CvMemStorage *mem_stor=cvCreateMemStorage(0);*/
float min_hessian_value=1001.0f;
double startValueOfFocus = 350;
char* left_image_file_path = "camera_picture_left.png";
char* right_image_file_path = "camera_picture_right.png";
Array left_points, right_points;
left_points.init(1,1);
right_points.init(1,1);
Array forReconstructionLeftPoints, forReconstructionRightPoints;
forReconstructionLeftPoints.init(1,1);
forReconstructionRightPoints.init(1,1);
while(true)
{
char ch=cvWaitKey(33);
if(ch==27)
break;
// open left and right images
if(ch == 'o' || ch == 'O')
{
openTwoImages(left_image_file_path, right_image_file_path, left_img, right_img );
MergeTwoImages(left_img,right_img,merged_images);
}
// save both left and right images from camera
if(ch == 's' || ch == 'S')
{
if( left_img != 0 )
cvSaveImage(left_image_file_path, left_img);
if( right_img != 0)
cvSaveImage(right_image_file_path, right_img);
}
if(ch=='l'||ch=='L')
{
if(capture_4 == 0)
{
capture_4=cvCreateCameraCapture(0);
}
cur_frame_buf=cvQueryFrame(capture_4);
if(left_img==0)
left_img=cvCreateImage(cvSize(cur_frame_buf->width,cur_frame_buf->height),IPL_DEPTH_8U,3);
cvCopy(cur_frame_buf,left_img);
if(right_img == 0)
{
right_img=cvCreateImage(cvSize(cur_frame_buf->width,cur_frame_buf->height),IPL_DEPTH_8U,3);
cvCopy(cur_frame_buf,right_img);
}
MergeTwoImages(left_img,right_img,merged_images);
}
if(ch=='r'||ch=='R')
{
if(capture_4 == 0)
{
capture_4=cvCreateCameraCapture(0);
}
cur_frame_buf=cvQueryFrame(capture_4);
if(right_img==0)
right_img=cvCreateImage(cvSize(cur_frame_buf->width,cur_frame_buf->height),IPL_DEPTH_8U,3);
cvCopy(cur_frame_buf,right_img);
if(left_img == 0)
{
left_img=cvCreateImage(cvSize(cur_frame_buf->width,cur_frame_buf->height),IPL_DEPTH_8U,3);
cvCopy(cur_frame_buf,left_img);
}
MergeTwoImages(left_img,right_img,merged_images);
}
if(ch=='b'||ch=='B')
{
if(capture_4 == 0)
{
capture_4=cvCreateCameraCapture(0);
}
cur_frame_buf=cvQueryFrame(capture_4);
cvCopy(cur_frame_buf,left_img);
cvCopy(cur_frame_buf,right_img);
}
if(ch=='q'||ch=='Q' && left_img!=0)
{
//proceed left
extractFeaturesFromImage(left_img, min_hessian_value, gray_img_left, key_points_left, descriptors_left);
}
if(ch=='w'||ch=='W' && right_img!=0)
{
//proceed right
extractFeaturesFromImage(right_img, min_hessian_value, gray_img_right, key_points_right, descriptors_right);
}
if(ch=='m'||ch=='M' && left_img!=0 && right_img!=0)
{
//merge two images in to bigger one
MergeTwoImages(left_img,right_img,merged_images);
}
if(ch=='c'||ch=='C' && merged_images!=0)
{
//comparison of two images
if(fundamentalMatrix != 0)
{
cvReleaseMat(& fundamentalMatrix);
fundamentalMatrix = 0;
}
left_to_right_corresponding_points.clear();
right_to_left_corresponding_points.clear();
GetCorrespondingPointsForSURF(key_points_left,descriptors_left,key_points_right,descriptors_right,left_to_right_corresponding_points,right_to_left_corresponding_points);
}
if(ch == 'E' || ch == 'e')
{
//drawing lines for corresponding points
KeyPoint *leftPoint,*rightPoint,*leftPoint2,*rightPoint2;
int width_part=merged_images->width>>1;
/*for(int iL=0;iL<left_to_right_corresponding_points.size();iL++)
{
leftPoint=(CvSURFPoint*)cvGetSeqElem(key_points_left,left_to_right_corresponding_points[iL].first);
rightPoint=(CvSURFPoint*)cvGetSeqElem(key_points_right,left_to_right_corresponding_points[iL].second);
cvLine(merged_images,cvPoint(leftPoint->pt.x,leftPoint->pt.y),cvPoint(rightPoint->pt.x+width_part,rightPoint->pt.y),CV_RGB(255,0,0));
}*/
int sizeOfAccepptedLeftToRightCorrespondings = left_to_right_corresponding_points.size();
bool* acceptedLeftToRightCorrespondings = 0;
getAcceptedCorrespondingsForFindingModelParameters(left_to_right_corresponding_points,
key_points_left,
key_points_right,
fundamentalMatrix,
acceptedLeftToRightCorrespondings,
sizeOfAccepptedLeftToRightCorrespondings);
while(true)
{
merged_images_copy=cvCreateImage(cvSize(merged_images->width,merged_images->height),merged_images->depth,3);
cvCopy(merged_images,merged_images_copy);
int iL=selectedLeftLine;
int iR=iL;
if(iL>=left_to_right_corresponding_points.size())
iL=left_to_right_corresponding_points.size()-1;
if(iR>=right_to_left_corresponding_points.size())
iR=right_to_left_corresponding_points.size()-1;
char str[100]={0};
if(iL >= 0 )
{
bool isLeftToRightLineIsAccepted = acceptedLeftToRightCorrespondings[iL];
// difference value
sprintf(str,"%f",left_to_right_corresponding_points[iL].comparer_value);
cvPutText(merged_images_copy,str,cvPoint(0,merged_images_copy->height-40),&my_font,CV_RGB(0,255,0));
// count of Matches
sprintf(str,"%d",left_to_right_corresponding_points[iL].counterOfMatches);
cvPutText(merged_images_copy,str,cvPoint(200,merged_images_copy->height-40),&my_font,CV_RGB(255,255,0));
// median of compared values
sprintf(str,"%lf",left_to_right_corresponding_points[iL].medianOfComparedMatches);
cvPutText(merged_images_copy,str,cvPoint(250,merged_images_copy->height-40),&my_font,CV_RGB(255,0,0));
// Variance of compared values
sprintf(str,"V=%lf",left_to_right_corresponding_points[iL].Variance());
cvPutText(merged_images_copy,str,cvPoint(0,merged_images_copy->height-80),&my_font,CV_RGB(0,255,0));
// Standard deviation of compared values
sprintf(str,"SD=%lf",sqrt( left_to_right_corresponding_points[iL].Variance() ));
cvPutText(merged_images_copy,str,cvPoint(250,merged_images_copy->height-80),&my_font,CV_RGB(0,255,0));
double SD = sqrt( left_to_right_corresponding_points[iL].Variance() ) ;
double median = left_to_right_corresponding_points[iL].medianOfComparedMatches;
double compValue = left_to_right_corresponding_points[iL].comparer_value;
double mark_1_5 = median - 1.5 * SD - compValue;
// Mark 1.5
sprintf(str,"m1.5=%lf", mark_1_5);
cvPutText(merged_images_copy,str,cvPoint(0,merged_images_copy->height-120),&my_font,CV_RGB(0,255,0));
sprintf(str,"angle=%lf", left_to_right_corresponding_points[iL].degreesBetweenDeltaVector);
cvPutText(merged_images_copy,str,cvPoint(0,merged_images_copy->height-150),&my_font,CV_RGB(0,255,0));
leftPoint= &(key_points_left[ left_to_right_corresponding_points[iL].comp_pair.first ]);
rightPoint=&(key_points_right[ left_to_right_corresponding_points[iL].comp_pair.second ]);
cvLine(merged_images_copy,cvPoint(leftPoint->pt.x,leftPoint->pt.y),cvPoint(rightPoint->pt.x+width_part,rightPoint->pt.y),CV_RGB(0,255,0));
drawEpipolarLinesOnLeftAndRightImages(merged_images_copy, cvPoint(leftPoint->pt.x,leftPoint->pt.y),
cvPoint(rightPoint->pt.x,rightPoint->pt.y), fundamentalMatrix);
CvScalar color = CV_RGB(255, 0, 0);
if(isLeftToRightLineIsAccepted)
{
color = CV_RGB(0,255,0);
}
cvCircle(merged_images_copy, cvPoint(leftPoint->pt.x,leftPoint->pt.y), 5, color);
cvCircle(merged_images_copy, cvPoint(rightPoint->pt.x+width_part,rightPoint->pt.y), 5, color);
}
//cvLine(merged_images_copy,cvPoint(leftPoint->pt.x,leftPoint->pt.y),cvPoint(rightPoint->pt.x,rightPoint->pt.y),CV_RGB(255,0,255));
if(iR >= 0 )
{
sprintf(str,"%f",right_to_left_corresponding_points[iR].comparer_value);
cvPutText(merged_images_copy,str,cvPoint(width_part,merged_images_copy->height-40),&my_font,CV_RGB(255,0,0));
rightPoint2= &(key_points_right [right_to_left_corresponding_points[iR].comp_pair.first]);
leftPoint2= &(key_points_left [right_to_left_corresponding_points[iR].comp_pair.second]);
cvLine(merged_images_copy,cvPoint(leftPoint2->pt.x,leftPoint2->pt.y),cvPoint(rightPoint2->pt.x+width_part,rightPoint2->pt.y),CV_RGB(255,0,0));
}
//cvLine(merged_images_copy,cvPoint(leftPoint2->pt.x+width_part,leftPoint2->pt.y),cvPoint(rightPoint2->pt.x+width_part,rightPoint2->pt.y),CV_RGB(255,0,255));
cvShowImage("twoSnapshots",merged_images_copy);
cvReleaseImage(&merged_images_copy);
char ch2=cvWaitKey(33);
if(ch2==27)
break;
if(ch2=='z' && selectedLeftLine>0)
{
selectedLeftLine--;
}
if(ch2=='x' && selectedLeftLine<1000)
{
selectedLeftLine++;
}
if( ch2 == 'a' || ch2 == 'A')
{
acceptedLeftToRightCorrespondings[selectedLeftLine] = true;
}
if( ch2 == 'd' || ch2 == 'D')
{
acceptedLeftToRightCorrespondings[selectedLeftLine] = false;
}
}//end of while(true)
SaveAcceptedCorresspondings(
left_to_right_corresponding_points,
right_to_left_corresponding_points,
key_points_left,
key_points_right,
acceptedLeftToRightCorrespondings,
sizeOfAccepptedLeftToRightCorrespondings
);
ConvertAcceptedCorresspondingsToMyArray(left_to_right_corresponding_points,
right_to_left_corresponding_points,
key_points_left,
key_points_right,
acceptedLeftToRightCorrespondings,
sizeOfAccepptedLeftToRightCorrespondings,
left_points,
right_points
);
delete[] acceptedLeftToRightCorrespondings;
}
if( ch == 'T' || ch == 't')
{
clock_t startTime = clock();
openTwoImages(left_image_file_path, right_image_file_path, left_img, right_img );
// proceed left
extractFeaturesFromImage(left_img, min_hessian_value, gray_img_left, key_points_left, descriptors_left);
//proceed right
extractFeaturesFromImage(right_img, min_hessian_value, gray_img_right, key_points_right, descriptors_right);
//comparison of two images
if(fundamentalMatrix != 0)
{
cvReleaseMat(& fundamentalMatrix);
fundamentalMatrix = 0;
}
left_to_right_corresponding_points.clear();
right_to_left_corresponding_points.clear();
GetCorrespondingPointsForSURF(key_points_left,descriptors_left,key_points_right,descriptors_right,left_to_right_corresponding_points,right_to_left_corresponding_points);
// searching fundamental matrix and corresponding points
findFundamentalMatrixAndCorrespondingPointsForReconstruction(
left_to_right_corresponding_points,
right_to_left_corresponding_points,
fundamentalMatrix,
key_points_left,
key_points_right,
descriptors_left,
descriptors_right,
left_img,
right_img,
gray_img_left,
gray_img_right,
forReconstructionLeftPoints,
forReconstructionRightPoints,
min_hessian_value, 450);
// selecting points for finding model parameters
int sizeOfAccepptedLeftToRightCorrespondings = left_to_right_corresponding_points.size();
bool* acceptedLeftToRightCorrespondings = 0;
getAcceptedCorrespondingsForFindingModelParameters(left_to_right_corresponding_points,
key_points_left,
key_points_right,
fundamentalMatrix,
acceptedLeftToRightCorrespondings,
sizeOfAccepptedLeftToRightCorrespondings);
ConvertAcceptedCorresspondingsToMyArray(left_to_right_corresponding_points,
right_to_left_corresponding_points,
key_points_left,
key_points_right,
acceptedLeftToRightCorrespondings,
sizeOfAccepptedLeftToRightCorrespondings,
left_points,
right_points
);
delete[] acceptedLeftToRightCorrespondings;
// start process of determination parameters of model and reconstruction of scene
cv::Mat mat_left_img(left_img, true);
cv::Mat mat_right_img(right_img, true);
mainLevenbergMarkvardt_LMFIT(startValueOfFocus, "currentPLYExportFile", left_points, right_points,
mat_left_img, mat_right_img,
forReconstructionLeftPoints, forReconstructionRightPoints);
mat_left_img.release();
mat_right_img.release();
cout << "Code execution time: "<< double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " seconds." << endl;
}
if( ch == 'I' || ch == 'i')
{
//-- Step 3: Matching descriptor vectors using FLANN matcher
FlannBasedMatcher matcher;
std::vector< DMatch > matches;
matcher.match( descriptors_left, descriptors_right, matches );
//double max_dist = 0; double min_dist = 100;
////-- Quick calculation of max and min distances between keypoints
//for( int i = 0; i < descriptors_left.rows; i++ )
//{ double dist = matches[i].distance;
// if( dist < min_dist ) min_dist = dist;
// if( dist > max_dist ) max_dist = dist;
//}
//printf("-- Max dist : %f \n", max_dist );
//printf("-- Min dist : %f \n", min_dist );
//-- Draw only "good" matches (i.e. whose distance is less than 2*min_dist,
//-- or a small arbitary value ( 0.02 ) in the event that min_dist is very
//-- small)
//-- PS.- radiusMatch can also be used here.
//std::vector< DMatch > good_matches;
left_to_right_corresponding_points.clear();
right_to_left_corresponding_points.clear();
for( int i = 0; i < descriptors_left.rows; i++ )
{
//if( matches[i].distance <= max(2*min_dist, 0.02) )
{
//good_matches.push_back( matches[i]);
left_to_right_corresponding_points.push_back( ComparedIndexes(matches[i].distance, pair<int, int> (i, matches[i].trainIdx)) );
}
}
cout<< "Count of good matches :" << left_to_right_corresponding_points.size() << endl;
stable_sort(left_to_right_corresponding_points.begin(),left_to_right_corresponding_points.end(),my_comparator_for_stable_sort);
}
//if( ch == 'K' || ch == 'k')
//{
// CvSURFPoint *leftPoint;
// //proceed left
// gray_img_left=cvCreateImage(cvSize((left_img->width),(left_img->height)),IPL_DEPTH_8U,1);
// cvCvtColor(left_img,gray_img_left,CV_RGB2GRAY);
// cvExtractSURF(gray_img_left,NULL,&key_points_left,&descriptors_left,mem_stor,cvSURFParams(min_hessian_value,0));
// cv::Mat mat_gray_leftImage(gray_img_left, true);
// cvReleaseImage(&gray_img_left);
// // proceed right
// gray_img_right=cvCreateImage(cvSize((right_img->width),(right_img->height)),IPL_DEPTH_8U,1);
// cvCvtColor(right_img,gray_img_right,CV_RGB2GRAY);
// cv::Mat mat_gray_rightImage(gray_img_right, true);
// cvReleaseImage(&gray_img_right);
// vector<Point2f> LK_left_points;
// vector<Point2f> LK_right_points;
// LK_right_points.resize(key_points_left->total);
// for( int i = 0; i < key_points_left->total; i++)
// {
// leftPoint=(CvSURFPoint*)cvGetSeqElem(key_points_left, i);
// LK_left_points.push_back(Point2f( leftPoint->pt.x, leftPoint->pt.y));
// }
//
// vector<uchar> status;
// vector<float> err;
// cv::calcOpticalFlowPyrLK(
// mat_gray_leftImage,
// mat_gray_rightImage,
// LK_left_points,
// LK_right_points,
// status,
// err);
// int width_part=merged_images->width>>1;
//
// float minErr = err[0];
// for(int k = 0; k < err.size(); k++)
// {
// if(status[k] && err[k] < minErr)
// {
// minErr = err[k];
// }
// }
// cout<< "Lucass Kanade min error: " << minErr<< endl;
// int i = 0;
// merged_images_copy=cvCreateImage(cvSize(merged_images->width,merged_images->height),merged_images->depth,3);
// cvCopy(merged_images,merged_images_copy);
// for(; i < LK_left_points.size(); ++i)
// {
// if(err[i] < 5 * minErr && status[i])
// {
// cvLine(merged_images_copy,cvPoint(LK_left_points[i].x,LK_left_points[i].y),cvPoint(LK_right_points[i].x+width_part,LK_right_points[i].y),
// CV_RGB(100 + (( i *3) % 155), 100+ ((i*7)%155), 100+ ((i*13)%155)));
// }
// }
// cvShowImage("twoSnapshots",merged_images_copy);
//
// while(true)
// {
// char ch2=cvWaitKey(33);
// if(ch2==27)
// break;
//
// }
//
// cvReleaseImage(&merged_images_copy);
// status.clear();
// err.clear();
// LK_left_points.clear();
// LK_right_points.clear();
// mat_gray_leftImage.release();
// mat_gray_rightImage.release();
//}
if( ch == 'F' || ch == 'f')
{
findFundamentalMatrixAndCorrespondingPointsForReconstruction(
left_to_right_corresponding_points,
right_to_left_corresponding_points,
fundamentalMatrix,
key_points_left,
key_points_right,
descriptors_left,
descriptors_right,
left_img,
right_img,
gray_img_left,
gray_img_right,
forReconstructionLeftPoints,
forReconstructionRightPoints,
min_hessian_value);
}
if( ch == 'P' || ch == 'p')
{
cv::Mat mat_left_img(left_img, true);
cv::Mat mat_right_img(right_img, true);
mainLevenbergMarkvardt_LMFIT(startValueOfFocus, "currentPLYExportFile", left_points, right_points,
mat_left_img, mat_right_img,
forReconstructionLeftPoints, forReconstructionRightPoints);
mat_left_img.release();
mat_right_img.release();
}
if(merged_images!=0)
{
cvShowImage("twoSnapshots",merged_images);
}
}
IplImage* callback(IplImage* image)
{
IplImage* Show1 = cvCreateImage( cvSize(640,480), IPL_DEPTH_8U, 1);
IplImage* Show2 = cvCreateImage( cvSize(640,480), IPL_DEPTH_8U, 1);
IplImage* ImageC1 = cvCreateImage( cvSize(640,480), IPL_DEPTH_8U, 1);
//转换为灰度图
cvCvtColor( image, ImageC1, CV_RGB2GRAY);
// cvFlip( ImageC1, NULL, 0);
double *mi;
double *md;
mi = new double[3*3];
md = new double[4];
CvMat intrinsic_matrix,distortion_coeffs;
//摄像机内参数
cvInitMatHeader(&intrinsic_matrix,3,3,CV_64FC1,mi);
//镜头畸变参数
cvInitMatHeader(&distortion_coeffs,1,4,CV_64FC1,md);
/////////////////////////////////////////////////
double fc1,fc2,cc1,cc2,kc1,kc2,kc3,kc4;
/*fc1 = 636.796592;
fc2 = 639.002846;
cc1 = 320.575856;
cc2 = 238.909624;
kc1 = 0.008447;
kc2 = -0.072905;
kc3 = -0.001818;
kc4 = -0.000562; */
fc1 = 426.331624;
fc2 = 426.556478;
cc1 = 365.956413;
cc2 = 202.451300;
kc1 = -0.422263;
kc2 = 0.182853;
kc3 = -0.007801;
kc4 = 0.002509;
cvmSet(&intrinsic_matrix, 0, 0, fc1);
cvmSet(&intrinsic_matrix, 0, 1, 0);
cvmSet(&intrinsic_matrix, 0, 2, cc1);
cvmSet(&intrinsic_matrix, 1, 0, 0);
cvmSet(&intrinsic_matrix, 1, 1, fc2);
cvmSet(&intrinsic_matrix, 1, 2, cc2);
cvmSet(&intrinsic_matrix, 2, 0, 0);
cvmSet(&intrinsic_matrix, 2, 1, 0);
cvmSet(&intrinsic_matrix, 2, 2, 1);
cvmSet(&distortion_coeffs, 0, 0, kc1);
cvmSet(&distortion_coeffs, 0, 1, kc2);
cvmSet(&distortion_coeffs, 0, 2, kc3);
cvmSet(&distortion_coeffs, 0, 3, kc4);
/////////////////////////////////////////////////
//下面有两种矫正方法，第一种用opencv库，第二种自定义方法
//矫正畸变(opencv)
cvUndistort2( ImageC1, Show1, &intrinsic_matrix, &distortion_coeffs);
//矫正畸变
for (int nx=0; nx<640; nx++)
{
for (int ny=0; ny<480; ny++)
{
// double x=nx-50;
// double y=ny-50;
double x=nx;
double y=ny;
double xx=(x-cc1)/fc1;
double yy=(y-cc2)/fc2;
double r2=pow(xx,2)+pow(yy,2);
double r4=pow(r2,2);
double xxx=xx*(1+kc1*r2+kc2*r4)+2*kc3*xx*yy+kc4*(r2+2*xx*xx);
double yyy=yy*(1+kc1*r2+kc2*r4)+2*kc4*xx*yy+kc3*(r2+2*yy*yy);
double xxxx = xxx*fc1+cc1;
double yyyy = yyy*fc2+cc2;
if (xxxx>0 && xxxx<640 && yyyy>0 && yyyy<480)
{
_I(Show2,nx,ny) = (int)_IF(ImageC1,xxxx,yyyy);
}
else
{
_I(Show2,nx,ny) = 0;
}
}
}
//显示
// cvShowImage("径向矫正1", Show1);
// cvShowImage("径向矫正2", Show2);
// cvWaitKey(1);
cvReleaseImage( &Show1 );
// cvReleaseImage( &Show2 );
cvReleaseImage( &ImageC1 );
return Show2; //选了第二种方法
}
void process_image()
{
// std::cout << "Checking publish count: " << image_in->publish_count << std::endl;
// image_in->lock_atom();
if (image_in->publish_count > 0) {
cvSetData(cvimage_in, codec_in->get_raster(), 3*704);
cvConvertImage(cvimage_in, cvimage_bgr, CV_CVTIMG_SWAP_RB);
// image_in->unlock_atom();
CvSize board_sz = cvSize(12, 12);
CvPoint2D32f* corners = new CvPoint2D32f[12*12];
int corner_count = 0;
//This function has a memory leak in the current version of opencv!
int found = cvFindChessboardCorners(cvimage_bgr, board_sz, corners, &corner_count,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
IplImage* gray = cvCreateImage(cvSize(cvimage_bgr->width, cvimage_bgr->height), IPL_DEPTH_8U, 1);
cvCvtColor(cvimage_bgr, gray, CV_BGR2GRAY);
cvFindCornerSubPix(gray, corners, corner_count,
cvSize(5, 5), cvSize(-1, -1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 10, 0.01f ));
cvReleaseImage(&gray);
if (take_pic && corner_count == 144) {
std::stringstream ss;
img_cnt++;
ss << dir_name << "/Image" << img_cnt << ".jpg";
// std::ofstream imgfile(ss.str().c_str());
// imgfile.write((char*)image_in->jpeg_buffer, image_in->compressed_size);
// imgfile.close();
cvSaveImage(ss.str().c_str(), cvimage_bgr);
ss.str("");
ss << dir_name << "/Position" << img_cnt << ".txt";
std::ofstream posfile(ss.str().c_str());
observe->lock_atom();
posfile << "P: " << observe->pan_val << std::endl
<< "T: " << observe->tilt_val << std::endl
<< "Z: " << observe->lens_zoom_val << std::endl
<< "F: " << observe->lens_focus_val;
observe->unlock_atom();
posfile.close();
take_pic = false;
}
float maxdiff = 0;
for(int c=0; c<12*12; c++) {
float diff = sqrt( pow(corners[c].x - last_corners[c].x, 2.0) +
pow(corners[c].y - last_corners[c].y, 2.0));
last_corners[c].x = corners[c].x;
last_corners[c].y = corners[c].y;
if (diff > maxdiff) {
maxdiff = diff;
}
}
printf("Max diff: %g\n", maxdiff);
cvDrawChessboardCorners(cvimage_bgr, board_sz, corners, corner_count, found);
if (undistort) {
cvUndistort2(cvimage_bgr, cvimage_undistort, intrinsic_matrix, distortion_coeffs);
} else {
cvCopy(cvimage_bgr, cvimage_undistort);
}
CvFont font;
cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.8, 0.8, 0, 2);
std::stringstream ss;
observe->lock_atom();
ss << "P: " << observe->pan_val;
ss << " T: " << observe->tilt_val;
ss << " Z: " << observe->lens_zoom_val;
ss << " F: " << observe->lens_focus_val;
observe->unlock_atom();
cvPutText(cvimage_undistort, ss.str().c_str(), cvPoint(15,30), &font, CV_RGB(255,0,0));
ss.str("");
ss << "Found " << corner_count << " corners";
if (centering) {
ss << " -- Autocentering";
}
cvPutText(cvimage_undistort, ss.str().c_str(), cvPoint(15,60), &font, CV_RGB(255,0,0));
image_out->width = 704;
image_out->height = 480;
image_out->compression = "raw";
image_out->colorspace = "rgb24";
// codec_out->realloc_raster_if_needed();
cvSetData(cvimage_out, codec_out->get_raster(), 3*image_out->width);
cvConvertImage(cvimage_undistort, cvimage_out, CV_CVTIMG_SWAP_RB);
codec_out->set_flow_data();
image_out->publish();
CvPoint2D32f COM = cvPoint2D32f(0,0);
if (centering && corner_count > 20) {
//average corners:
for (int i = 0; i < corner_count; i++) {
COM.x += corners[i].x / corner_count;
COM.y += corners[i].y / corner_count;
}
if ( (fabs(COM.x - 354.0) > 10) || (fabs(COM.y - 240.0) > 10) ) {
float rel_pan,rel_tilt;
rel_pan = (COM.x - 354.0) * .001;
rel_tilt = -(COM.y - 240.0) * .001;
control->pan_val = rel_pan;
control->pan_rel = true;
control->pan_valid = true;
control->tilt_val = rel_tilt;
control->tilt_rel = true;
control->tilt_valid = true;
control->publish();
}
}
delete[] corners;
} else {
// image_in->unlock_atom();
}
}
