激光雷达和相机联合标定之 |
您所在的位置：网站首页 › velodyne坐标系 › 激光雷达和相机联合标定之 |
激光雷达和相机联合标定之
|
1. 概述
1.该方法适用于相机和激光雷达朝向方向相同或者近似相同的状态，相机和激光雷达之间的R矩阵较小，主要标定误差为T矩阵。 /ps：可以先将激光雷达旋转到与相机安装坐标系相似的世界坐标系下，再使用该方法进行标定。/ 2.该方法分为两个主要步骤，T矩阵标定；R|T矩阵联合标定 3.其中T矩阵标定基于相机的小孔成像原理中，世界坐标系下物体与像素坐标系之间的相似三角形关系获得。 4.R|T矩阵联合标定通过在小范围内迭代求解获得。 2.T矩阵标定 2.1 图像特征圆提取 2.1.1 边缘检测源代码中提供了3种不同的边缘检测算法： sobel边缘检测 cv::Mat ImageEdge::computeEdgeImage() { cv::Mat gray_img; if(this->m_img.channels()>1) { gray_img = cv::Mat(this->m_img.size(), CV_8UC1); cv::cvtColor(this->m_img, gray_img, CV_BGR2GRAY); } else { this->m_img.copyTo(gray_img); } cv::Mat edges; cv::Mat grad_x, grad_y; cv::Mat abs_grad_x, abs_grad_y; cv::Sobel(gray_img, grad_x, CV_16S, 1, 0, 3); cv::convertScaleAbs(grad_x, abs_grad_x); cv::Sobel(gray_img, grad_y, CV_16S, 0, 1, 3); cv::convertScaleAbs(grad_y, abs_grad_y); cv::addWeighted(abs_grad_x, 0.5, abs_grad_y, 0.5, 0, edges); if(m_write_img) { cv::imwrite(m_imgsave_path+"/edge_img.jpg",edges); } return edges; } 自适应阈值分割检测 cv::Mat ImageEdge::segmetation() { cv::Mat src_gray; if(this->m_img.channels() > 1) { cv::cvtColor(m_img, src_gray, CV_BGR2GRAY); } else { src_gray = m_img; } cv::resize(src_gray, src_gray, cv::Size(src_gray.rows, src_gray.cols)); cv::Mat seg; cv::adaptiveThreshold(src_gray, seg, 1, CV_ADAPTIVE_THRESH_GAUSSIAN_C, CV_THRESH_BINARY, 7 , 5); if(m_write_img) { cv::imwrite(m_imgsave_path+"/segment.jpg",seg*255); } return seg; } IDT边缘检测–未完全理解 cv::Mat ImageEdge::computeIDTEdgeImage(cv::Mat& edge_img) { cv::Mat edges; edge_img.convertTo(edges, CV_32FC1); cv::Mat default_edges = edges; cv::Mat weights = cv::Mat::ones(edges.size(), CV_32FC1); cv::Mat new_weights = cv::Mat::ones(edges.size(), CV_32FC1); cv::Mat new_edges = cv::Mat::zeros(edges.size(), CV_32FC1); int weights_middle = m_distance_weights.cols/2; for(int rows_cols = 0; rows_cols for(int x=0;x cv::imwrite(m_imgsave_path+"/idt_edge_img.jpg", idt_edge_img.mul(cv::Scalar::all(255.0))); } return idt_edge_img; }基于三种不同的边缘检测算法，获得的边缘检测图如下： row_image 其中pcl定义新数据类型方法如下： struct Point { PCL_ADD_POINT4D; float intensity; uint16_t ring; float range; EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment // PCL_MAKE_ALIGNED_OPERATOR_NEW }EIGEN_ALIGN16; POINT_CLOUD_REGISTER_POINT_STRUCT(Point, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (uint16_t, ring, ring)) 点云的坐标系为，垂直于激光雷达发射面，沿着行人行走的方向为Z轴，垂直于Z轴的水平方向为x轴，高度方向为y轴。基于相机的内参，判断存在于点云内部的点 pcl::PointCloud visible_cloud; this->project(projection_matrix, cv::Rect(0, 0, image_width, image_height), &visible_cloud); PointCloudEdge visible_scan(visible_cloud); 2.2.2 检测步骤 边缘检测 基于单圈内相邻点的距离差，计算该点是否为突变点，在同一平面上的点，相差为0，两个平面的边缘线处，相邻点距离差较大。然后对计算的差值进行归一化。 void PointCloudEdge::intensityByDiff(Processing processing) { std::vector rings = getRings(); for(std::vector::iterator ring = rings.begin(); ring continue; } (*ring->begin())->intensity = 0; (*(ring->end()-1))->intensity = 0; for(std::vector::iterator pt=ring->begin()+1; ptend()-1; pt++) { prev = *(pt-1); next = *(pt+1); switch(processing) { case Processing::DISTORTIONS: (*pt)->intensity = std::max(std::max(prev->range-(*pt)->range, next->range-(*pt)->range),float(0)); break; case Processing::INTENSITY_EDGES: (*pt)->intensity = std::max(std::max(prev->intensity-(*pt)->intensity, next->intensity-(*pt)->intensity),float(0)); break; default: std::cout // continue; // } // pcl::PointCloud plane_no_line; // removeLines(*plane_ptr, line_inliers, plane_no_line); // m_plane=plane_no_line; // } 基于ransac的特征圆检测 radiuses.clear(); std::vector centers; std::vector inliers_indicies; pcl::PointCloud *four_spheres=new pcl::PointCloud(); float tolerance = 0.08; for(int i=0;i continue; } Eigen::VectorXf coeficients; ransac_sphere.getModelCoefficients(coeficients); // std::cout std::vector near_indexes; near_indexes.push_back(std::pair(0,1)); near_indexes.push_back(std::pair(1,3)); near_indexes.push_back(std::pair(3,2)); near_indexes.push_back(std::pair(2,0)); bool res = true; for(std::vector::iterator ne=near_indexes.begin();ne std::vector translation(3, 0); enum INDEX { X=0, Y=1, Z=2 }; float focal_len = projection.at(0,0); // std::cout img = ImageEdge(img.computeIDTEdgeImage()); float x_min = x_rough - max_translation; float y_min = y_rough - max_translation; float z_min = z_rough - max_translation; float x_rot_min = - max_rotation; float y_rot_min = - max_rotation; float z_rot_min = - max_rotation; float step_transl = max_translation*2/(steps -1); float step_rot = max_rotation*2/(steps-1); PointCloudEdge transformed = scan.transform(x_rough, y_rough, z_rough, 0, 0, 0); float rough_val = Similarity::edgeSimilarity(img, transformed, P); std::cout float z=z_min; for(size_t zi=0; zi float y_r = y_rot_min; for(size_t y_ri=0; y_ri PointCloudEdge transformed = scan.transform(x,y,z,x_r,y_r,z_r); float value = Similarity::edgeSimilarity(img, transformed, P); Calibration6DoF calibration(x,y,z,x_r,y_r,z_r,value); if(value>best_calibration.value) { best_calibration.set(x,y,z,x_r,y_r,z_r,value); counter_best++; cv::Mat final_img = img.getImage().clone(); for(pcl::PointCloud::iterator pt = transformed.begin(); pt // cv::Point xy = Velodyne::Velodyne::project(*pt, P); // cv::circle(final_img, cv::Point2d(xy.x,xy.y), 3,cv::Scalar(255,0,0),2,8); // } // std::string filename = "/home/xxy/xxy/202202/ros_test/img/"+std::to_string(cnt++)+"_"+std::to_string(value)+"_mid.jpg"; // cv::imwrite(filename, final_img); if (value > rough_val) { average += calibration; counter++; cout cv::Point2i m_point_topleft= cv::Point2i(250,120); cv::Point2i m_point_bottomright= cv::Point2i(500,360); cv::Rect frame(cv::Point(0,0), img.size()); float CC=0; for(pcl::PointCloud::iterator pt = scan.begin(); pt // std::cout0.1220090645915436, -0.07107613516559942, -0.009396424500619524, 0.05404222930344704, 0.07256047312087824}; // cv::Rodrigues(rotM, rvec); cv::Mat distortion_coefficients = cv::Mat(5, 1, CV_64FC1, distCoeffD); cv::Mat camera_matrix = cv::Mat(3,3,CV_64FC1,camD); cv::solvePnP(centers3d,centers2d,camera_matrix, distortion_coefficients,rvec, tvec); cv::Rodrigues(rvec, rotM); std::cout |
sobel边缘检测 
idt边缘检测 
自适应阈值分割边缘检测 
【本文地址】
公司简介
联系我们
今日新闻 |
点击排行 |
|
推荐新闻 |
图片新闻 |
|
专题文章 |