| 1 | #include "sensor_simulator.h" |
| 2 | |
| 3 | cv::Mat SensorSimulator::renderDepthImage(){ |
| 4 | |
| 5 | cv::Mat depth_image(image_height, image_width, CV_32FC1, cv::Scalar(std::numeric_limits<float>::max())); |
| 6 | Eigen::Matrix3f R_wc = quat.toRotationMatrix(); |
| 7 | Eigen::Matrix3f R_cw = R_wc.inverse(); |
| 8 | |
| 9 | auto start = std::chrono::high_resolution_clock::now(); |
| 10 | #pragma omp parallel for |
| 11 | for (int v = 0; v < image_height; ++v) { |
| 12 | for (int u = 0; u < image_width; ++u) { |
| 13 | // 计算射线方向（图像平面坐标系） |
| 14 | float y = -(u - cx) / fx; |
| 15 | float z = -(v - cy) / fy; |
| 16 | float x = 1.0f; |
| 17 | Eigen::Vector3f d(x, y, z); |
| 18 | d.normalize(); |
| 19 | |
| 20 | // 转换到世界坐标系下 |
| 21 | Eigen::Vector3f ray_direction = R_wc * d; // 考虑相机旋转 |
| 22 | Eigen::Vector3f ray_origin = pos; // 相机的位置 |
| 23 | |
| 24 | // 使用Octree查找射线方向上最近的点 |
| 25 | std::vector<int> pointIdxVec; |
| 26 | if (octree->getIntersectedVoxelIndices(ray_origin, ray_direction, pointIdxVec, 1)) { |
| 27 | pcl::PointXYZ closest_point = cloud->points[pointIdxVec[0]]; |
| 28 | Eigen::Vector3f point_in_world = closest_point.getVector3fMap(); |
| 29 | Eigen::Vector3f closest_point_camera = R_cw * (point_in_world - pos); |
| 30 | float distance = closest_point_camera(0); |
| 31 | if (distance < 0) distance = 0; |
| 32 | if (distance > max_depth_dist) distance = max_depth_dist; |
| 33 | if (normalize_depth) distance = distance / max_depth_dist; |
| 34 | depth_image.at<float>(v, u) = distance; |
| 35 | } |
| 36 | } |
| 37 | } |
| 38 | auto end = std::chrono::high_resolution_clock::now(); |
| 39 | std::chrono::duration<double> elapsed = end - start; |
| 40 | // std::cout << "生成图像耗时: " << elapsed.count() << " 秒" << std::endl; // 输出耗时 |
| 41 | |
| 42 | // 将无效值设置为0 |
| 43 | for (int v = 0; v < image_height; ++v) { |
| 44 | for (int u = 0; u < image_width; ++u) { |
| 45 | if (depth_image.at<float>(v, u) == std::numeric_limits<float>::max()) |
| 46 | depth_image.at<float>(v, u) = max_depth_dist; |
| 47 | } |
| 48 | } |
| 49 | |
| 50 | return depth_image; |
| 51 | } |
| 52 | |
| 53 | pcl::PointCloud<pcl::PointXYZ> SensorSimulator::renderLidarPointcloud() { |
| 54 | Eigen::Matrix3f R_wc = quat.toRotationMatrix(); |