利用std::sort将cv::Point和pcl::PointXYZ按自定义方式排序

本文介绍C++中使用std::sort进行自定义排序的方法,包括通过比较函数、lambda表达式及运算符重载实现复杂数据结构的排序。

自定义排序谓词

std::sort中的比较函数compare要声明为静态成员函数或全局函数,不能作为普通成员函数,否则会报错。因为:非静态成员函数是依赖于具体对象的,而std::sort这类函数是全局的,因此无法在std::sort中调用非静态成员函数。

//对点的坐标按x从小到大排序,若x值相同则按y从小到大排序
bool compare(const cv::Point pt1, const cv::Point pt2)
{
	return pt1.x < pt2.x;
}
//省略赋值操作
std::vector<cv::Point> P;
std::sort(P.begin(), P.end(), compare); 

利用lambda表达式

C++11引入了lambda,允许inline函数的定义式被用作一个参数,或是一个local对象。

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
//省略赋值操作 按x坐标值从小到大排序
std::sort(cloud->begin(), cloud->end(),
 [](pcl::PointXYZ pt1, pcl::PointXYZ pt2) {return pt1.x < pt2.x; });

通过运算符重载

设有自定义结构体类型mypoint,如下:

struct mypoint
{
	float x;
	float y;
};

重载“<”运算符:

//按x坐标值从小到大排序
bool operator<(const mypoint& pt1, const mypoint& pt2)
{
	return pt1.x < pt2.x;
}

调用std::sort排序:

//省略赋值操作
std::vector<mypoint> vec;
std::sort(vec.begin(), vec.end());
#include "PointCloudProjection.h" #include <pcl/filters/extract_indices.h> #include <pcl/common/transforms.h> #include <pcl/io/pcd_io.h> #include <fstream> #include <stdexcept> #include <cmath> #include <algorithm> #include <iostream> #include "json.hpp" using json = nlohmann::json; // 移除 using namespace Internal; 避免作用域冲突 // 内部类方法实现(保持不变) std::vector<int> Internal::OrientedBBox::get_points_in_box(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud) const { std::vector<int> inside_indices; pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>()); pcl::transformPointCloud(*cloud, *transformed_cloud, this->transform); constexpr float n = 2.0f; const float half_x = this->extent[0] / 2.0f * n; const float half_y = this->extent[1] / 2.0f * n; const float half_z = this->extent[2] / 2.0f * n; for (size_t i = 0; i < transformed_cloud->points.size(); ++i) { const auto& pt = transformed_cloud->points[i]; if (std::fabs(pt.x) <= half_x && std::fabs(pt.y) <= half_y && std::fabs(pt.z) <= half_z) { inside_indices.push_back(static_cast<int>(i)); } } std::cout << "------------------------》OBB内筛选到的点数量:" << inside_indices.size() << std::endl; return inside_indices; } std::vector<Eigen::Vector3f> Internal::OrientedBBox::get_corners() const { std::vector<Eigen::Vector3f> corners; const Eigen::Vector3f half_ext = this->extent / 2.0f; std::vector<Eigen::Vector3f> local_corners = { {half_ext.x(), half_ext.y(), half_ext.z()}, {half_ext.x(), half_ext.y(), -half_ext.z()}, {half_ext.x(), -half_ext.y(), half_ext.z()}, {half_ext.x(), -half_ext.y(), -half_ext.z()}, {-half_ext.x(), half_ext.y(), half_ext.z()}, {-half_ext.x(), half_ext.y(), -half_ext.z()}, {-half_ext.x(), -half_ext.y(), half_ext.z()}, {-half_ext.x(), -half_ext.y(), -half_ext.z()} }; const Eigen::Affine3f local_to_global = this->transform.inverse(); for (const auto& lc : local_corners) { corners.push_back(local_to_global * lc); } return corners; } Eigen::Matrix3f Internal::OrientedBBox::get_axes() const { return this->transform.inverse().linear(); } Eigen::Affine3f Internal::OrientedBBox::get_global_to_local() const { return this->transform; } Eigen::Affine3f Internal::OrientedBBox::get_local_to_global() const { return this->transform.inverse(); } // 获取指定面的顶点中心(全局坐标) bool Internal::OrientedBBox::get_face_info(FaceType face, std::vector<Eigen::Vector3f>& out_vertices, Eigen::Vector3f& out_center) const { out_vertices.reserve(4); Eigen::Vector3f half_ext = extent / 2.0f; Eigen::Affine3f local_to_global = get_local_to_global(); // 局部→全局变换 // 局部坐标系下的面顶点(顺时针顺序) std::vector<Eigen::Vector3f> local_vertices; switch (face) { case FaceType::X_POS: local_vertices = { {half_ext.x(), half_ext.y(), half_ext.z()}, {half_ext.x(), half_ext.y(), -half_ext.z()}, {half_ext.x(), -half_ext.y(), -half_ext.z()}, {half_ext.x(), -half_ext.y(), half_ext.z()} }; out_center = local_to_global * Eigen::Vector3f(half_ext.x(), 0, 0); break; case FaceType::Y_POS: local_vertices = { {half_ext.x(), half_ext.y(), half_ext.z()}, {half_ext.x(), half_ext.y(), -half_ext.z()}, {-half_ext.x(), half_ext.y(), -half_ext.z()}, {-half_ext.x(), half_ext.y(), half_ext.z()} }; out_center = local_to_global * Eigen::Vector3f(0, half_ext.y(), 0); break; case FaceType::Z_POS: local_vertices = { {half_ext.x(), half_ext.y(), half_ext.z()}, {half_ext.x(), -half_ext.y(), half_ext.z()}, {-half_ext.x(), -half_ext.y(), half_ext.z()}, {-half_ext.x(), half_ext.y(), half_ext.z()} }; out_center = local_to_global * Eigen::Vector3f(0, 0, half_ext.z()); break; default: return false; } // 转换到全局坐标 for (const auto& lv : local_vertices) { out_vertices.push_back(local_to_global * lv); } return true; } // 获取面的局部坐标系(u:面内X轴, v:面内Y轴, n:法向量)- 内联实现 bool Internal::OrientedBBox::get_face_local_axes(FaceType face, Eigen::Vector3f& out_u, Eigen::Vector3f& out_v, Eigen::Vector3f& out_n) const { std::vector<Eigen::Vector3f> vertices; Eigen::Vector3f face_center; if (!get_face_info(face, vertices, face_center)) { return false; } // 计算法向量n(面的外法线,单位向量) Eigen::Vector3f v1 = vertices[1] - vertices[0]; Eigen::Vector3f v2 = vertices[2] - vertices[0]; out_n = v1.cross(v2).normalized(); if (out_n.norm() < 1e-6f) { return false; // 面退化(非平面) } // 计算面内X轴u(从面中心到第一个顶点,投影到面内) Eigen::Vector3f center_to_v0 = vertices[0] - face_center; out_u = center_to_v0 - (center_to_v0.dot(out_n)) * out_n; // 去除法向分量 out_u.normalize(); if (out_u.norm() < 1e-6f) { // 退化处理 out_u = Eigen::Vector3f::UnitX(); if (std::fabs(out_u.dot(out_n)) > 0.9f) { out_u = Eigen::Vector3f::UnitY(); } out_u -= (out_u.dot(out_n)) * out_n; // 确保在面内 out_u.normalize(); } // 计算面内Y轴v(n × u,确保正交) out_v = out_n.cross(out_u).normalized(); return true; } // 绕面的法向量旋转(左旋转为正角度,右旋转为负角度)- 内联实现 Internal::OrientedBBox Internal::OrientedBBox::rotate_around_face_normal(FaceType face, float theta_deg) const { Eigen::Vector3f u, v, n; if (!get_face_local_axes(face, u, v, n)) { throw std::runtime_error("获取面坐标系失败,无法旋转"); } // 角度转换(左旋转+θ,右旋转-θ) float theta = theta_deg * M_PI / 180.0f; // 构建局部→全局转换矩阵M(u, v, n为列向量) Eigen::Matrix3f M; M.col(0) = u; M.col(1) = v; M.col(2) = n; // 局部坐标系下绕Z轴旋转的矩阵(显式转换为旋转矩阵) Eigen::Vector3f z_axis(0.0f, 0.0f, 1.0f); // Z轴向量 Eigen::AngleAxisf angle_axis(theta, z_axis); // 角度轴 Eigen::Matrix3f Rz = angle_axis.toRotationMatrix(); // 显式转换为3x3旋转矩阵 // 全局旋转矩阵 = M * Rz * M^T Eigen::Matrix3f R = M * Rz * M.transpose(); // 计算新的旋转矩阵 Eigen::Matrix3f current_rot = transform.inverse().linear(); Eigen::Matrix3f new_rot = R * current_rot; // 构建新的变换矩阵 Eigen::Affine3f new_local_to_global = Eigen::Affine3f::Identity(); new_local_to_global.rotate(new_rot); new_local_to_global.translate(center); // 新的全局→局部变换矩阵 Eigen::Affine3f new_transform = new_local_to_global.inverse(); // 打印new_transform的内容(旋转矩阵 + 平移向量) std::cout << "===== 新的全局→局部变换矩阵 (new_transform) =====" << std::endl; std::cout << "旋转部分(线性矩阵):" << std::endl << new_transform.linear() << std::endl; std::cout << "平移部分:" << new_transform.translation().transpose() << std::endl << std::endl; return OrientedBBox(center, extent, new_transform.inverse()); } // 内部工具函数实现(显式指定Internal::命名空间) Internal::OrientedBBox Internal::parse_obb(const std::vector<ObbData>& obb_data) { Internal::OrientedBBox obb; for (const auto& data : obb_data) { if (data.obj_type == "负防区") { continue; } const Eigen::Vector3f center(data.pos_x, data.pos_y, data.pos_z); const Eigen::Vector3f dimensions( std::fabs(data.scale_x), std::fabs(data.scale_y), std::fabs(data.scale_z) ); Eigen::Affine3f transform_to_local = Eigen::Affine3f::Identity(); transform_to_local.rotate(Eigen::AngleAxisf(data.rot_x, Eigen::Vector3f::UnitX())); transform_to_local.rotate(Eigen::AngleAxisf(data.rot_y, Eigen::Vector3f::UnitY())); transform_to_local.rotate(Eigen::AngleAxisf(data.rot_z, Eigen::Vector3f::UnitZ())); obb = Internal::OrientedBBox(center, dimensions, transform_to_local); break; } return obb; } cv::Mat Internal::gen_proj_internal( const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& local_cloud, const Internal::OrientedBBox& obb, const Eigen::Vector3f& cam_pos, std::vector<cv::Point>& out_corners, cv::Point& out_center, float cam_rot_deg) { const cv::Size img_size(1280, 640); cv::Mat img(img_size, CV_8UC3, cv::Scalar(0, 0, 0)); const Eigen::Vector3f local_obb_center(0.0f, 0.0f, 0.0f); // OBB在局部坐标系的中心 // 1. 计算投影方向相机坐标系 Eigen::Vector3f proj_dir = local_obb_center - cam_pos; const float dir_len = proj_dir.norm(); if (dir_len < 1e-6f) { proj_dir = Eigen::Vector3f::UnitZ(); // 避免除以零 } else { proj_dir.normalize(); } const Eigen::Vector3f world_up(0.0f, 0.0f, 1.0f); Eigen::Vector3f proj_x = world_up.cross(proj_dir); if (proj_x.norm() < 1e-6f) { proj_x = Eigen::Vector3f::UnitX(); // 避免退化 } else { proj_x.normalize(); } Eigen::Vector3f proj_y = proj_dir.cross(proj_x); proj_y.normalize(); // 2. 计算点云的投影坐标(用于绘制点) std::vector<Eigen::Vector3f> proj_pts; proj_pts.reserve(local_cloud->size()); Eigen::Matrix3f rotation = Eigen::Matrix3f::Identity(); if (std::fabs(cam_rot_deg) > 1e-6f) { const float angle_rad = cam_rot_deg * M_PI / 180.0f; rotation << std::cos(angle_rad), -std::sin(angle_rad), 0.0f, std::sin(angle_rad), std::cos(angle_rad), 0.0f, 0.0f, 0.0f, 1.0f; } for (const auto& pt : local_cloud->points) { const Eigen::Vector3f pt_eigen(pt.x, pt.y, pt.z); const Eigen::Vector3f pt_cam = pt_eigen - cam_pos; Eigen::Vector3f temp_point( pt_cam.dot(proj_x), pt_cam.dot(proj_y), pt_cam.dot(proj_dir) ); if (std::fabs(cam_rot_deg) > 1e-6f) { temp_point = rotation * temp_point; } proj_pts.push_back(temp_point); } // 3. 计算OBB角点的投影坐标(用于计算缩放比例绘制边框) const Eigen::Vector3f half_ext = obb.extent / 2.0f; std::vector<Eigen::Vector3f> local_corners = { {half_ext.x(), half_ext.y(), half_ext.z()}, {half_ext.x(), half_ext.y(), -half_ext.z()}, {half_ext.x(), -half_ext.y(), half_ext.z()}, {half_ext.x(), -half_ext.y(), -half_ext.z()}, {-half_ext.x(), half_ext.y(), half_ext.z()}, {-half_ext.x(), half_ext.y(), -half_ext.z()}, {-half_ext.x(), -half_ext.y(), half_ext.z()}, {-half_ext.x(), -half_ext.y(), -half_ext.z()} }; std::vector<Eigen::Vector3f> corner_proj; for (const auto& corner : local_corners) { const Eigen::Vector3f corner_cam = corner - cam_pos; Eigen::Vector3f temp_point( corner_cam.dot(proj_x), corner_cam.dot(proj_y), corner_cam.dot(proj_dir) ); if (std::fabs(cam_rot_deg) > 1e-6f) { temp_point = rotation * temp_point; } corner_proj.push_back(temp_point); } // 4. 关键修改:用OBB角点的投影范围计算缩放比例(确保同一OBB大小一致) float obb_x_min = 1e9f, obb_x_max = -1e9f; float obb_y_min = 1e9f, obb_y_max = -1e9f; for (const auto& cp : corner_proj) { obb_x_min = std::min(obb_x_min, cp.x()); obb_x_max = std::max(obb_x_max, cp.x()); obb_y_min = std::min(obb_y_min, cp.y()); obb_y_max = std::max(obb_y_max, cp.y()); } const float obb_max_range = std::max(obb_x_max - obb_x_min, obb_y_max - obb_y_min); const float safe_obb_range = (obb_max_range < 1e-6f) ? 1e-6f : obb_max_range; const float scale = std::min(img_size.width, img_size.height) * 0.8f / safe_obb_range; // 0.8是预留边距(避免OBB边缘超出图像),可根据需要调整 // 5. 投影中心与OBB中心对齐(确保OBB在图像中居中) Eigen::Vector3f center_cam = local_obb_center - cam_pos; Eigen::Vector3f proj_center( center_cam.dot(proj_x), center_cam.dot(proj_y), center_cam.dot(proj_dir) ); if (std::fabs(cam_rot_deg) > 1e-6f) { proj_center = rotation * proj_center; } const cv::Point img_center(img_size.width / 2, img_size.height / 2); // 6. 绘制点云 for (const auto& p : proj_pts) { const int x = static_cast<int>(img_center.x + (p.x() - proj_center.x()) * scale); const int y = static_cast<int>(img_center.y - (p.y() - proj_center.y()) * scale); const int clamped_x = std::clamp(x, 0, img_size.width - 1); const int clamped_y = std::clamp(y, 0, img_size.height - 1); cv::drawMarker(img, cv::Point(clamped_x, clamped_y), cv::Scalar(255, 255, 255), 1, // marker大小 1); // 线宽 } // 7. 处理OBB角点并绘制边框 std::vector<int> corner_indices = {0, 1, 2, 3, 4, 5, 6, 7}; // 按Z轴(深度)排序,只保留前4个可见角点 std::sort(corner_indices.begin(), corner_indices.end(), [&](int a, int b) { return corner_proj[a].z() > corner_proj[b].z(); }); corner_indices.resize(4); // 按角度排序,确保边框绘制顺序正确 Eigen::Vector3f centroid(0.0f, 0.0f, 0.0f); for (int idx : corner_indices) { centroid += corner_proj[idx]; } centroid /= 4.0f; std::sort(corner_indices.begin(), corner_indices.end(), [&](int a, int b) { const float ang_a = std::atan2(corner_proj[a].y() - centroid.y(), corner_proj[a].x() - centroid.x()); const float ang_b = std::atan2(corner_proj[b].y() - centroid.y(), corner_proj[b].x() - centroid.x()); return ang_a < ang_b; }); // 映射角点到图像坐标 out_corners.clear(); for (int idx : corner_indices) { const int x = static_cast<int>(img_center.x + (corner_proj[idx].x() - proj_center.x()) * scale); const int y = static_cast<int>(img_center.y - (corner_proj[idx].y() - proj_center.y()) * scale); out_corners.emplace_back(std::clamp(x, 0, img_size.width - 1), std::clamp(y, 0, img_size.height - 1)); } // 绘制OBB边框 for (size_t i = 0; i < 4; ++i) { cv::line(img, out_corners[i], out_corners[(i + 1) % 4], cv::Scalar(0, 255, 0), 2, cv::LINE_AA); } // 8. 绘制OBB中心点(红点) Eigen::Vector3f temp_center = proj_center; // 已与OBB中心对齐 const int cx = static_cast<int>(img_center.x + (temp_center.x() - proj_center.x()) * scale); const int cy = static_cast<int>(img_center.y - (temp_center.y() - proj_center.y()) * scale); out_center = cv::Point(std::clamp(cx, 0, img_size.width - 1), std::clamp(cy, 0, img_size.height - 1)); cv::circle(img, out_center, 5, cv::Scalar(0, 0, 255), -1); return img; } // 内存级接口实现(调用内部函数时显式指定Internal::) int get_proj_results( const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud, const std::vector<ObbData>& obb_data, ProjResult* results, bool use_all_points) { try { if (!cloud || cloud->empty()) return -1; if (obb_data.empty()) return -2; if (!results) return -3; // 显式调用Internal::parse_obb const Internal::OrientedBBox obb = Internal::parse_obb(obb_data); if (obb.extent.norm() < 1e-6f) return -4; pcl::PointCloud<pcl::PointXYZ>::Ptr processed_cloud; if (use_all_points) { processed_cloud = cloud->makeShared(); } else { const std::vector<int> in_obb_indices = obb.get_points_in_box(cloud); std::cout << "------------------------???" << in_obb_indices.size() << std::endl; if (in_obb_indices.empty()) return -5; pcl::PointCloud<pcl::PointXYZ>::Ptr in_obb_pcd(new pcl::PointCloud<pcl::PointXYZ>()); pcl::ExtractIndices<pcl::PointXYZ> extractor; pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); inliers->indices = in_obb_indices; extractor.setInputCloud(cloud); extractor.setIndices(inliers); extractor.setNegative(false); extractor.filter(*in_obb_pcd); processed_cloud = in_obb_pcd; } pcl::PointCloud<pcl::PointXYZ>::Ptr local_cloud(new pcl::PointCloud<pcl::PointXYZ>()); pcl::transformPointCloud(*processed_cloud, *local_cloud, obb.get_global_to_local()); const Eigen::Vector3f half_ext = obb.extent / 2.0f; const float cam_distance = 15.0f; const std::vector<Eigen::Vector3f> cam_positions = { {half_ext.x() + cam_distance, 0.0f, 0.0f}, {0.0f, half_ext.y() + cam_distance, 0.0f}, {0.0f, 0.0f, half_ext.z() + cam_distance} }; const float cam_rotations[] = {0.0f, 0.0f, 90.0f}; for (size_t i = 0; i < 3; ++i) { std::vector<cv::Point> obb_corners; cv::Point obb_center; // 显式调用Internal::gen_proj_internal const cv::Mat proj_img = Internal::gen_proj_internal( local_cloud, obb, cam_positions[i], obb_corners, obb_center, cam_rotations[i] ); if (proj_img.empty()) return -6; results[i].img = proj_img.clone(); results[i].obb_corners = obb_corners; results[i].obb_center = obb_center; } return 0; } catch (...) { return -99; } } // 文件级接口实现 int gen_proj_images( const char* pcd_path, const char* json_path, bool use_all_points) { try { if (!pcd_path || !json_path || strlen(pcd_path) == 0 || strlen(json_path) == 0) return -1; if (!std::ifstream(pcd_path).good()) return -2; if (!std::ifstream(json_path).good()) return -3; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>()); if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_path, *cloud) == -1) return -4; if (cloud->empty()) return -5; std::vector<ObbData> obb_data_list; json j; std::ifstream json_file(json_path); json_file >> j; json obb_json_array; if (j.is_object()) { obb_json_array.push_back(j); } else if (j.is_array()) { obb_json_array = j; } else { return -6; } for (const auto& item : obb_json_array) { ObbData obb_data; obb_data.pos_x = item.at("position").at("x").get<float>(); obb_data.pos_y = item.at("position").at("y").get<float>(); obb_data.pos_z = item.at("position").at("z").get<float>(); obb_data.rot_x = item.at("rotation").at("x").get<float>(); obb_data.rot_y = item.at("rotation").at("y").get<float>(); obb_data.rot_z = item.at("rotation").at("z").get<float>(); obb_data.scale_x = item.at("scale").at("x").get<float>(); obb_data.scale_y = item.at("scale").at("y").get<float>(); obb_data.scale_z = item.at("scale").at("z").get<float>(); obb_data.obj_type = item.value("obj_type", ""); obb_data_list.push_back(obb_data); } ProjResult results[3]; int ret = get_proj_results(cloud, obb_data_list, results, use_all_points); if (ret != 0) return ret; const std::string save_paths[3] = { "OBB_Local_X+_View_Original.png", "OBB_Local_Y+_View_Original.png", "OBB_Local_Z+_View_Rotated90deg.png" }; for (size_t i = 0; i < 3; ++i) { if (!cv::imwrite(save_paths[i], results[i].img)) { free_proj_results(results); return -7; } } free_proj_results(results); return 0; } catch (...) { return -99; } } // 内存释放接口 void free_proj_results(ProjResult* results) { if (results) { for (int i = 0; i < 3; ++i) { results[i].img.release(); results[i].obb_corners.clear(); } } } 这个代码还是会导致boxwidget 变形
最新发布
10-26
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

给算法爸爸上香

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值