源码是《Learning Opencv》中的,自己在这个基础上加入了重建部分,其实就是读取了生成的相机外参:平移旋转矩阵,然后在绘制中调用一下这个矩阵。由于加了平移之后就不知道物体移动到哪里去了,demo中只加了旋转部分。开了一个线程跑opencv重建,主线程用Opengl绘制。
识别效果很一般,经常会识别失败,所以这个算法已经被放弃,把demo丢在这里作为纪念。
按p键可以切换到透视角度,视觉效果好一些=A= 不能上传太大的图所以只有很小的片段了
run.h
#pragma once
#include <cv.h>
class check
{
public:
CvMat *Rot = nullptr;
CvMat *Trans = nullptr;
int found;
void run();
};
#include "run.h"
#include <cv.h>
#include <highgui.h>
#include <stdio.h>
#include <stdlib.h>
void check::run() {
int board_w = 12;
int board_h = 12;
int board_n = board_w * board_h;
CvSize board_sz = cvSize(board_w, board_h);
CvMat *intrinsic = (CvMat*)cvLoad("Intrinsics.xml");
CvMat *distortion = (CvMat*)cvLoad("Distortion.xml");
IplImage *image = 0, *gray_image = 0;
CvCapture *cap = cvCaptureFromCAM(0);
IplImage* mapx = cvCreateImage(cvSize(640,480), IPL_DEPTH_32F, 1);
IplImage* mapy = cvCreateImage(cvSize(640, 480), IPL_DEPTH_32F, 1);
CvPoint2D32f* corners = new CvPoint2D32f[board_n];
CvMat* RotRodrigues = cvCreateMat(3, 1, CV_32F);
CvMat* image_points = cvCreateMat(4, 1, CV_32FC2);
CvMat* object_points = cvCreateMat(4, 1, CV_32FC3);
char key;
int count = 0;
while (1)
{
image = cvQueryFrame(cap);
key = cvWaitKey(10);
cvShowImage("Camera", image);
gray_image = cvCreateImage(cvGetSize(image), 8, 1);
cvCvtColor(image, gray_image, CV_BGR2GRAY);
cvInitUndistortMap(
intrinsic,
distortion,
mapx,
mapy
);
IplImage *t = cvCloneImage(image);
cvRemap(t, image, mapx, mapy);
int corner_count = 0;
found = cvFindChessboardCorners(
image,
board_sz,
corners,
&corner_count,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS
);
cvFindCornerSubPix(gray_image, corners, corner_count,
cvSize(11, 11), cvSize(-1, -1),
cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
CvPoint2D32f objPts[4], imgPts[4];
objPts[0].x = 0; objPts[0].y = 0;
objPts[1].x = board_w - 1; objPts[1].y = 0;
objPts[2].x = 0; objPts[2].y = board_h - 1;
objPts[3].x = board_w - 1; objPts[3].y = board_h - 1;
imgPts[0] = corners[0];
imgPts[1] = corners[board_w - 1];
imgPts[2] = corners[(board_h - 1)*board_w];
imgPts[3] = corners[(board_h - 1)*board_w + board_w - 1];
for (int i = 0; i < 4; ++i) {
CV_MAT_ELEM(*image_points, CvPoint2D32f, i, 0) = imgPts[i];
CV_MAT_ELEM(*object_points, CvPoint3D32f, i, 0) = cvPoint3D32f(objPts[i].x, objPts[i].y, 0);
}
Rot = cvCreateMat(3, 3, CV_32F);
Trans = cvCreateMat(3, 1, CV_32F);
cvFindExtrinsicCameraParams2(object_points, image_points,
intrinsic, distortion,
RotRodrigues, Trans);
cvRodrigues2(RotRodrigues, Rot);
//delete t;
}
cvReleaseCapture(&cap);
}
main.cpp
#include "run.h"
#include <iostream>
#include <stdlib.h>
#include <thread>
#include "gl/glut.h"
float fRotate = 0.0f; //旋转因子
float fScale = 1.0f; //缩放因子
float tRotate = 0.0f; //旋转因子
check* c;
bool bPersp = false; //是否为透视投影 (vs 正投影)
bool bAnim = false; //
bool bWire = false; //
bool isRotate = false; //
float ax, ay, az;
float mx,my;
bool isDown = false;
float rotateMatrix[16] = { 0 };
float translateMatrix[16] = { 0 };
float wHeight, wWidth;
int min(int x, int y)
{
return x < y ? x : y;
}
void initMat(CvMat* mat, float* dstMat)
{
for (int i = 0; i < mat->rows; i++) {
float* pData = (float*)(mat->data.ptr + i*mat->step);
for (int j = 0; j < mat->cols; j++) {
dstMat[4 * i + j] = *pData;
pData++;//指向下一个元素
}
}
}
void mouse(int button, int state, int x, int y)
{
if (button == GLUT_LEFT_BUTTON) {
if (state == GLUT_DOWN) {
isDown = true;
mx = x;
my = y;
}
else if (state == GLUT_UP) {
isDown = false;
}
}
}
void mouseMotion(int x, int y)
{
if (isDown) {
ax += 1.0f*(y - my) / 10.0f;
ay += 1.0f*(x - mx) / 10.0f;
mx = x;
my = y;
}
glutPostRedisplay();
}
void draw()
{
int numx = 13;
int numy = 13;
const float size = 0.1f;
glDisable(GL_COLOR_MATERIAL);
GLfloat mat_ambient[] = { 0.247250, 0.199500, 0.074500, 1.000000, };
GLfloat mat_diffuse[] = { 0.751640, 0.606480, 0.226480, 1.000000, };
GLfloat mat_specular[] = { 0.628281, 0.555802, 0.366065, 1.000000, };
GLfloat mat_shininess[] = { 83.199997, };
glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);
glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuse);
glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
glMaterialfv(GL_FRONT, GL_SHININESS, mat_shininess);
glPushMatrix();
glTranslatef(-5, -5, -10);
glRectf(0,0, 10, 10);
glPopMatrix();
glPushMatrix();
glTranslatef(-5,-5, -10);
glRotatef(90, 1, 0, 0);
glRectf(0,0, 10, 10);
glPopMatrix();
glPushMatrix();
glTranslatef(-5,-5, 0);
glRotatef(90, 0, 1, 0);
glRectf(0,0, 10, 10);
glPopMatrix();
if (c->found == 0)return;
CvMat* mat1 = c->Rot;
CvMat* mat2 = c->Trans;
if (mat1 == nullptr || mat2 == nullptr)return;
initMat(mat1, rotateMatrix);
//initMat(mat2, translateMatrix);
//translateMatrix[8] = -translateMatrix[8];
rotateMatrix[15] = 1;
//translateMatrix[15] = 1;
glEnable(GL_COLOR_MATERIAL);
//设置颜色追踪的材料属性以及多边形的面
glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
glPushMatrix();
glTranslatef(-numx*size / 2 + size/2, 0, -numy*size / 2 + size/2);
//glTranslatef(translateMatrix[0], translateMatrix[4], translateMatrix[8]);
//glMultMatrixf(translateMatrix);
glMultMatrixf(rotateMatrix);
for (int i = 0; i < numx; i++) {
for (int j = 0; j < numy; j++) {
if ((i + j) % 2 == 1) {
glColor3f(0, 0, 0);
}
else {
glColor3f(1, 1, 1);
}
glRectf(0, 0,size,size);
glTranslatef(0, size,0);
}
glTranslatef(size, -size*numy,0);
}
glPopMatrix();
}
void updateView(int width, int height)
{
glViewport(0, 0, width, height);//设置视窗大小
glMatrixMode(GL_PROJECTION);//设置矩阵模式为投影
glLoadIdentity(); //初始化矩阵为单位矩阵
float whRatio = (GLfloat)width / (GLfloat)height;//设置显示比例
if (bPersp) {
gluPerspective(45, whRatio, 1, 100);
}
else glOrtho(-3, 3, -3, 3, -100, 100); //正投影
glMatrixMode(GL_MODELVIEW); //设置矩阵模式为模型
}
void reshape(int width, int height)
{
if (height == 0) //如果高度为0
{
height = 1; //让高度为1(避免出现分母为0的现象)
}
//height = width = min(height, width);
wHeight = height;
wWidth = width;
updateView(wHeight, wWidth); //更新视角
}
void idle()
{
glutPostRedisplay();//调用当前绘制函数
}
float eye[] = { 0, 0, 30 };
float center[] = { 0, 0, 0 };
float place[] = { 0, 0, 5 };
//按键回调函数
void key(unsigned char k, int x, int y)
{
switch (k)
{
case 'q': {exit(0); break; } //退出
case 'p': {bPersp = !bPersp; updateView(wHeight, wWidth); break; } //切换正投影、透视投影
case ' ': {bAnim = !bAnim; break; } //旋转模式的切换
case 'o': {bWire = !bWire; break; } //渲染方式的切换
//整体操作
case 'a': { //向左移动
center[0] += 0.1f;
eye[0] += 0.1f;
break;
}
case 'd': { //向右移动
center[0] -= 0.1f;
eye[0] -= 0.1f;
break;
}
case 'w': { //向上移动
center[1] -= 0.1f;
eye[1] -= 0.1f;
break;
}
case 's': { //向下移动
center[1] += 0.1f;
eye[1] += 0.1f;
break;
}
case 'z': { //向前移动
center[2] -= 0.1f;
eye[2] -= 0.1f;
break;
}
case 'c': { //向后移动
center[2] += 0.1f;
eye[2] += 0.1f;
break;
}
case 'l': { //右移
place[0] += 0.1f;
if (place[0] > 1.5f)place[0] = 1.5f; //不超出桌面范围
break;
}
case 'j': { //左移
place[0] -= 0.1f;
if (place[0] < -1.5f)place[0] = -1.5f;
break;
}
case 'i': { //后移
place[1] += 0.1f;
if (place[1] > 1.5f)place[1] = 1.5f;
break;
}
case 'k': { //前移
place[1] -= 0.1f;
if (place[1] < -1.5f)place[1] = -1.5f;
break;
}
case 'e': { //旋转
isRotate = !isRotate;
break;
}
}
}
void redraw()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //清除颜色缓存和深度缓存
glClearColor(0.3, 0.3, 0.3,1);
glLoadIdentity(); //初始化矩阵为单位矩阵
gluLookAt(eye[0], eye[1], eye[2],
center[0], center[1], center[2],
0, 1, 0);
glEnable(GL_LIGHTING); //开启光照模式
GLfloat light_pos[] = { 5.0 ,5.0,5.0,1 }; //定义环境光位置
GLfloat white[] = { 1.0f, 1.0f, 1.0f, 1.0f }; //定义白色
glLightfv(GL_LIGHT0, GL_POSITION, light_pos); //设置第0号光源的光照位置
glLightfv(GL_LIGHT0, GL_SPECULAR, white); //设置镜面反射光照颜色
glLightfv(GL_LIGHT0, GL_DIFFUSE, white); //设置漫射光成分
glLightfv(GL_LIGHT0, GL_AMBIENT, white); //设置第0号光源多次反射后的光照颜色(环境光颜色)
glEnable(GL_LIGHT0); //开启第0号光源
if (bWire) {
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); //设置多边形绘制模式:正反面,线型
}
else {
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); //设置多边形绘制模式:正反面,填充
}
glEnable(GL_DEPTH_TEST); //开启深度测试
glRotatef(ax, 1, 0, 0);
glRotatef(ay, 0, 1, 0);
draw();
glutSwapBuffers(); //交换缓冲区
}
void init()
{
c->run();
}
int main(int argc, char *argv[])
{
c = new check();
std::thread* th = new std::thread(init);
//init();
glutInit(&argc, argv);//对glut的初始化
glutInitDisplayMode(GLUT_RGBA | GLUT_DEPTH | GLUT_DOUBLE);//初始化显示模式:RGB颜色模型,深度测试,双缓冲
glutInitWindowSize(600, 480);//设置窗口大小
int windowHandle = glutCreateWindow("三维重建");//设置窗口标题
glutMouseFunc(mouse);
glutMotionFunc(mouseMotion);
glutDisplayFunc(redraw);//注册绘制回调函数
glutReshapeFunc(reshape);//注册重绘回调函数
glutKeyboardFunc(key);//注册按键回调函数
glutIdleFunc(idle);//注册全局回调函数:空闲时调用
glutMainLoop();// glut事件处理循环
return 0;
}
生成棋盘图的代码
#include <cv.h>
#include <highgui.h>
int main()
{
CvMat* mat = cvCreateMat(520,520, CV_32F);
for (int i = 0; i < mat->rows; i++) {
float* pData = (float*)(mat->data.ptr + i*mat->step);
for (int j = 0; j < mat->cols; j++) {
if (((i / 40))%2== (j/40)%2) {
*pData = 255;
}
else *pData = 0;
pData++;//指向下一个元素
}
}
cvSaveImage("calibration.jpg",mat);
cvWaitKey(0);
}
最后一段是要预先跑的一段代码,用于对输入的棋盘图片进行建模,代码可以在《learning opencv》一书的源码中找到,在这里还是贴出来,只不过并没有包含需要用的图片,要图片的话也许还是需要去那本书的源码文件夹找一找
#include <cv.h>
#include <highgui.h>
#include <stdio.h>
#include <stdlib.h>
int n_boards = 23; //Will be set by input list
const int board_dt = 10;
int board_w;
int board_h;
void help() {
printf("Calibration from disk. Call convention:\n\n"
" ch11_ex11_1_fromdisk board_w board_h image_list\n\n"
"Where: board_{w,h} are the # of internal corners in the checkerboard\n"
" width (board_w) and height (board_h)\n"
" image_list is space separated list of path/filename of checkerboard\n"
" images\n\n"
"Hit 'p' to pause/unpause, ESC to quit. After calibration, press any other key to step through the images\n\n");
}
int main(int argc, char* argv[]) {
CvCapture* capture;
help();
board_w = 12;
board_h = 12;
int board_n = board_w * board_h;
CvSize board_sz = cvSize(board_w, board_h);
char names[2048];
//cvNamedWindow("Calibration");
//ALLOCATE STORAGE
//图像点 对象点 点计数
CvMat* image_points = cvCreateMat(n_boards*board_n, 2, CV_32FC1);
CvMat* object_points = cvCreateMat(n_boards*board_n, 3, CV_32FC1);
CvMat* point_counts = cvCreateMat(n_boards, 1, CV_32SC1);
//本征方程
//
CvMat* intrinsic_matrix = cvCreateMat(3, 3, CV_32FC1);
CvMat* distortion_coeffs = cvCreateMat(4, 1, CV_32FC1);
IplImage* image = 0;// = cvQueryFrame( capture );
IplImage* gray_image = 0; //for subpixel
CvPoint2D32f* corners = new CvPoint2D32f[board_n];
int corner_count;
int successes = 0;
int step;
for (int frame = 0; frame<n_boards; frame++) {
sprintf(names, "calibration/IMG_0%d.jpg", frame + 191);
if (image) {
cvReleaseImage(&image);
image = 0;
}
image = cvLoadImage(names, 1);
gray_image = cvLoadImage(names, 0);
if (gray_image == 0 && image) { //We'll need this for subpixel accurate stuff
printf("here\n");
gray_image = cvCreateImage(cvGetSize(image), 8, 1);
}
if (!image)
printf("null image\n");
int found = cvFindChessboardCorners(
image,
board_sz,
corners,
&corner_count,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS
);
//使用openCV中的标定板角点检测函数,检测出角点。
//Get Subpixel accuracy on those corners
cvFindCornerSubPix(gray_image, corners, corner_count,
cvSize(11, 11), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
//Draw it
//找到亚像素角度位置,函数:cvFindCornerSubPix
cvDrawChessboardCorners(image, board_sz, corners, corner_count, found);
char n[100];
sprintf(n, "corner%d.jpg", frame + 1);
cvSaveImage(n, image);
cvShowImage("calibration", image);
// If we got a good board, add it to our data
//
printf("%d %d\n", corner_count, board_n);
if (corner_count == board_n) {
step = successes*board_n;
// printf("Found = %d for %s\n",found,names);
for (int i = step, j = 0; j<board_n; ++i, ++j) {
/// CV_MAT_ELEM(*image_points, CvPoint2D32f,0,i) = cvPoint2D32f(corners[j].x,corners[j].y);
/// CV_MAT_ELEM(*object_points,CvPoint3D32f,0,i) = cvPoint3D32f(j/board_w, j%board_w, 0.0f);
CV_MAT_ELEM(*image_points, float, i, 0) = corners[j].x;
CV_MAT_ELEM(*image_points, float, i, 1) = corners[j].y;
CV_MAT_ELEM(*object_points, float, i, 0) = j / board_w;
CV_MAT_ELEM(*object_points, float, i, 1) = j%board_w;
CV_MAT_ELEM(*object_points, float, i, 2) = 0.0f;
}
// CV_MAT_ELEM(*point_counts, int,0,successes) = board_n;
CV_MAT_ELEM(*point_counts, int, successes, 0) = board_n;
successes++;
}
// if( successes == n_boards ) break;
int c = cvWaitKey(15);
if (c == 'p') {
c = 0;
while (c != 'p' && c != 27) {
c = cvWaitKey(250);
}
}
if (c == 27)
return 0;
}
printf("successes = %d, n_boards=%d\n", successes, n_boards);
//ALLOCATE MATRICES ACCORDING TO HOW MANY IMAGES WE FOUND CHESSBOARDS ON
/// CvMat* image_points2 = cvCreateMat(1,successes*board_n,CV_32FC2);
/// CvMat* object_points2 = cvCreateMat(1,successes*board_n,CV_32FC3);
/// CvMat* point_counts2 = cvCreateMat(1,successes,CV_32SC1);
CvMat* object_points2 = cvCreateMat(successes*board_n, 3, CV_32FC1);
CvMat* image_points2 = cvCreateMat(successes*board_n, 2, CV_32FC1);
CvMat* point_counts2 = cvCreateMat(successes, 1, CV_32SC1);
//TRANSFER THE POINTS INTO THE CORRECT SIZE MATRICES
for (int i = 0; i<successes*board_n; ++i) {
/// CV_MAT_ELEM(*image_points2, CvPoint2D32f,0,i) = CV_MAT_ELEM(*image_points, CvPoint2D32f,0,i);
/// CV_MAT_ELEM(*object_points2,CvPoint3D32f,0,i) = CV_MAT_ELEM(*object_points,CvPoint3D32f,0,i);
CV_MAT_ELEM(*image_points2, float, i, 0) = CV_MAT_ELEM(*image_points, float, i, 0);
CV_MAT_ELEM(*image_points2, float, i, 1) = CV_MAT_ELEM(*image_points, float, i, 1);
CV_MAT_ELEM(*object_points2, float, i, 0) = CV_MAT_ELEM(*object_points, float, i, 0);
CV_MAT_ELEM(*object_points2, float, i, 1) = CV_MAT_ELEM(*object_points, float, i, 1);
CV_MAT_ELEM(*object_points2, float, i, 2) = CV_MAT_ELEM(*object_points, float, i, 2);
}
for (int i = 0; i<successes; ++i) {
/// CV_MAT_ELEM(*point_counts2,int,0, i) = CV_MAT_ELEM(*point_counts, int,0,i);
CV_MAT_ELEM(*point_counts2, int, i, 0) = CV_MAT_ELEM(*point_counts, int, i, 0);
}
cvReleaseMat(&object_points);
cvReleaseMat(&image_points);
cvReleaseMat(&point_counts);
//cvWaitKey();//Now we have to reallocate the matrices
// return 0;
// At this point we have all of the chessboard corners we need.
//
// Initialize the intrinsic matrix such that the two focal
// lengths have a ratio of 1.0
//
CV_MAT_ELEM(*intrinsic_matrix, float, 0, 0) = 1.0f;
CV_MAT_ELEM(*intrinsic_matrix, float, 1, 1) = 1.0f;
printf("cvCalibrateCamera2\n");
cvCalibrateCamera2(
object_points2,
image_points2,
point_counts2,
cvGetSize(image),
intrinsic_matrix,
distortion_coeffs,
NULL,
NULL,
0//CV_CALIB_FIX_ASPECT_RATIO
);
// Save our work
cvSave("Intrinsics.xml", intrinsic_matrix);
cvSave("Distortion.xml", distortion_coeffs);
// Load test
CvMat *intrinsic = (CvMat*)cvLoad("Intrinsics.xml");
CvMat *distortion = (CvMat*)cvLoad("Distortion.xml");
// Build the undistort map which we will use for all
// subsequent frames.
//
// Just run the camera to the screen, now only showing the undistorted
// image.
//
//rewind(fptr);
//cvNamedWindow("Undistort");
printf("\n\nPress any key to step through the images, ESC to quit\n\n");
for (int frame = 0; frame<n_boards; frame++) {
sprintf(names, "calibration/IMG_0%d.jpg", frame + 191);
if (image) {
cvReleaseImage(&image);
image = 0;
}
image = cvLoadImage(names);
IplImage* mapx = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
IplImage* mapy = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
printf("cvInitUndistortMap\n");
cvInitUndistortMap(
intrinsic,
distortion,
mapx,
mapy
);
char n[100];
sprintf(n, "distort%d.jpg", frame + 1);
IplImage *t = cvCloneImage(image);
cvRemap(t, image, mapx, mapy);
cvReleaseImage(&t);
cvSaveImage(n, image);
}
system("pause");
return 0;