|
|
@@ -7,7 +7,7 @@
|
|
|
#include <pcl/common/transforms.h>
|
|
|
|
|
|
|
|
|
-constexpr float kMinProbability = 0.0f;
|
|
|
+constexpr float kMinProbability = 0.01f;
|
|
|
constexpr float kMaxProbability = 1.f - kMinProbability;
|
|
|
constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
|
|
|
constexpr int kPadding = INT_MAX / 4;
|
|
|
@@ -63,10 +63,13 @@ public:
|
|
|
m_costs_lr = 0.0;
|
|
|
m_costs_rr = 0.0;
|
|
|
}
|
|
|
- template <typename T>
|
|
|
+ template <typename T>
|
|
|
bool operator()(const T* const variable, T* residual) const {
|
|
|
// 每一轮重新初始化残差值
|
|
|
T costs[4]={T(0),T(0),T(0),T(0)};
|
|
|
+ std::vector<std::vector<T> > deviation_calc_vector;
|
|
|
+ deviation_calc_vector.resize(4);
|
|
|
+ T deviation_weight = T(0.05);
|
|
|
|
|
|
T cx = variable[0];
|
|
|
T cy = variable[1];
|
|
|
@@ -74,6 +77,13 @@ public:
|
|
|
T length = variable[3];
|
|
|
T width = variable[4];
|
|
|
T theta_front = variable[5];
|
|
|
+ T theta_front_weight = T(0.8);
|
|
|
+ T wheel_length_img_ratio = variable[6];
|
|
|
+ T wheel_width_length_ratio = variable[7];
|
|
|
+ // [1/3, 5/3]
|
|
|
+ T limited_wheel_length_img_ratio = T(0.8) / (T(1) + ceres::exp(T(-wheel_length_img_ratio))) + T(0.75);
|
|
|
+ // [0.25, 0.35]
|
|
|
+ T limited_wheel_length_ratio = T(0.25) / (T(1) + ceres::exp(T(-wheel_width_length_ratio))) + T(0.25);
|
|
|
|
|
|
//整车旋转
|
|
|
Eigen::Rotation2D<T> rotation(theta);
|
|
|
@@ -107,11 +117,12 @@ public:
|
|
|
const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
|
|
|
//旋转
|
|
|
const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
|
|
|
- interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
|
|
|
- point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
|
|
|
+ interpolator.Evaluate(point_rotation(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio* m_scale + rows / 2.0 + 0.5 + T(kPadding),
|
|
|
+ point_rotation(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
|
|
|
&residual[i]);
|
|
|
+ //residual[i] += theta_front*theta_front_weight;
|
|
|
costs[0] += residual[i];
|
|
|
-
|
|
|
+ deviation_calc_vector[0].push_back(residual[i]);
|
|
|
}
|
|
|
//右前轮
|
|
|
int right_front_num = m_right_front_cloud.size();
|
|
|
@@ -122,10 +133,12 @@ public:
|
|
|
const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
|
|
|
//旋转
|
|
|
const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
|
|
|
- interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
|
|
|
- point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
|
|
|
+ interpolator.Evaluate(point_rotation(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio* m_scale + rows / 2.0 + 0.5 + T(kPadding),
|
|
|
+ point_rotation(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
|
|
|
&residual[left_front_num + i]);
|
|
|
+ //residual[left_front_num + i] += theta_front*theta_front_weight;
|
|
|
costs[1] += residual[left_front_num+i];
|
|
|
+ deviation_calc_vector[1].push_back(residual[left_front_num+i]);
|
|
|
}
|
|
|
//左后轮
|
|
|
int left_rear_num = m_left_rear_cloud.size();
|
|
|
@@ -136,44 +149,63 @@ public:
|
|
|
//减去经过车辆旋转计算的左前中心
|
|
|
const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
|
|
|
|
|
|
- interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
|
|
|
- tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
|
|
|
+ interpolator.Evaluate(tanslate_point(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio * m_scale + rows / 2.0 + 0.5 + T(kPadding),
|
|
|
+ tanslate_point(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
|
|
|
&residual[left_front_num + right_front_num + i]);
|
|
|
costs[2] += residual[left_front_num + right_front_num + i];
|
|
|
+ deviation_calc_vector[2].push_back(residual[left_front_num + right_front_num + i]);
|
|
|
}
|
|
|
|
|
|
//右后轮
|
|
|
-
|
|
|
+ int right_rear_num = m_right_rear_cloud.size();
|
|
|
for (int i = 0; i < m_right_rear_cloud.size(); ++i) {
|
|
|
const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud[i].x) - cx),
|
|
|
(T(m_right_rear_cloud[i].y) - cy));
|
|
|
//减去经过车辆旋转计算的左前中心
|
|
|
const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
|
|
|
|
|
|
- interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
|
|
|
- tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
|
|
|
+ interpolator.Evaluate(tanslate_point(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio * m_scale + rows / 2.0 + 0.5 + T(kPadding),
|
|
|
+ tanslate_point(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
|
|
|
&residual[left_front_num + right_front_num + left_rear_num + i]);
|
|
|
costs[3] += residual[left_front_num + right_front_num + left_rear_num + i];
|
|
|
+ deviation_calc_vector[3].push_back(residual[left_front_num + right_front_num + left_rear_num + i]);
|
|
|
}
|
|
|
|
|
|
+ // 前轮旋转loss
|
|
|
+ residual[left_front_num + right_front_num + left_rear_num + right_rear_num] = theta_front*theta_front_weight;
|
|
|
+ // 四轮方差loss
|
|
|
+ residual[left_front_num + right_front_num + left_rear_num + right_rear_num+1] = T(0);
|
|
|
+ //printf("--------------\n");
|
|
|
+ T avg_costs[4] = {T(costs[0]/T(left_front_num)), T(costs[1]/T(right_front_num)), T(costs[2]/T(left_rear_num)), T(costs[3]/T(right_rear_num))};
|
|
|
+ for (int i = 0; i < deviation_calc_vector.size(); ++i) {
|
|
|
+ T t_deviation = T(0);
|
|
|
+ for (int j = 0; j < deviation_calc_vector[i].size(); ++j) {
|
|
|
+ t_deviation += deviation_weight * ceres::abs(deviation_calc_vector[i][j] - avg_costs[i]);
|
|
|
+ }
|
|
|
+ residual[left_front_num + right_front_num + left_rear_num + right_rear_num+1] += t_deviation;
|
|
|
+ //printf("??? dev: %.6f\n", t_deviation);
|
|
|
+ //costs[i] = t_deviation;
|
|
|
+ //printf("dev: %.6f\n", costs[i]);
|
|
|
+ }
|
|
|
+
|
|
|
char buf[30];
|
|
|
memset(buf, 0, 30);
|
|
|
- sprintf(buf, "%.7f", costs[0]);
|
|
|
+ sprintf(buf, "%.12f", costs[0]);
|
|
|
m_costs_lf = std::stod(buf) / left_front_num;
|
|
|
|
|
|
memset(buf, 0, 30);
|
|
|
- sprintf(buf, "%.7f", costs[1]);
|
|
|
+ sprintf(buf, "%.12f", costs[1]);
|
|
|
m_costs_rf = std::stod(buf) / right_front_num;
|
|
|
|
|
|
memset(buf, 0, 30);
|
|
|
- sprintf(buf, "%.7f", costs[2]);
|
|
|
+ sprintf(buf, "%.12f", costs[2]);
|
|
|
m_costs_lr = std::stod(buf) / left_rear_num;
|
|
|
|
|
|
-
|
|
|
memset(buf, 0, 30);
|
|
|
- sprintf(buf, "%.7f", costs[3]);
|
|
|
- m_costs_rr = std::stod(buf) / m_right_rear_cloud.size();
|
|
|
+ sprintf(buf, "%.12f", costs[3]);
|
|
|
+ m_costs_rr = std::stod(buf) / right_rear_num;
|
|
|
// m_costs_lf = costs[0];
|
|
|
+ //printf("%.6f %.6f %.6f %.6f\n",m_costs_lf, m_costs_rf, m_costs_lr, m_costs_rr);
|
|
|
return true;
|
|
|
}
|
|
|
void get_costs(double &lf, double &rf, double &lr, double &rr)
|
|
|
@@ -202,12 +234,12 @@ bool detect_wheel_ceres::update_mat(int rows, int cols)
|
|
|
cv::Size size(n+2,n+2);
|
|
|
cv::Rect rect(center-cv::Point(n/2,n/2),size);
|
|
|
double x = n / double(L);
|
|
|
- double sigma = 0.06;
|
|
|
+ double sigma = 0.05;
|
|
|
double miu = 0.0;
|
|
|
double scale = 0.02;
|
|
|
- double translation = -0.05;
|
|
|
+ double translation = 0.03;
|
|
|
double gauss_value = (scale/(sqrt(2*M_PI)*sigma))*exp(-pow(x-miu, 2)/(2*sigma*sigma))+translation;
|
|
|
- double quadratic_value = x-0.08;
|
|
|
+ double quadratic_value = std::max(2*x-0.25, 0.045);
|
|
|
// LOG(INFO) << rect.tl().x<<", "<<rect.tl().y<<", "<<rect.br().x<<", "<<rect.br().y;
|
|
|
// 对内外分别取色
|
|
|
cv::rectangle(map,rect,std::max(gauss_value, quadratic_value));
|
|
|
@@ -227,15 +259,16 @@ bool detect_wheel_ceres::update_mat(int rows, int cols)
|
|
|
detect_wheel_ceres::detect_wheel_ceres()
|
|
|
{
|
|
|
int cols=800;
|
|
|
- int rows=200;
|
|
|
+ int rows=800;
|
|
|
update_mat(rows, cols);
|
|
|
}
|
|
|
detect_wheel_ceres::~detect_wheel_ceres(){}
|
|
|
|
|
|
|
|
|
|
|
|
-bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec, Detect_result &detect_result)
|
|
|
+bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec, Detect_result &detect_result, std::string &error_info)
|
|
|
{
|
|
|
+ error_info="";
|
|
|
//清理点云
|
|
|
m_left_front_cloud.clear();
|
|
|
m_right_front_cloud.clear();
|
|
|
@@ -357,14 +390,16 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
|
|
|
bool solve_result = false;
|
|
|
double total_solve_time = 0;
|
|
|
bool stop_sign = false;
|
|
|
- for (int j = 2; j < 4&&!stop_sign; j++)
|
|
|
- {
|
|
|
- // double input_vars[] = {detect_result.cx, detect_result.cy, detect_result.theta, detect_result.wheel_base, detect_result.width, detect_result.front_theta};
|
|
|
- double map_rows = map_cols * 1.0 / j ;
|
|
|
- update_mat(map_rows, map_cols);
|
|
|
+ // for (int j = 2; j < 4&&!stop_sign; j++)
|
|
|
+ // {
|
|
|
+ // // double input_vars[] = {detect_result.cx, detect_result.cy, detect_result.theta, detect_result.wheel_base, detect_result.width, detect_result.front_theta};
|
|
|
+ // double map_rows = map_cols * 1.0 / j ;
|
|
|
+ // update_mat(map_rows, map_cols);
|
|
|
+
|
|
|
+ // !!! 将车轮比例添加到优化模型中
|
|
|
Detect_result t_detect_result = detect_result;
|
|
|
- // 寻找最小loss值对应的初始旋转角
|
|
|
- for (int i = -5; i < 6&&!stop_sign; i+=2)
|
|
|
+ // 寻找最小loss值对应的初始旋转角
|
|
|
+ for (int i = -3; i < 4&&!stop_sign; i+=6)
|
|
|
{
|
|
|
t_detect_result.theta = (-angle_x + i) * M_PI / 180.0;
|
|
|
// printf("double x:%.3f y: %.3f wheel:%.3f width:%.3f theta : %.3f front theta: %.3f\n",input_vars[0],input_vars[1],input_vars[3],input_vars[4],input_vars[2],input_vars[5]);
|
|
|
@@ -377,7 +412,9 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
|
|
|
t_detect_result.front_theta *= (-M_PI) / 180.0;
|
|
|
}
|
|
|
auto t1=std::chrono::system_clock::now();
|
|
|
- bool current_result = Solve(t_detect_result, t_loss_result);
|
|
|
+ std::string t_error_info;
|
|
|
+ bool current_result = Solve(t_detect_result, t_loss_result, t_error_info);
|
|
|
+ error_info = t_error_info;
|
|
|
auto t2=std::chrono::system_clock::now();
|
|
|
auto duration=t2-t1;
|
|
|
static double second=0.0;
|
|
|
@@ -395,17 +432,17 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
|
|
|
detect_result = t_detect_result;
|
|
|
solve_result = current_result;
|
|
|
loss_result = t_loss_result;
|
|
|
- optimized_map_rows = map_rows;
|
|
|
+ // optimized_map_rows = map_rows;
|
|
|
calc_count++;
|
|
|
- /*// 新增，优化时间足够长则认为已找到正确结果
|
|
|
- if(second > 0.02)
|
|
|
- {
|
|
|
- stop_sign = true;
|
|
|
- break;
|
|
|
- }*/
|
|
|
+ // // 新增，优化时间足够长则认为已找到正确结果
|
|
|
+ // if(second > 0.02)
|
|
|
+ // {
|
|
|
+ // stop_sign = true;
|
|
|
+ // break;
|
|
|
+ // }
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
+ // }
|
|
|
// std::cout<<"solve time "<<total_solve_time<<std::endl;
|
|
|
// LOG(INFO) << "final avg cost for four wheels: "<<whole_loss[0]<<" "<<whole_loss[1]<<" "<<whole_loss[2]<<" "<<whole_loss[3]<<" ";
|
|
|
|
|
|
@@ -421,6 +458,10 @@ bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> clou
|
|
|
// // LOG(INFO) <<"going to show img ";
|
|
|
// Solve(detect_result, t_loss_result, true);
|
|
|
|
|
|
+// printf(" x:%.3f y: %.3f wheel:%.3f width:%.3f theta : %.3f front theta: %.3f\n",
|
|
|
+// detect_result.cx,detect_result.cy,detect_result.wheel_base,detect_result.width,detect_result.theta,detect_result.front_theta);
|
|
|
+ if(solve_result)
|
|
|
+ error_info="";
|
|
|
return solve_result;
|
|
|
|
|
|
}
|
|
|
@@ -569,7 +610,7 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, double &avg_loss)
|
|
|
}
|
|
|
|
|
|
|
|
|
-bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_result, bool save_img)
|
|
|
+bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_result, std::string &error_info, bool save_img)
|
|
|
{
|
|
|
double SCALE=300.0;
|
|
|
double cx=detect_result.cx;
|
|
|
@@ -578,17 +619,21 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
|
|
|
double init_wheel_base=2.7;
|
|
|
double init_width=1.55;
|
|
|
double init_theta_front=0*M_PI/180.0;
|
|
|
+ // 车轮长图比, 比例取值1/3-5/3, 当前变量无限制
|
|
|
+ double init_wheel_length_img_ratio = 0;
|
|
|
+ // 车轮宽长比, 默认长为1,比例取值范围0.2-0.8, 当前变量范围无限制
|
|
|
+ double init_wheel_width_length_ratio = 0;
|
|
|
|
|
|
- double variable[] = {cx,cy,init_theta,init_wheel_base,init_width,init_theta_front};
|
|
|
+ double variable[] = {cx, cy, init_theta, init_wheel_base, init_width, init_theta_front, init_wheel_length_img_ratio, init_wheel_width_length_ratio};
|
|
|
// printf("solve x:%.3f y: %.3f wheel:%.3f width:%.3f theta : %.3f front theta: %.3f\n", variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);
|
|
|
// 第二部分：构建寻优问题
|
|
|
ceres::Problem problem;
|
|
|
CostFunctor *cost_func = new CostFunctor(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_map,SCALE);
|
|
|
//使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
|
|
|
ceres::CostFunction* cost_function =new
|
|
|
- ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 6>(
|
|
|
+ ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 8>(
|
|
|
cost_func,
|
|
|
- m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
|
|
|
+ m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size()+2);
|
|
|
problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
|
|
|
|
|
|
|
|
|
@@ -596,12 +641,18 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
|
|
|
ceres::Solver::Options options;
|
|
|
options.use_nonmonotonic_steps=false;
|
|
|
options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
|
|
|
+ options.logging_type = ceres::LoggingType::SILENT;
|
|
|
options.max_num_iterations=60;
|
|
|
options.num_threads=3;
|
|
|
options.minimizer_progress_to_stdout = false;//输出到cout
|
|
|
ceres::Solver::Summary summary;//优化信息
|
|
|
ceres::Solve(options, &problem, &summary);//求解!!!
|
|
|
|
|
|
+// if(!summary.IsSolutionUsable())
|
|
|
+// {
|
|
|
+// error_info.append("检测失败\t");
|
|
|
+// return false;
|
|
|
+// }
|
|
|
// std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
|
|
|
// debug_img(detect_result, loss_result, true);
|
|
|
double loss=summary.final_cost/(m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
|
|
|
@@ -621,28 +672,33 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
|
|
|
//检验
|
|
|
// printf("loss: %.5f\n", loss);
|
|
|
// printf("middle x:%.3f y: %.3f wheel:%.3f width:%.3f theta : %.3f front theta: %.3f\n", variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);
|
|
|
- if(loss>0.0115)
|
|
|
+ if(loss>0.005)
|
|
|
{
|
|
|
+ error_info.append("总loss过大 ").append(std::to_string(loss)).append("\t");
|
|
|
// LOG(WARNING) <<"总loss过大 "<<loss;
|
|
|
return false;
|
|
|
}
|
|
|
- if (detect_result.width < 1.350 || detect_result.width > 2.000 || detect_result.wheel_base > 3.150 || detect_result.wheel_base < 2.200)
|
|
|
+ if (detect_result.width < 1.25 || detect_result.width > 2.000 || detect_result.wheel_base > 3.200 || detect_result.wheel_base < 2.200)
|
|
|
{
|
|
|
-// LOG(WARNING) <<"宽度(1.35, 2.00) 轴距(2.20, 3.15): "<<detect_result.width<<", "<<detect_result.wheel_base;
|
|
|
+ error_info.append(std::string("宽度(1.25, 2.00) 轴距(2.20, 3.20): ").append(std::to_string(detect_result.width).append(", ").append(std::to_string(detect_result.wheel_base)).append("\t")));
|
|
|
+// LOG(WARNING) <<"宽度(1.35, 2.00) 轴距(2.20, 3.30): "<<detect_result.width<<", "<<detect_result.wheel_base;
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
- detect_result.width += 0.15; //车宽+10cm
|
|
|
- // printf(" x:%.3f y: %.3f wheel:%.3f width:%.3f theta : %.3f front theta: %.3f\n",x,y,wheel_base,width,theta,front_theta);
|
|
|
+ detect_result.width += 0.22; //车宽一边+11cm
|
|
|
+// printf(" x:%.3f y: %.3f wheel:%.3f width:%.3f theta : %.3f front theta: %.3f\n",
|
|
|
+// detect_result.cx,detect_result.cy,detect_result.wheel_base,detect_result.width,detect_result.theta,detect_result.front_theta);
|
|
|
|
|
|
// //added by yct
|
|
|
if(detect_result.theta > 120 || detect_result.theta < 60)
|
|
|
{
|
|
|
+ error_info.append("总角度错误 ").append(std::to_string(detect_result.theta)).append("\t");
|
|
|
// LOG(WARNING) <<"总角度错误 "<<detect_result.theta;
|
|
|
return false;
|
|
|
}
|
|
|
if(fabs(detect_result.front_theta)>35)
|
|
|
{
|
|
|
+ error_info.append("前轮角度错误 ").append(std::to_string(detect_result.front_theta)).append("\t");
|
|
|
// LOG(WARNING) <<"前轮角度错误 "<<detect_result.front_theta;
|
|
|
return false;
|
|
|
}
|
|
|
@@ -659,17 +715,23 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
|
|
|
loss_result.rb_loss = costs_rr;
|
|
|
loss_result.total_avg_loss = loss;
|
|
|
// std::cout << "222"<< std::endl;
|
|
|
- if (costs_lf <= 1e-6 && costs_rf <= 1e-6 && costs_lr <= 1e-6 && costs_rr <= 1e-6)
|
|
|
+ if (costs_lf == costs_rf && costs_lf == costs_lf && costs_lf == costs_rr)// (costs_lf <= 1e-6 && costs_rf <= 1e-6 && costs_lr <= 1e-6 && costs_rr <= 1e-6)
|
|
|
{
|
|
|
+ return false;
|
|
|
loss_result.lf_loss = loss;
|
|
|
loss_result.rf_loss = loss;
|
|
|
loss_result.lb_loss = loss;
|
|
|
loss_result.rb_loss = loss;
|
|
|
}
|
|
|
// 判断每个轮子平均loss是否满足条件
|
|
|
- double single_wheel_loss_threshold = 0.109;
|
|
|
+ double single_wheel_loss_threshold = 0.5;
|
|
|
if(loss_result.lf_loss > single_wheel_loss_threshold || loss_result.rf_loss > single_wheel_loss_threshold || loss_result.lb_loss > single_wheel_loss_threshold || loss_result.rb_loss > single_wheel_loss_threshold)
|
|
|
{
|
|
|
+ error_info.append("四轮loss过大 ").append(std::to_string(loss_result.lf_loss)).append(", ")
|
|
|
+ .append(std::to_string(loss_result.rf_loss)).append(", ")
|
|
|
+ .append(std::to_string(loss_result.lb_loss)).append(", ")
|
|
|
+ .append(std::to_string(loss_result.rb_loss)).append("\t");
|
|
|
+// LOG(WARNING)<<error_info;
|
|
|
// LOG(WARNING) <<"四轮loss过大"<<loss_result.lf_loss<<", "<<loss_result.rf_loss<<", "<<loss_result.lb_loss<<", "<<loss_result.rb_loss;
|
|
|
return false;
|
|
|
}
|
|
|
@@ -685,7 +747,7 @@ bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_r
|
|
|
Detect_result t_detect_result = detect_result;
|
|
|
t_detect_result.theta *= -M_PI/180.0;
|
|
|
t_detect_result.front_theta *= -M_PI/180.0;
|
|
|
- t_detect_result.width -= 0.15;
|
|
|
+ t_detect_result.width -= 0.22;
|
|
|
transform_src(t_detect_result);
|
|
|
|
|
|
return true;
|
