void VideoHandle::prehandleImage(Size size) { //undistort { Mat temp = frame.clone(); undistort(temp, frame, distortmtx, distortdist); } //resize resize(frame, frame_resize, size); //2HSV cvtColor(frame_resize, frame_resize_hsv, COLOR_BGR2HSV); //threshold // thresholdlow = (Mat_<uchar>(1,3) << select_color_hsv[0]-20, 90, 30); //no human-light -> 20(unstable); with human-light -> 40 // thresholdhigh = (Mat_<uchar>(1,3) << select_color_hsv[0]+20, 255, 255); thresholdlow = (Mat_<uchar>(1,3) << 115-20, 90, 30); //no human-light -> 20(unstable); with human-light -> 40 thresholdhigh = (Mat_<uchar>(1,3) << 115+20, 255, 255); inRange(frame_resize_hsv, thresholdlow, thresholdhigh, mask); //morphlogy { Mat temp = mask.clone(); morphologyEx(temp, mask, MORPH_OPEN, Mat::ones(3, 3, CV_8U)); } }
void MainWindow::on_cmdRun_clicked() { if (selectedFile != "") { VideoCapture vid(selectedFile.toStdString().c_str()); VideoWriter vout; vout.open((selectedFile + "out.mpeg").toStdString().c_str(),CV_FOURCC('M','P','E','G'),30,Size(vid.get(CV_CAP_PROP_FRAME_WIDTH), vid.get(CV_CAP_PROP_FRAME_HEIGHT))); qDebug() << "Frame cnt " << frame_count; QFileInfo filedir = QFileInfo(selectedFile); QDir currdir = filedir.dir(); int sample_size = frame_count; if (sample_size <= 0) return; int sample_interval = frame_count / sample_size; int sample_cnt = 0; ui->prog->setMaximum(frame_count); bool undistorted = ui->chkDistort->isChecked(); Mat cammatrix; Mat distortion; double rms; if (undistorted) { FileStorage file(calibFile.toStdString().c_str(),FileStorage::READ); file["rms"] >> rms; file["cameramatrix"] >> cammatrix; file["distortion"] >> distortion; } for (int i = 0; i < frame_count; i++) { Mat r; vid >> r; QString writepath = selectedFile + "-" + QString::number(i) + ".png"; Mat c; if (undistorted) { undistort(r, c, cammatrix, distortion); } else { c = r; } imwrite(writepath.toStdString().c_str(), c); vout << c; ui->prog->setValue(i); } ui->prog->setValue(frame_count); }
int main() { ssc_init("/dev/ttyUSB0", 9600); pan( 0 ); tilt( 0 ); cascade = (CvHaarClassifierCascade*)cvLoad( "cascade.xml", 0, 0, 0 ); if( !cascade ) { fprintf( stderr, "ERROR: Could not load classifier cascade\n" ); return 1; } storage = cvCreateMemStorage(0); struct camera *cam = init_camera( LCAM ); load_params( cam ); cvNamedWindow( "Face", CV_WINDOW_AUTOSIZE ); CvPoint pt1, pt2; CvRect *r; CvSeq *faces; float multip = 0.1; int x_diff, y_diff; while( cvWaitKey( 10 ) == -1 ) { cap_frame( cam ); undistort( cam ); cvClearMemStorage( storage ); faces = cvHaarDetectObjects( cam->undist, cascade, storage, 1.1, 2, CV_HAAR_DO_CANNY_PRUNING, cvSize(40, 40) ); if (faces->total > 0) { ; r = (CvRect*)cvGetSeqElem( faces, 0 ); pt1.x = r->x; pt2.x = r->x + r->width; pt1.y = r->y; pt2.y = r->y + r->height; cvRectangle( cam->undist, pt1, pt2, CV_RGB(255,0,0), 3, 8, 0 ); x_diff = (cam->frame->width / 2) - (r->x + (r->width/2)) ; y_diff = (cam->frame->height / 2) - (r->y + (r->height/2)) ; pan( -(int)(x_diff * multip) ); tilt( (int)(y_diff * multip) ); } cvShowImage( "Face", cam->undist ); } cvDestroyAllWindows(); cvReleaseMemStorage( &storage ); free_camera( cam ); }
/*! * @brief メソッドImageProcessing::getUndistortionImage().キャリブレーションデータを用いてKinectから取得した画像を補正する * @param cv::Mat& inputOriginalImage * @return cv::Mat undistortionImage */ Mat ImageProcessing::getUndistortionImage(Mat& inputOriginalImage) { Mat undistotionImage; undistort(inputOriginalImage, undistotionImage, internal_cameraparam, distortion_coefficients, Mat()); return undistotionImage; }
boost::shared_ptr< Image > Undistortion::undistort( boost::shared_ptr< Image > pImage ) { // shortcut if no distortion if ( !hasDistortion() ) return pImage; return undistort( *pImage ); }
/* * laser scan callback */ void NiftiLaserFiltering::scan_cb(const sensor_msgs::LaserScan& scan) { //ROS_INFO_STREAM("Got scan"); tmp_scan = scan; time_correct(tmp_scan); undistort(tmp_scan); robot_filter(tmp_scan); shadow_filter(tmp_scan); scan_filtered_pub.publish(tmp_scan); }
void Cognition::myRemap(Mat &img) { FileStorage readfs("camera.yml",FileStorage::READ); if(readfs.isOpened()) { Mat cameraMatrix, distCoeffs; readfs["camera_matrix"]>>cameraMatrix; readfs["distortion_coefficients"]>>distCoeffs; Mat temp=img.clone(); undistort(temp,img,cameraMatrix,distCoeffs); }
void RadialTangentialDistortion::undistort( const Eigen::MatrixBase<DERIVED> & yconst, const Eigen::MatrixBase<DERIVED_JY> & outJy) const { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC( Eigen::MatrixBase<DERIVED>, 2); EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE_OR_DYNAMIC( Eigen::MatrixBase<DERIVED_JY>, 2, 2); Eigen::MatrixBase<DERIVED> & y = const_cast<Eigen::MatrixBase<DERIVED> &>(yconst); y.derived().resize(2); // we use f^-1 ' = ( f'(f^-1) ) ' // with f^-1 the undistortion // and f the distortion undistort(y); // first get the undistorted image Eigen::Vector2d kp = y; Eigen::Matrix2d Jd; distort(kp, Jd); // now y = f^-1(y0) DERIVED_JY & J = const_cast<DERIVED_JY &>(outJy.derived()); J = Jd.inverse(); /* std::cout << "J: " << std::endl << J << std::endl; double mx2_u = y[0]*y[0]; double my2_u = y[1]*y[1]; //double mxy_u = y[0]*y[1]; double rho2_u = mx2_u+my2_u; double rad_dist_u = _k1*rho2_u+_k2*rho2_u*rho2_u; // take the inverse as Jacobian. J(0,0) = 1/(1 + rad_dist_u + _k1*2*mx2_u + _k2*rho2_u*4*mx2_u + 2*_p1*y[1] + 6*_p2*y[0]); J(1,0) = (_k1*2*y[0]*y[1] + _k2*4*rho2_u*y[0]*y[1] + _p1*2*y[0] + 2*_p2*y[1]); J(0,1) = J(1,0); J(1,1) = 1/(1 + rad_dist_u + _k1*2*my2_u + _k2*rho2_u*4*my2_u + 6*_p1*y[1] + 2*_p2*y[0]); // this should only happen if the distortion coefficients are 0 // the coefficients being zero removes the cross dependence then it is safe to set J(1,0) = 0 if (J(1,0) != 0) J(1,0) = 1/J(1,0); J(0,1) = J(1,0);*/ }
void ImageConverter::imageCb(const sensor_msgs::ImageConstPtr& msg) { cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); return; } src1 = cv_ptr->image; Mat temp = src1.clone(); undistort(temp, src1, cameraMatrix, distCoeffs); }
/** @brief Image capture @param[in] ppCameras @return void */ void ImageProcessing(Mat &img) { Mat imgHSV; Mat unImg; Mat imgBinary; // capture loop imshow("Camera", img); // untouched image // ========================================================================= // Pre-processing // ========================================================================= // Convert the captured image frame BGR to HSV undistort(img,unImg,CameraMatrix1,disCoeffs1); cvtColor(unImg, imgHSV, COLOR_BGR2HSV); // Threshold the image inRange(imgHSV, Scalar(lowH, lowS, lowV), Scalar(highH, highS, highV), imgBinary); //morphological closing (fill small holes in the foreground) dilate( imgBinary, imgBinary, getStructuringElement(MORPH_RECT, Size(5, 5)) ); erode(imgBinary, imgBinary, getStructuringElement(MORPH_RECT, Size(5, 5)) ); //morphological opening (remove small objects from the foreground) erode(imgBinary, imgBinary, getStructuringElement(MORPH_RECT, Size(5, 5)) ); dilate( imgBinary, imgBinary, getStructuringElement(MORPH_RECT, Size(5, 5)) ); GaussianBlur( imgBinary, imgBinary, Size(5, 5), 2, 2 ); imshow("Binarized Image", imgBinary); // ===================================================================== // Circle detection // Hough Circles or Polygonal Curve fitting algorithm, pick one // ===================================================================== //HoughCircles(unImg, imgBinary); PolygonalCurveDetection(unImg, imgBinary); char key = waitKey(1); }
void VideoHandle::selectImageColor() { flag_select = false; namedWindow("select_color"); setMouseCallback("select_color", onMouse, this); while(!flag_select) { getImage(); Mat temp = frame.clone(); undistort(frame, temp, distortmtx, distortdist); frame = temp; showImage("select_color"); } cout << selectx << " " << selecty << endl; cout << select_color_hsv << endl; destroyWindow("select_color"); }
void extractExamplesForView(const TrackList<SiftFeature>& tracks, const cv::Mat& F, const CameraProperties& camera1, const CameraProperties& camera2, const std::string& image_format, const std::string& view, int time, const std::vector<double>& scales, const std::vector<double>& angles) { // March along epipolar line. DistortedEpipolarRasterizer rasterizer(camera2, F); rasterizer.init(); cv::Mat K1(camera1.matrix()); cv::Mat K1_inv = K1.inv(); TrackList<SiftFeature>::const_iterator track; for (track = tracks.begin(); track != tracks.end(); ++track) { Track<SiftFeature>::const_iterator iter = track->find(time); CHECK(iter != track->end()) << "Track does not contain the current frame"; const SiftFeature& feature = iter->second; cv::Point2d y1(feature.position.x, feature.position.y); // Undo intrinsics, undistort, and re-apply intrinsics. cv::Point2d x1 = affineTransformImagePoint(y1, K1_inv); x1 = undistort(x1, camera1.distort_w); x1 = affineTransformImagePoint(x1, K1); // Extract the pixels of the epipolar line. std::vector<cv::Point> line; rasterizer.compute(x1, line); // Load first image. cv::Mat image; std::string image_file = makeImageFilename(image_format, view, time); bool ok = readGrayImage(image_file, image); CHECK(ok) << "Could not load image"; std::deque<std::deque<SiftFeature> > features; extractFeaturesAlongLine(line, image, scales, angles, features); } }
void DiscreteFrustum::interpolatedUndistort(double* z) const { int idx = index(*z); double start = bin_depth_ * idx; int idx1; if(*z - start < bin_depth_ / 2) idx1 = idx; else idx1 = idx + 1; int idx0 = idx1 - 1; if(idx0 < 0 || idx1 >= num_bins_ || counts_(idx0) < 50 || counts_(idx1) < 50) { undistort(z); return; } double z0 = (idx0 + 1) * bin_depth_ - bin_depth_ * 0.5; double coeff1 = (*z - z0) / bin_depth_; double coeff0 = 1.0 - coeff1; double mult = coeff0 * multipliers_.coeffRef(idx0) + coeff1 * multipliers_.coeffRef(idx1); *z *= mult; }
void ObjectDetector::tryImageIMSync() { // std::cout<<"imageQueue.size "<<imageQueue.size()<<std::endl; // std::cout<<"navdataQueue.size "<<navdataQueue.size()<<std::endl; //for (int i = 0; i < imageQueue.size(); i++) cout << imageQueue[i]->header.stamp << ", "; //cout << " | "; //for (int i = 0; i < navdataQueue.size(); i++) cout << navdataQueue[i].header.stamp << ", "; //cout << endl; while(imageQueue.size() > 0) { cv_bridge::CvImagePtr cv_ptr = imageQueue.front(); while (1 < navdataQueue.size() && navdataQueue[1].header.stamp < cv_ptr->header.stamp) navdataQueue.pop_front(); if (1 < navdataQueue.size()) { if (cv_ptr->header.stamp >= navdataQueue[0].header.stamp) { undistort(cv_ptr->image, frame, K, D); navdata_prev = navdata; ardrone_autonomy::Navdata & nd0 = navdataQueue[0], nd1 = navdataQueue[1]; navdata = navdataInterpolate(nd0, nd1, cv_ptr->header.stamp); navdata.altd = 800; if (navdata.altd > 0) { view_H = getBirdsEyeTransform(navdata.rotZ*GRAD, navdata.rotY*GRAD, navdata.rotX*GRAD, navdata.altd*0.001); warpPerspective(frame, view, view_H, view_size, INTER_CUBIC | WARP_INVERSE_MAP); imshow("view", view); findObjects(); } imshow("frame", frame); } imageQueue.pop_front(); } else break; } waitKey(1); }
void CameraCalibration::DrawChessboardAndImage(const int c_idx, const int img_idx, const bool is_undistorted, const bool is_stereobind) { Mat img = imread(data_path+"/"+calib_params[c_idx].ImageList.at(img_idx), CV_LOAD_IMAGE_COLOR); if(is_undistorted) { Mat undistort_img; undistort(img, undistort_img, calib_params[c_idx].CameraMatrix, calib_params[c_idx].DistCoeffs, calib_params[c_idx].CameraMatrix); img = undistort_img; } view[c_idx]->ActivateScissorAndClear(); gl_img_tex->Upload(img.ptr<unsigned char>(), GL_BGR, GL_UNSIGNED_BYTE); gl_img_tex->RenderToViewportFlipY(); // OpenGL rendering for left and right views CamIntrins[c_idx].Load(); if(!c_idx) CamExtrins[c_idx].at(img_idx).Load(); else { // right camera if(is_stereobind) StereoBind(CamExtrins[0].at(img_idx)).Load(); else CamExtrins[c_idx].at(img_idx).Load(); } DrawAxis(); gl_chessboard_tex->Render3DPlanTexture(BoardTexWdith, BoardTexHeight); }
void CameraCalibration::calibrate() { const string inputSettingsFile = "default.xml"; // Read the settings FileStorage fs( inputSettingsFile, FileStorage::READ ); if ( !fs.isOpened() ) { FileStorage fs( inputSettingsFile, FileStorage::WRITE ); fs.release(); cerr << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << endl; return; } else { s.read( fs["Settings"] ); // close Settings file fs.release(); } if ( !s.goodInput ) { cerr << "Invalid input detected. Application stopping." << endl; return; } vector<vector<Point2f> > imagePoints; Mat 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; for ( int i = 0; ; ++i ) { 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() >= (unsigned)s.nrFrames ) { if ( runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints ) ) { mode = CALIBRATED; } else { mode = DETECTION; } } // If no more images then run calibration, save and stop loop. if ( view.empty() ) { if ( imagePoints.size() > 0 ) { runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints); } break; } imageSize = view.size(); // Format input image. if ( s.flipVertical ) { flip( view, view, 0 ); } vector<Point2f> pointBuf; bool found; // Find feature points on the input format switch ( s.calibrationPattern ) { 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; } // If done with success, improve the found corners' coordinate accuracy for chessboard if ( found ) { 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::MAX_ITER, 30, 0.1 ) ); } // For camera only take new samples after delay time if ( mode == CAPTURING && (!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 ); } //----------------------------- 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 ); } //------------------------- Video capture output undistorted ------------------------------ if ( mode == CALIBRATED && s.showUndistorsed ) { Mat temp = view.clone(); undistort( temp, view, cameraMatrix, distCoeffs ); } //------------------------------ Show image and check for input commands ------------------- imshow( "Image View", view ); char key = (char)waitKey( s.inputCapture.isOpened() ? 50 : s.delay ); if ( key == ESC_KEY ) { break; } if ( key == 'u' && mode == CALIBRATED ) { s.showUndistorsed = !s.showUndistorsed; } if ( s.inputCapture.isOpened() && key == 'g' ) { mode = CAPTURING; imagePoints.clear(); } } // -----------------------Show the undistorted image for the image list ------------------------ 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 ); for ( int i = 0; i < (int)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; } } } }
void CalibrateThread::run() { Size boardSize, imageSize; float squareSize = 1.f, aspectRatio = 1.f; Mat cameraMatrix, distCoeffs; //QString of = ui->lineEdit_WorkFolder->text() + '/' + ui->lineEdit_OutputName->text(); QByteArray ba = strFileName.toLatin1(); const char* outputFilename = ba.data(); int i, nframes = 0; bool writeExtrinsics = true, writePoints = true; bool undistortImage = false; int flags = 0; VideoCapture capture; bool flipVertical = false; bool showUndistorted = false; int delay = 1000; clock_t prevTimestamp = 0; int mode = CAPTURING; vector<vector<Point2f> > imagePoints; vector<string> imageList; Pattern pattern = CHESSBOARD; boardSize.width = m_width; boardSize.height = m_height; squareSize = m_squaresize; //ui->textEdit_Information->append("\nCalibrating... Please wait for a while\n"); if( imgList.size() == 0 ) { //QMessageBox::warning(NULL, "Error", "Please choose a right folder"); emit popupErrorInformation("Please choose a right folder"); emit closeImageWindow(); return; } else { nframes = imgList.size(); } emit appendText("\nCalibrating... Please wait for a while\n"); //namedWindow( "Image View", 1 ); //bDialog->show(); for(i = 0; i < nframes ;i++) { //ui->textEdit_Information->append("Processing the image No. " + QString::number(i + 1)); emit appendText("Processing the image No. " + QString::number(i + 1)); Mat view, viewGray; bool blink = false; qDebug(imgList.at(i).toLatin1().data()); if( i < (int)imgList.size() ) view = imread(imgList.at(i).toLatin1().data(), 1); if(!view.data) { //QMessageBox::warning(NULL, "Error", ); emit popupErrorInformation("Could not open image files"); return; } imageSize = view.size(); if( flipVertical ) flip( view, view, 0 ); vector<Point2f> pointbuf; cvtColor(view, viewGray, CV_BGR2GRAY); bool found; switch( pattern ) { case CHESSBOARD: found = findChessboardCorners( view, boardSize, pointbuf, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE); break; case CIRCLES_GRID: found = findCirclesGrid( view, boardSize, pointbuf ); break; case ASYMMETRIC_CIRCLES_GRID: found = findCirclesGrid( view, boardSize, pointbuf, CALIB_CB_ASYMMETRIC_GRID ); break; } // improve the found corners' coordinate accuracy if( pattern == CHESSBOARD && found) cornerSubPix( viewGray, pointbuf, Size(11,11), Size(-1,-1), TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 )); if( mode == CAPTURING && found && (!capture.isOpened() || clock() - prevTimestamp > delay*1e-3*CLOCKS_PER_SEC) ) { imagePoints.push_back(pointbuf); prevTimestamp = clock(); blink = capture.isOpened(); } if(found) drawChessboardCorners( view, boardSize, Mat(pointbuf), found ); 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(undistortImage) msg = format( "%d/%d Undist", (int)imagePoints.size(), nframes ); else msg = format( "%d/%d", (int)imagePoints.size(), nframes ); } putText( view, msg, textOrigin, 1, 1, mode != CALIBRATED ? Scalar(0,0,255) : Scalar(0,255,0)); if( blink ) bitwise_not(view, view); if( mode == CALIBRATED && undistortImage ) { Mat temp = view.clone(); undistort(temp, view, cameraMatrix, distCoeffs); } Mat rgb; cvtColor(view, rgb, CV_BGR2RGB); QImage image32 = QImage(rgb.cols, rgb.rows, QImage::Format_RGB32); QRgb value; for(int r = 0; r < rgb.rows; r++) { for(int c = 0; c < rgb.cols; c++) { value = qRgb(rgb.ptr<uchar>(0)[r * rgb.cols * 3 + c * 3 + 0], rgb.ptr<uchar>(0)[r * rgb.cols * 3 + c * 3 + 1], rgb.ptr<uchar>(0)[r * rgb.cols * 3 + c * 3 + 2]); image32.setPixel(c, r, value); } } emit showBitmap(image32); int key; if(i < nframes - 1) { key = 0xff & waitKey(500); } else { key = waitKey(500); } if( (key & 255) == 27 ) break; if( key == 'u' && mode == CALIBRATED ) undistortImage = !undistortImage; } if(imagePoints.size() > 0) { emit appendText("\n" + QString::number(imagePoints.size()) + " out of " + QString::number(nframes) + " images are effective!\n" ); runAndSave(outputFilename, imagePoints, imageSize, boardSize, pattern, squareSize, aspectRatio, flags, cameraMatrix, distCoeffs, writeExtrinsics, writePoints); } else { emit appendText("Calibrating is not successful! \nPlease change the parameters and try again!"); emit popupErrorInformation("Sorry, no enough points are detected! Please try another group of images!"); emit closeImageWindow(); return; } emit appendText("Calibrating Successfully! \nPlease go to the folder to check the out put files!"); emit closeImageWindow(); if( !capture.isOpened() && showUndistorted ) { Mat view, rview, map1, map2; initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(), getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), imageSize, CV_16SC2, map1, map2); for( i = 0; i < (int)imageList.size(); i++ ) { view = imread(imageList[i], 1); if(!view.data) continue; //undistort( view, rview, cameraMatrix, distCoeffs, cameraMatrix ); remap(view, rview, map1, map2, INTER_LINEAR); imshow("Image View", rview); int c = 0xff & waitKey(); if( (c & 255) == 27 || c == 'q' || c == 'Q' ) break; } } return; }
int main(int argc, char** argv){ int status,i; Ice::CommunicatorPtr ic; Ice::PropertiesPtr prop; cv::Size boardSize; Pattern pattern = CHESSBOARD; float squareSize; float aspectRatio; int nframes; bool writeExtrinsics = false; bool writePoints = false; int delay; clock_t prevTimestamp = 0; int mode = DETECTION; bool undistortImage = false; bool globalCalibrated=false; std::vector<cameraData> cameras; try{ ic = Ice::initialize(argc,argv); prop = ic->getProperties(); }catch (const Ice::Exception& ex) { std::cerr << ex << std::endl; return 1; } catch (const char* msg) { std::cerr <<"Error :" << msg << std::endl; return 1; } std::string componentPrefix("cameraCalibrator"); std::cout << "Ncameras: " << prop->getPropertyAsIntWithDefault(componentPrefix+".nCameras",1) << std::endl; for (int i=0; i< prop->getPropertyAsIntWithDefault(componentPrefix+".nCameras",1); i++){ cameraData cam; std::stringstream ss; ss << componentPrefix << ".camera." <<i <<"."; cam.outputFilename=prop->getProperty(ss.str()+"outFile"); cam.proxy = new jderobot::cameraClient(ic,ss.str()); if (cam.proxy != NULL){ cam.proxy->start(); } else{ throw "rgbdViewer: failed to load RGB Camera"; } cam.calibrated=false; cameras.push_back(cam); } //parsing calibration parameteres boardSize.height=prop->getPropertyAsIntWithDefault(componentPrefix+".pattern.height",6); boardSize.width=prop->getPropertyAsIntWithDefault(componentPrefix+".pattern.width",9); std::string patterType =prop->getProperty(componentPrefix+".pattern.type"); if( !strcmp( patterType.c_str(), "circles" ) ) pattern = CIRCLES_GRID; else if( !strcmp( patterType.c_str(), "acircles" ) ) pattern = ASYMMETRIC_CIRCLES_GRID; else if( !strcmp( patterType.c_str(), "chessboard" ) ) pattern = CHESSBOARD; else return fprintf( stderr, "Invalid pattern type: must be chessboard or circles\n" ), -1; squareSize=atof(prop->getProperty(componentPrefix+".pattern.size").c_str()); aspectRatio=atof(prop->getProperty(componentPrefix+".pattern.ratio").c_str()); nframes=prop->getPropertyAsIntWithDefault(componentPrefix+".frames",10); writePoints=boost::lexical_cast<bool>(prop->getProperty(componentPrefix+".writePoints")); writeExtrinsics=boost::lexical_cast<bool>(prop->getProperty(componentPrefix+".writeEstrinsics")); delay=prop->getPropertyAsIntWithDefault(componentPrefix+".delay",1000); //init the poling while (1){ cv::Mat frame; for (std::vector<cameraData>::iterator it= cameras.begin(); it != cameras.end(); it++){ while (it->frame.cols ==0){ it->proxy->getImage(it->frame); } it->imageSize=it->frame.size(); } break; } std::cout << "Poling init done" << std::endl; while (1){ for (std::vector<cameraData>::iterator it= cameras.begin(); it != cameras.end(); it++){ it->proxy->getImage(it->frame); it->frame.copyTo(it->raw_frame); it->pointbuf.resize(0); if (it->frame.channels() ==3) cv::cvtColor(it->frame, it->viewGray, cv::COLOR_BGR2GRAY); else it->frame.copyTo(it->viewGray); it->found=false; switch( pattern ) { case CHESSBOARD: it->found = cv::findChessboardCorners( it->frame, boardSize, it->pointbuf, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE); break; case CIRCLES_GRID: it->found = cv::findCirclesGrid( it->frame, boardSize, it->pointbuf ); break; case ASYMMETRIC_CIRCLES_GRID: it->found = cv::findCirclesGrid( it->frame, boardSize, it->pointbuf, cv::CALIB_CB_ASYMMETRIC_GRID ); break; default: return fprintf( stderr, "Unknown pattern type\n" ), -1; } if( pattern == CHESSBOARD && it->found) cv::cornerSubPix( it->viewGray, it->pointbuf, cv::Size(11,11), cv::Size(-1,-1), cv::TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 )); if(it->found) cv::drawChessboardCorners( it->frame, boardSize, cv::Mat(it->pointbuf), it->found ); } bool globalFound=true; for (std::vector<cameraData>::iterator it= cameras.begin(); it != cameras.end(); it++){ globalFound=globalFound && it->found; } if( mode == CAPTURING && globalFound && (clock() - prevTimestamp > delay*1e-3*CLOCKS_PER_SEC) ) { for (std::vector<cameraData>::iterator it= cameras.begin(); it != cameras.end(); it++){ cv::Mat temp; it->raw_frame.copyTo(temp); it->images.push_back(temp); it->imagePoints.push_back(it->pointbuf); } prevTimestamp = clock(); } for (std::vector<cameraData>::iterator it= cameras.begin(); it != cameras.end(); it++){ std::string msg = mode == CAPTURING ? "100/100" : mode == CALIBRATED ? "Calibrated" : "Press 'g' to start"; int baseLine = 0; cv::Size textSize = cv::getTextSize(msg, 1, 1, 1, &baseLine); cv::Point textOrigin(cameras[0].frame.cols - 2*textSize.width - 10, cameras[0].frame.rows - 2*baseLine - 10); if( mode == CAPTURING ) { if(undistortImage) msg = cv::format( "%d/%d Undist", (int)it->imagePoints.size(), nframes ); else msg = cv::format( "%d/%d", (int)it->imagePoints.size(), nframes ); } cv::putText( it->frame, msg, textOrigin, 1, 1, mode != CALIBRATED ? cv::Scalar(0,0,255) : cv::Scalar(0,255,0)); if( mode == CALIBRATED && undistortImage ) { cv::Mat temp = it->frame.clone(); undistort(temp, it->frame, it->cameraMatrix, it->distCoeffs); } std::stringstream ss; ss << "Image " << it-cameras.begin(); cv::imshow(ss.str(),it->frame); int key=0xff & cv::waitKey(1); if( (key & 255) == 27 ) break; if( key == 'u' && mode == CALIBRATED ) undistortImage = !undistortImage; if( key == 'g' ) { mode = CAPTURING; it->imagePoints.clear(); } if( !globalCalibrated && it->imagePoints.size() >= (unsigned)nframes ) { if( runAndSave(it->outputFilename, it->imagePoints, it->imageSize, boardSize, pattern, squareSize, aspectRatio, 0, it->cameraMatrix, it->distCoeffs, writeExtrinsics, writePoints)){ mode = CALIBRATED; it->calibrated=true;; } else mode = DETECTION; //save pictures int cameraID = std::distance(cameras.begin(),it); for ( std::vector<cv::Mat>::iterator itImage = it->images.begin(); itImage != it->images.end(); itImage++){ std::stringstream ss; ss << "camera-" << cameraID << "-" << std::distance(it->images.begin(), itImage) << ".png"; cv::imwrite(ss.str(),*itImage); } } } for (std::vector<cameraData>::iterator it= cameras.begin(); it != cameras.end(); it++){ if (it == cameras.begin()){ globalCalibrated=it->calibrated; } else globalCalibrated=globalCalibrated && it->calibrated; } } }
Mat Camera::undistortImage(Mat image) { Mat imund; undistort(image,imund,K,Dist); return imund; }
void Camera::undistortImage(Mat& src, Mat& dst) { OPENCV_ASSERT(isCalibrated,"Camera.undistortImage","Cannot undistort before camera calibration!"); undistort(src, dst, cameraMatrix, distortionCoeffs); }
/// it has multithread support under c++ 11 void CameraCalibration::getImagesAndFindPatterns(const string &cameraName) { // set mode mode = CAPTURING; frameCounter = 0; imageCounter = 0; capturedFrame currentImage; // read current path currentPath = boost::filesystem::current_path(); resultsPath = currentPath; cout << "current Results Path" << currentPath << '\n' << endl; //mode = s.inputType == Settings::IMAGE_LIST ? CAPTURING : DETECTION;// check enum type // Capture only the frames settled in the configuration file // in the original code it was a endless loop for (int i = 0;; ++i) { Mat view,currentView; bool blinkOutput = false; // capture the image view = s.nextImage(); frameCounter = frameCounter + 1; //------------------------- Show original distorted image ----------------------------------- Mat originalView = view.clone(); //imshow("original Image", originalView); //----- If no more image, or got enough, then stop calibration and show result ------------- if (mode == CAPTURING && imagePoints.size() >= (unsigned)s.nrFrames) { if (runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints)) mode = CALIBRATED; else mode = DETECTION; } if (view.empty()) // If no more images then run calibration, save and stop loop. { if (imagePoints.size() > 0) runCalibrationAndSave(s, imageSize, cameraMatrix, distCoeffs, imagePoints); break; } imageSize = view.size(); // Format input image. if (s.flipVertical) flip(view, view, 0); 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; } 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::MAX_ITER, 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(); circlePoints = pointBuf; // save ordered circle points } // Draw the corners with ID according order criteria distance(x+y) from origin point 1 savedImage = view.clone(); drawChessboardCorners(view, s.boardSize, Mat(pointBuf), found); // order the points according to d(x+y) from the upper left corner that is used as the origin frame std::sort(pointBuf.begin(), pointBuf.end(), [](const cv::Point2f &a, const cv::Point2f &b) {return ((a.x + a.y) < (b.x + b.y)); }); int pointCounter = 1; for (auto k:pointBuf){ cv::putText(view,std::to_string(pointCounter),cv::Point(k.x,k.y),cv::FONT_HERSHEY_PLAIN,1.0,cv::Scalar(255,0,0),1); pointCounter = pointCounter + 1; } } //----------------------------- Output Text ------------------------------------------------ string msg = (mode == CAPTURING) ? "100/100" : mode == CALIBRATED ? "Calibrated" : "the images are being captured"; 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); // save the image used for calibration to disk imageCounter = imageCounter + 1; string pathToFile; string filename; if (imageCounter <= s.nrFrames) { string imageName{ "Image" + string(std::to_string(imageCounter)) + cameraName + ".jpg" }; boost::filesystem::path p{ "/" }; // add a slash to generate a portable string filename.assign(resultsPath.string() + p.generic_string() + imageName); vector<int> compression_params; compression_params.push_back(IMWRITE_JPEG_QUALITY); compression_params.push_back(100); // check if the file already exist? if yes, erase it bool found = getPathForThisFile(imageName,pathToFile); if (found){ boost::filesystem::remove(pathToFile); } // write the new version of the file cv::imwrite(filename, savedImage); // save the points use to estimate the scale factor int currentBufferPosition = imagePoints.size(); int pointCounter = 1; circlePatternInfo currentCircle; circlesDataPerImage dataCurrentImage; vector<circlePatternInfo> circlesInfoFromThisImage; for (auto k : circlePoints){ currentCircle.circleID = pointCounter; currentCircle.circlePosition = cv::Point2f(k.x, k.y); currentCircle.circle3DPosition = circle_Mis_Positions.at(pointCounter - 1); circlesInfoFromThisImage.push_back(currentCircle); pointCounter = pointCounter + 1; } circlePoints.clear(); dataCurrentImage.imageID= imageCounter; dataCurrentImage.cameraID = s.cameraID; dataCurrentImage.circlesData = circlesInfoFromThisImage; // save all the data from the asymetric circles pattern DataFromCirclesPattern.push_back(dataCurrentImage); } } //------------------------- Video capture output undistorted ------------------------------ if (mode == CALIBRATED && s.showUndistorsed) { Mat temp = view.clone(); undistort(temp, view, cameraMatrix, distCoeffs); string msgEsckey = "Press 'esc' key to quit"; putText(view, msgEsckey, textOrigin, 1, 1, GREEN, 2); } //------------------------------ Show image and check for input commands ------------------- imshow(cameraName, view); //delay 30ms so that screen can refresh. //image will not appear without this waitKey() command cv::waitKey(30); char c = waitKey(1); if (c == ESC_KEY) // Escape key break; // Breaks the capture loop } }
//function wrapper for array input void undistort(const cv::Mat img[], cv::Mat img_undist[], const cv::Mat cameraMatrix, const cv::Mat distCoeffs){ for (int i=0; i<m; i++){ undistort(img[i], img_undist[i], camIntrinsic, dist); } }
void Calibration::undistort(Mat img) { img.copyTo(undistortBuffer); undistort(undistortBuffer, img); }
void Camera::getPointCloudUndistorted(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud) { pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_distorted(new pcl::PointCloud<pcl::PointXYZRGBA>); getPointCloud(cloud_distorted); undistort(cloud_distorted,cloud); }
std::pair<int, cv::Point2d> undistortIndexedPoint( const std::pair<int, cv::Point2d>& point, double w) { return std::make_pair(point.first, undistort(point.second, w)); }
void Calibration::undistort(Mat img, int interpolationMode) { img.copyTo(undistortBuffer); undistort(undistortBuffer, img, interpolationMode); }
/////////////////////////////////////////////////////// // 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; }