单目激光线扫3D三维坐标计算方法总结

• 此方法总结归纳来自于halcon的官方例程

• 官方图片位置 C:\Users\Public\Documents\MVTec\HALCON-18.05-Progress\examples\images\sheet_of_light

1、标定确定内参、确定激光平面

• 数据准备：多张标定板标定图、两组以上的标定板与激光线图。

2、确定履带位移pose

• 数据准备：两个位置标定板图

3、计算3D坐标

• 数据准备：采集N张线激光图像

基于opencv、pcl单目线激光三维重建完整代码下载：https://download.csdn.net/download/qq_36638362/13082209 

MFC应用程序下载（说明在文本下方）：https://download.csdn.net/download/qq_36638362/13129186

 配置教程：https://download.csdn.net/download/qq_36638362/10809976

效果图：

opencv部分实现代码（含有部分PCL代码）：

#include 
#include 
#include 
#include 
#include 
#include 
using namespace cv;
using namespace std;void calRealPoint(std::vector>& obj, int boardwidth, int boardheight, int imgNumber, float squaresize)
{//	Mat imgpoint(boardheight, boardwidth, CV_32FC3,Scalar(0,0,0));std::vector imgpoint;for (int rowIndex = 0; rowIndex < boardheight; rowIndex++){for (int colIndex = 0; colIndex < boardwidth; colIndex++){//	imgpoint.at(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 parameterMat rotationMatrix;//3*3Rodrigues(rvec, rotationMatrix);double zConst = 0;//实际坐标系的距离，若工作平面与相机距离固定可设置为0double s;//获取图像坐标cv::Mat imagePoint = (Mat_(3, 1) << double(inPoints.x), double(inPoints.y), 1);// cv::Mat::ones(3, 1, cv::DataType::type); //u,v,1// imagePoint.at(0, 0) = inPoints.x;// imagePoint.at(1, 0) = inPoints.y;//计算比例参数Scv::Mat tempMat, tempMat2;tempMat = rotationMatrix.inv() * cameraMatrix.inv() * imagePoint;tempMat2 = rotationMatrix.inv() * tvec;s = zConst + tempMat2.at(2, 0);s /= tempMat.at(2, 0);//计算世界坐标Mat wcPoint = rotationMatrix.inv() * (s * cameraMatrix.inv() * imagePoint - tvec);Point3f worldPoint(wcPoint.at(0, 0), wcPoint.at(1, 0), wcPoint.at(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 corner;std::vector> 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> objRealPoint;calRealPoint(objRealPoint, 7, 7, 20, 3);//标定cv::Mat cameraMatrix, distCoeff;vector rvecsMat;vector tvecsMat;float rms = calibrateCamera(objRealPoint, imagePoint, cv::Size(rgbImage.cols, rgbImage.rows), cameraMatrix, distCoeff, rvecsMat, tvecsMat, CV_CALIB_FIX_K3);std::vector 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(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 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(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 Points3d_20to19;for (size_t i = 0; i < Points3d_20.size(); i++){Mat Point3d_mat = (Mat_(3, 1) << double(Points3d_20[i].x), double(Points3d_20[i].y), double(Points3d_20[i].z));Mat rotationMatrix1;//3*3Rodrigues(rvecsMat[19], rotationMatrix1);Mat rotationMatrix2;//3*3Rodrigues(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(0, 0), Point3d_to19_mat.at(1, 0), Point3d_to19_mat.at(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的坐标值}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的坐标值}float line_plane[4] = { 0 };//定义用来储存平面参数的数组 cvFitPlane(points_mat, line_plane);//调用方程 line_plane[2]=-0.23太小//line_plane[2] = -0.5;//确定履带位移：根据内参+像素点 => 计算RTstd::vector 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> objRealPoint2;calRealPoint(objRealPoint2, 7, 7, 2, 3);std::vector> imagePoint2;imagePoint2.push_back(corner_1);imagePoint2.push_back(corner_20);vector rvecsMat2;rvecsMat2.resize(2);vector 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_(3, 1) << 0.0, 0.0, 0.0);Mat rotationMatrix1;//3*3Rodrigues(rvecsMat2[0], rotationMatrix1);Mat rotationMatrix20;//3*3Rodrigues(rvecsMat2[1], rotationMatrix20);Mat rotationMatrix19;//3*3Rodrigues(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 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(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(0, 0), (k - 1)* move_step.at(1, 0), (k - 1)*move_step.at(2, 0));Points3d_all.push_back(Points3d);}//imshow("image", image);//waitKey(10);}//显示pcl::PointCloud::Ptr cloud(new pcl::PointCloud); // 创建点云（指针）(*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()){}
}

MFC软件 https://download.csdn.net/download/qq_36638362/13129186

下面的测试本地图片作为例子介绍。支持usb相机标定和实时检测。

1.标定相机：标定，并将19幅图右击作为标杆

2.标定激光平面

3.标定履带位移参数

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！