下面是从CSDN下载的一个程序，用到了其他的一些库，大致实现了HALCON示例中的线结构光三维重建的功能，下面是完整代码，具体解释见注释
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/cloud_viewer.h>
using namespace cv;
using namespace std;
void calRealPoint(std::vector<std::vector<cv::Point3f>>& obj, int boardwidth, int boardheight, int imgNumber, float squaresize)
{
// Mat imgpoint(boardheight, boardwidth, CV_32FC3,Scalar(0,0,0));
std::vector<cv::Point3f> imgpoint;
for (int rowIndex = 0; rowIndex < boardheight; rowIndex++)
{
for (int colIndex = 0; colIndex < boardwidth; colIndex++)
{
// imgpoint.at<Vec3f>(rowIndex, colIndex) = Vec3f(rowIndex * squaresize, colIndex*squaresize, 0);
imgpoint.push_back(cv::Point3f((float)colIndex * squaresize - (boardwidth / 2 * squaresize), (float)rowIndex * squaresize - (boardheight / 2 * squaresize), 0));
}
}
for (int imgIndex = 0; imgIndex < imgNumber; imgIndex++)
{
obj.push_back(imgpoint);
}
}
//像素位置、内参、R、T==》》世界坐标
Point3f getWorldPoints(Point2f &inPoints, Mat &rvec, Mat &tvec, Mat &cameraMatrix)
{
//initialize parameter
Mat rotationMatrix;//3*3
Rodrigues(rvec, rotationMatrix);
double zConst = 0;//实际坐标系的距离，若工作平面与相机距离固定可设置为0
double s;
//获取图像坐标
cv::Mat imagePoint = (Mat_<double>(3, 1) << double(inPoints.x), double(inPoints.y), 1);
// cv::Mat::ones(3, 1, cv::DataType<double>::type); //u,v,1
// imagePoint.at<double>(0, 0) = inPoints.x;
// imagePoint.at<double>(1, 0) = inPoints.y;
//计算比例参数S
cv::Mat tempMat, tempMat2;
tempMat = rotationMatrix.inv() * cameraMatrix.inv() * imagePoint;
tempMat2 = rotationMatrix.inv() * tvec;
s = zConst + tempMat2.at<double>(2, 0);
s /= tempMat.at<double>(2, 0);
//计算世界坐标
Mat wcPoint = rotationMatrix.inv() * (s * cameraMatrix.inv() * imagePoint - tvec);
Point3f worldPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0), wcPoint.at<double>(2, 0));
return worldPoint;
}
//Ax+by+cz=D
void cvFitPlane(const CvMat* points, float* plane) {
// Estimate geometric centroid.
int nrows = points->rows;
int ncols = points->cols;
int type = points->type;
CvMat* centroid = cvCreateMat(1, ncols, type);
cvSet(centroid, cvScalar(0));
for (int c = 0; c < ncols; c++) {
for (int r = 0; r < nrows; r++)
{
centroid->data.fl[c] += points->data.fl[ncols*r + c];
}
centroid->data.fl[c] /= nrows;
}
// Subtract geometric centroid from each point.
CvMat* points2 = cvCreateMat(nrows, ncols, type);
for (int r = 0; r < nrows; r++)
for (int c = 0; c < ncols; c++)
points2->data.fl[ncols*r + c] = points->data.fl[ncols*r + c] - centroid->data.fl[c];
// Evaluate SVD of covariance matrix.
CvMat* A = cvCreateMat(ncols, ncols, type);
CvMat* W = cvCreateMat(ncols, ncols, type);
CvMat* V = cvCreateMat(ncols, ncols, type);
cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T);
cvSVD(A, W, NULL, V, CV_SVD_V_T);
// Assign plane coefficients by singular vector corresponding to smallest singular value.
plane[ncols] = 0;
for (int c = 0; c < ncols; c++) {
plane[c] = V->data.fl[ncols*(ncols - 1) + c];
plane[ncols] += plane[c] * centroid->data.fl[c];
}
// Release allocated resources.
cvReleaseMat(¢roid);
cvReleaseMat(&points2);
cvReleaseMat(&A);
cvReleaseMat(&W);
cvReleaseMat(&V);
}
//主函数
int main()
{
//检测圆形标定板
cv::Mat rgbImage, grayImage;
std::vector<cv::Point2f> corner;
std::vector<std::vector<cv::Point2f>> imagePoint;
for (int i = 1; i <= 20; i++)
{
string path = "img/connection_rod_calib_" + to_string(i / 10) + to_string(i % 10) + ".png";
rgbImage = cv::imread(path, CV_LOAD_IMAGE_COLOR);
cv::cvtColor(rgbImage, grayImage, CV_BGR2GRAY);
bool isFind;
isFind = findCirclesGrid(grayImage, cv::Size(7, 7), corner);
if (isFind)
{
//cornerSubPix(grayImage, corner, cv::Size(7, 7), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));
drawChessboardCorners(rgbImage, cv::Size(7, 7), corner, isFind);
imagePoint.push_back(corner);
}
}
//标准图用于投影变换
std::vector<std::vector<cv::Point3f>> objRealPoint;
calRealPoint(objRealPoint, 7, 7, 20, 3);
//标定
cv::Mat cameraMatrix, distCoeff;
vector<Mat> rvecsMat;
vector<Mat> tvecsMat;
float rms = calibrateCamera(objRealPoint, imagePoint, cv::Size(rgbImage.cols, rgbImage.rows), cameraMatrix, distCoeff, rvecsMat, tvecsMat, CV_CALIB_FIX_K3);
std::vector<cv::Point3f> Points3d_19;
//第19张标定图激光图==》》三维坐标(Z坐标为0)
//cameraMatrix, distCoeff, rvecsMat[18], tvecsMat[18]
Mat rod_lightline_19 = imread("img/connection_rod_lightline_019.png", CV_LOAD_IMAGE_GRAYSCALE);
threshold(rod_lightline_19, rod_lightline_19, 80, 255, THRESH_BINARY);
for (size_t i = 0; i < rod_lightline_19.cols; i++)
{
int sum = 0; int num = 0;
for (size_t j = 0; j < rod_lightline_19.rows; j++)
{
if (rod_lightline_19.at<uchar>(j, i) == 255)
{
sum += j;
num++;
}
}
if (num == 0)
continue;
Points3d_19.push_back(getWorldPoints(Point2f(i, 1.0*sum / num), rvecsMat[18], tvecsMat[18], cameraMatrix));
}
std::vector<cv::Point3f> Points3d_20;
//第20张标定图激光图==》》三维坐标(Z坐标为0)需要转到19张图坐标系下
//cameraMatrix, distCoeff, rvecsMat[19], tvecsMat[19]
Mat rod_lightline_20 = imread("img/connection_rod_lightline_020.png", CV_LOAD_IMAGE_GRAYSCALE);
threshold(rod_lightline_20, rod_lightline_20, 80, 255, THRESH_BINARY);
for (size_t i = 0; i < rod_lightline_20.cols; i++)
{
int sum = 0; int num = 0;
for (size_t j = 0; j < rod_lightline_20.rows; j++)
{
if (rod_lightline_20.at<uchar>(j, i) == 255)
{
sum += j;
num++;
}
}
if (num == 0)
continue;
Points3d_20.push_back(getWorldPoints(Point2f(i, 1.0*sum / num), rvecsMat[19], tvecsMat[19], cameraMatrix));
}
std::vector<cv::Point3f> Points3d_20to19;
for (size_t i = 0; i < Points3d_20.size(); i++)
{
Mat Point3d_mat = (Mat_<double>(3, 1) << double(Points3d_20[i].x), double(Points3d_20[i].y), double(Points3d_20[i].z));
Mat rotationMatrix1;//3*3
Rodrigues(rvecsMat[19], rotationMatrix1);
Mat rotationMatrix2;//3*3
Rodrigues(rvecsMat[18], rotationMatrix2);
Mat Point3d_to19_mat = rotationMatrix2*rotationMatrix1.inv()*(Point3d_mat - tvecsMat[19]) + tvecsMat[18];
Points3d_20to19.push_back(Point3f(Point3d_to19_mat.at<double>(0, 0), Point3d_to19_mat.at<double>(1, 0), Point3d_to19_mat.at<double>(2, 0)));
}
//拟合激光平面
CvMat*points_mat = cvCreateMat(Points3d_19.size() + Points3d_20to19.size(), 3, CV_32FC1);//定义用来存储需要拟合点的矩阵
for (int i = 0; i < Points3d_19.size(); ++i)
{
points_mat->data.fl[i * 3 + 0] = Points3d_19[i].x;//矩阵的值进行初始化 X的坐标值
points_mat->data.fl[i * 3 + 1] = Points3d_19[i].y;// Y的坐标值
points_mat->data.fl[i * 3 + 2] = Points3d_19[i].z;// Z的坐标值</span>
}
for (int i = 0; i < Points3d_20to19.size(); ++i)
{
points_mat->data.fl[Points3d_19.size() * 3 + i * 3 + 0] = Points3d_20to19[i].x;//矩阵的值进行初始化 X的坐标值
points_mat->data.fl[Points3d_19.size() * 3 + i * 3 + 1] = Points3d_20to19[i].y;// Y的坐标值
points_mat->data.fl[Points3d_19.size() * 3 + i * 3 + 2] = Points3d_20to19[i].z;// Z的坐标值</span>
}
float line_plane[4] = {
0 };//定义用来储存平面参数的数组
cvFitPlane(points_mat, line_plane);//调用方程 line_plane[2]=-0.23太小
//line_plane[2] = -0.5;
//确定履带位移：根据内参+像素点 => 计算RT
std::vector<cv::Point2f> corner_1, corner_20;
Mat caltab_at_position_1 = imread("img/caltab_at_position_1.png", CV_LOAD_IMAGE_GRAYSCALE);
bool isFind_1 = findCirclesGrid(caltab_at_position_1, cv::Size(7, 7), corner_1);
drawChessboardCorners(caltab_at_position_1, cv::Size(7, 7), corner_1, isFind_1);
Mat caltab_at_position_20 = imread("img/caltab_at_position_2.png", CV_LOAD_IMAGE_GRAYSCALE);
bool isFind_20 = findCirclesGrid(caltab_at_position_20, cv::Size(7, 7), corner_20);
drawChessboardCorners(caltab_at_position_20, cv::Size(7, 7), corner_20, isFind_20);
std::vector<std::vector<cv::Point3f>> objRealPoint2;
calRealPoint(objRealPoint2, 7, 7, 2, 3);
std::vector<std::vector<cv::Point2f>> imagePoint2;
imagePoint2.push_back(corner_1);
imagePoint2.push_back(corner_20);
vector<Mat> rvecsMat2;
rvecsMat2.resize(2);
vector<Mat> tvecsMat2;
tvecsMat2.resize(2);
solvePnP(objRealPoint2[0], imagePoint2[0], cameraMatrix, distCoeff, rvecsMat2[0], tvecsMat2[0], false, SOLVEPNP_DLS);
solvePnP(objRealPoint2[1], imagePoint2[1], cameraMatrix, distCoeff, rvecsMat2[1], tvecsMat2[1], false, SOLVEPNP_DLS);
Mat Point3d_mat = (Mat_<double>(3, 1) << 0.0, 0.0, 0.0);
Mat rotationMatrix1;//3*3
Rodrigues(rvecsMat2[0], rotationMatrix1);
Mat rotationMatrix20;//3*3
Rodrigues(rvecsMat2[1], rotationMatrix20);
Mat rotationMatrix19;//3*3
Rodrigues(rvecsMat[18], rotationMatrix19);//标杆
Mat Point3d_1to19_mat = rotationMatrix19*rotationMatrix1.inv()*(Point3d_mat - tvecsMat2[0]) + tvecsMat[18];
Mat Point3d_20to19_mat = rotationMatrix19*rotationMatrix20.inv()*(Point3d_mat - tvecsMat2[1]) + tvecsMat[18];
Mat move_steps = Point3d_20to19_mat - Point3d_1to19_mat;//1-20移动距离
Mat move_step = move_steps / 19;//单步移动距离
//计算
std::vector<cv::Point3f> Points3d_all;
for (size_t k = 1; k <= 290; k++)
{
string path = "img/connection_rod_" + to_string(k / 100) + to_string(k / 10 % 10) + to_string(k % 10) + ".png";
Mat image = cv::imread(path, CV_LOAD_IMAGE_GRAYSCALE);
threshold(image, image, 80, 255, THRESH_BINARY);
for (size_t i = 0; i < image.cols; i++)
{
int sum = 0; int num = 0;
for (size_t j = 0; j < image.rows; j++)
{
if (image.at<uchar>(j, i) == 255)
{
sum += j;
num++;
}
}
if (num == 0)
continue;
cv::Point3f Points3d = getWorldPoints(Point2f(i, 1.0*sum / num), rvecsMat[18], tvecsMat[18], cameraMatrix);
Points3d.z = (line_plane[3] - line_plane[0] * Points3d.x - line_plane[1] * Points3d.y) / line_plane[2];
Points3d += Point3f((k - 1)*move_step.at<double>(0, 0), (k - 1)* move_step.at<double>(1, 0), (k - 1)*move_step.at<double>(2, 0));
Points3d_all.push_back(Points3d);
}
//imshow("image", image);
//waitKey(10);
}
//显示
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); // 创建点云（指针）
(*cloud).points.resize(Points3d_all.size());
for (size_t i = 0; i < Points3d_all.size(); i++)
{
(*cloud).points[i].x = Points3d_all[i].x;
(*cloud).points[i].y = Points3d_all[i].y;
(*cloud).points[i].z = Points3d_all[i].z;
}
pcl::io::savePLYFileASCII("point3d.ply", *cloud);
pcl::visualization::CloudViewer viewer("ply viewer");
viewer.showCloud(cloud);
while (!viewer.wasStopped())
{
}
}