USER
/*
说明: 车道线在原始图像坐标点映射到相机坐标系,车辆坐标系
输入:
std::vector<LaneLinePtr>& lane_objects: 图像坐标系车道线结果
image2novatel_pose_: 图像坐标系向车辆坐标系的映射矩阵
int center_x : 原始图像中心,wd/2
int center_y : 原始图像中心,ht/2
float pitch_angle: 车辆俯仰角
float camera_ground_height: 相机安装高度
center_blob : 3Dhead的中心点blob
intrinsic_params_inverse: 相机内参逆矩阵
输出:
std::vector<LaneLinePtr>& lane_objects:
车道线在相机坐标系,车辆坐标系的坐标点及曲线拟合
*/
/// TODO: 车身坐标系 ==> 世界坐标系? and vehicle min max
bool HMLaneDetector::TransImgLaneToCameraNovatel(
std::vector<LaneLinePtr> &lane_objects, Eigen::Affine3d image2novatel_pose_,
int center_x, int center_y, float pitch_angle, float camera_ground_height,
const Eigen::Matrix3f &intrinsic_params_inverse,
std::string save_result_path) {
FILE *fp = nullptr;
if (!save_result_path.empty()) {
fp = fopen(save_result_path.c_str(), "w");
if (fp == nullptr) {
AINFO << "Failed to open " << save_result_path;
return false;
}
}
//三阶曲线拟合
const int max_poly_order = 3;
std::vector<Point2DF> vehicle_curve_points_;
std::vector<Point2DF> camera_curve_points_;
int line_num = static_cast<int>(lane_objects.size());
if (fp != nullptr) {
fprintf(fp, "line_num: %d\n", line_num);
}
for (int i = 0; i < line_num; i++) {
LaneLine *lane_line = lane_objects[i].get();
LaneLineColor line_color = lane_line->color;
if (fp != nullptr) {
fprintf(fp, "line_color: %d\n", static_cast<int>(line_color));
}
LaneLineType line_type = lane_line->type;
if (fp != nullptr) {
fprintf(fp, "line_type: %d\n", static_cast<int>(line_type));
}
std::vector<LaneLinePoint> temp_points = lane_line->lane_line_point_set;
int point_num = static_cast<int>(temp_points.size());
if (fp != nullptr) {
fprintf(fp, "point_num: %d\n", point_num);
fprintf(fp, "image_point: \n");
}
for (int m = 0; m < point_num; m++) {
Point2DF image_point = temp_points[m].image_point;
if (fp != nullptr) {
fprintf(fp, "%f, %f, ", image_point.x, image_point.y);
}
}
if (fp != nullptr) {
fprintf(fp, "\n");
fprintf(fp, "camera_cood_point: \n");
}
for (int m = 0; m < point_num; m++) {
Point2DF image_point = temp_points[m].image_point;
Point3DF camera_point;
Point2DF camera_point_2D;
ImagePoint2Camera(image_point, pitch_angle, camera_ground_height,
intrinsic_params_inverse, camera_point);
lane_line->lane_line_point_set[m].camera_point.x = camera_point.x;
lane_line->lane_line_point_set[m].camera_point.y = camera_point.y;
lane_line->lane_line_point_set[m].camera_point.z = camera_point.z;
camera_point_2D.x = camera_point.x;
camera_point_2D.y = camera_point.z; // 注意相机坐标系,是z前方
camera_curve_points_.emplace_back(camera_point_2D);
if (fp != nullptr) {
fprintf(fp, "%f, %f, ", camera_point.x, camera_point.z);
}
Point3DF vehicle_point;
Point2DF vehicle_point_2D;
Image2Novatel(image_point, center_x, center_y, image2novatel_pose_,
vehicle_point);
lane_line->lane_line_point_set[m].vehicle_point.x = vehicle_point.x;
lane_line->lane_line_point_set[m].vehicle_point.y = vehicle_point.y;
lane_line->lane_line_point_set[m].vehicle_point.z = vehicle_point.z;
vehicle_point_2D.x = vehicle_point.x;
vehicle_point_2D.y = vehicle_point.y; //车辆坐标系,是y在前方
vehicle_curve_points_.emplace_back(vehicle_point_2D);
}
if (fp != nullptr) {
fprintf(fp, "\n");
}
// camera curve fitting
sort(
camera_curve_points_.begin(), camera_curve_points_.end(),
[](const Point2DF &a, const Point2DF &b) -> bool { return a.y < b.y; });
// vehicle curve fitting
sort(
vehicle_curve_points_.begin(), vehicle_curve_points_.end(),
[](const Point2DF &a, const Point2DF &b) -> bool { return a.y < b.y; });
//生成3阶多项式拟合点
std::vector<Eigen::Matrix<float, 2, 1>> camera_pos_vec;
Eigen::Matrix<float, max_poly_order + 1, 1> camera_coeff;
float camera_r_start = -1;
float camera_r_end = -1;
std::vector<Eigen::Matrix<float, 2, 1>> vehicle_pos_vec;
Eigen::Matrix<float, max_poly_order + 1, 1> vehicle_coeff;
float vehicle_r_start = -1;
float vehicle_r_end = -1;
bool is_x_axis = false;
for (int m = 0; m < point_num; m++) {
Eigen::Matrix<float, 2, 1> camera_pos;
float camera_x_pos = camera_curve_points_[m].x;
float camera_z_pos = camera_curve_points_[m].y;
camera_pos << camera_x_pos, camera_z_pos;
camera_pos_vec.emplace_back(camera_pos);
Eigen::Matrix<float, 2, 1> vehicle_pos;
float vehicle_x_pos = vehicle_curve_points_[m].x;
float vehicle_y_pos = vehicle_curve_points_[m].y;
vehicle_pos << vehicle_x_pos, vehicle_y_pos;
vehicle_pos_vec.emplace_back(vehicle_pos);
if (m == 0) {
camera_r_start = camera_z_pos;
camera_r_end = camera_z_pos;
vehicle_r_start = vehicle_y_pos;
vehicle_r_end = vehicle_y_pos;
} else {
camera_r_start = std::min(camera_r_start, camera_z_pos);
camera_r_end = std::max(camera_r_end, camera_z_pos);
vehicle_r_start = std::min(vehicle_r_start, vehicle_y_pos);
vehicle_r_end = std::max(vehicle_r_end, vehicle_y_pos);
}
}
// 3阶多项式拟合, 在相机坐标系,车辆坐标系多项式拟合有问题,需要进一步排查
bool camera_fit_flag =
PolyFit(camera_pos_vec, max_poly_order, &camera_coeff, is_x_axis);
bool vehicle_fit_flag =
PolyFit(vehicle_pos_vec, max_poly_order, &vehicle_coeff, is_x_axis);
if ((!camera_fit_flag) || (!vehicle_fit_flag)) {
return false;
}
lane_line->camera_curve.coeffs.clear();
lane_line->camera_curve.coeffs.emplace_back(camera_coeff(0, 0));
lane_line->camera_curve.coeffs.emplace_back(camera_coeff(1, 0));
lane_line->camera_curve.coeffs.emplace_back(camera_coeff(2, 0));
lane_line->camera_curve.coeffs.emplace_back(camera_coeff(3, 0));
lane_line->camera_curve.min = camera_r_start;
lane_line->camera_curve.max = camera_r_end;
lane_line->vehicle_curve.coeffs.emplace_back(vehicle_coeff(0, 0));
lane_line->vehicle_curve.coeffs.emplace_back(vehicle_coeff(1, 0));
lane_line->vehicle_curve.coeffs.emplace_back(vehicle_coeff(2, 0));
lane_line->vehicle_curve.coeffs.emplace_back(vehicle_coeff(3, 0));
lane_line->vehicle_curve.min = vehicle_r_start;
lane_line->vehicle_curve.max = vehicle_r_end;
}
if (fp != nullptr) {
fclose(fp);
}
return true;
} 请添加代码,将车辆坐标系转换成世界坐标系