//! Individua gli eventuali angoli presenti nella curva da calcolare
void detectCorners(const std::vector<T3DPointD> &inputPoints,
int minSampleNum, int minDist, int maxDist, double maxAngle,
std::vector<int> &cornerIndexes)
{
gMinSampleNum = minSampleNum;
gMinDist = minDist;
gMaxDist = maxDist;
gSquaredMinDist = minDist * minDist;
gSquaredMaxDist = maxDist * maxDist;
gMaxAngle = maxAngle;
interpolate(inputPoints);
if ((int)gPoints.size() > 2 * gMaxDist) {
findCornerCandidates();
findCorners((int)sqrt((float)gSquaredMaxDist) + 10, cornerIndexes);
}
gPoints.clear();
// check for no index equal to an adjacent
std::vector<int>::iterator it1, it2;
it1 = it2 = cornerIndexes.begin();
++it2;
while (it2 != cornerIndexes.end()) {
if (*it1 == *it2) {
it2 = cornerIndexes.erase(it2);
} else {
++it1;
++it2;
}
}
}
// Return an image with corners marked to display.
//
const cv::Mat &apply()
{
static const int radius = 4;
const int max = bar < 1 ? 1 : bar > maxBar ? maxBar : bar;
const std::vector<cv::Point2f> corners = findCorners(grayImage, max);
sourceImage.copyTo(cornersImage);
for (int i = 0; i < corners.size(); ++i) {
drawCircle(cornersImage, corners[i], radius);
}
return cornersImage;
}
int main(int argc, const char **argv)
{
// Instantiate a ModelManager:
ModelManager manager("Test wiimote");
nub::ref<OutputFrameSeries> ofs(new OutputFrameSeries(manager));
manager.addSubComponent(ofs);
nub::ref<InputFrameSeries> ifs(new InputFrameSeries(manager));
manager.addSubComponent(ifs);
// Parse command-line:
if (manager.parseCommandLine(argc, argv, "", 0, 0) == false) return(1);
manager.start();
//Init camara params
itsIntrinsicMatrix = cvCreateMat( 3, 3, CV_32FC1);
itsDistortionCoeffs = cvCreateMat( 4, 1, CV_32FC1);
itsCameraRotation = cvCreateMat( 1, 3, CV_64FC1);
itsCameraTranslation = cvCreateMat( 1, 3, CV_64FC1);
//cvmSet(itsDistortionCoeffs, 0, 0, -0.2403274);
//cvmSet(itsDistortionCoeffs, 1, 0, 2.5312502);
//cvmSet(itsDistortionCoeffs, 2, 0, -0.0439848);
//cvmSet(itsDistortionCoeffs, 3, 0, -0.0106820);
cvmSet(itsDistortionCoeffs, 0, 0, 0);
cvmSet(itsDistortionCoeffs, 1, 0, 0);
cvmSet(itsDistortionCoeffs, 2, 0, 0);
cvmSet(itsDistortionCoeffs, 3, 0, 0);
cvmSet(itsCameraRotation, 0, 0, 2.391102);
cvmSet(itsCameraRotation, 0, 1, 0);
cvmSet(itsCameraRotation, 0, 2, 0);
cvmSet(itsCameraTranslation, 0, 0, 0);
cvmSet(itsCameraTranslation, 0, 1, 0);
cvmSet(itsCameraTranslation, 0, 2, 840.954432);
//cvmSet(itsIntrinsicMatrix, 0, 0, 290.85342); cvmSet(itsIntrinsicMatrix, 0, 1, 0); cvmSet(itsIntrinsicMatrix, 0, 2, 320/2); //159.50000);
//cvmSet(itsIntrinsicMatrix, 1, 0, 0); cvmSet(itsIntrinsicMatrix, 1, 1, 290.85342 ); cvmSet(itsIntrinsicMatrix, 1, 2, 240/2); // 119.5);
//cvmSet(itsIntrinsicMatrix, 2, 0, 0); cvmSet(itsIntrinsicMatrix, 2, 1, 0); cvmSet(itsIntrinsicMatrix, 2, 2, 1);
cvmSet(itsIntrinsicMatrix, 0, 0, 415.5); cvmSet(itsIntrinsicMatrix, 0, 1, 0); cvmSet(itsIntrinsicMatrix, 0, 2, 320/2); //159.50000);
cvmSet(itsIntrinsicMatrix, 1, 0, 0); cvmSet(itsIntrinsicMatrix, 1, 1, 436 ); cvmSet(itsIntrinsicMatrix, 1, 2, 240/2); // 119.5);
cvmSet(itsIntrinsicMatrix, 2, 0, 0); cvmSet(itsIntrinsicMatrix, 2, 1, 0); cvmSet(itsIntrinsicMatrix, 2, 2, 1);
bool drawGrid = true;
bool saveCorners = false;
bool calibrate = false;
std::vector<CvPoint2D32f> allCorners;
while(1)
{
GenericFrame input = ifs->readFrame();
Image<PixRGB<byte> > img = input.asRgb();
int rows = 4, cols = 3;
std::vector<CvPoint2D32f> corners = findCorners(img, rows, cols);
if (corners.size() == (uint)(rows*cols))
{
if (saveCorners)
for(uint i=0; i<corners.size(); i++)
allCorners.push_back(corners[i]);
saveCorners = false;
cvDrawChessboardCorners(img2ipl(img), cvSize(rows,cols), &corners[0], corners.size(), 1);
}
if (calibrate)
{
calibrateViews(allCorners, rows, cols);
if (corners.size() == (uint)(rows*cols))
findExtrinsic(corners, rows, cols);
calibrate = false;
}
if (drawGrid)
projectGrid(img);
projectRect(img, 216.5, 279.5);
processUserInput(ofs, drawGrid, saveCorners, calibrate);
ofs->writeRGB(img, "Output", FrameInfo("Output", SRC_POS));
ofs->updateNext();
}
// stop all our ModelComponents
manager.stop();
// all done!
return 0;
}
CornerDetector(IMGH::Image *img, float thr=0.1) : corner_count(0) { findCorners(img, thr); }
void findCorners(int w, int h, pixel_t type, void *udata, float thr=0.1) {
grd_dummy.getGradientImage( w, h, type, udata );
findCorners( &grd_dummy, thr );
}
void findCorners(IMGH::Image *img, float thr=0.1) {
grd_dummy.getGradientImage( img );
findCorners( &grd_dummy, thr );
}
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;
}
