|
@@ -0,0 +1,352 @@
|
|
|
+//
|
|
|
+// Created by zx on 2020/7/1.
|
|
|
+//
|
|
|
+
|
|
|
+#include "detect_wheel_ceres.h"
|
|
|
+#include <ceres/cubic_interpolation.h>
|
|
|
+#include <pcl/common/transforms.h>
|
|
|
+
|
|
|
+
|
|
|
+constexpr float kMinProbability = 0.0f;
|
|
|
+constexpr float kMaxProbability = 1.f - kMinProbability;
|
|
|
+constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
|
|
|
+constexpr int kPadding = INT_MAX / 4;
|
|
|
+class GridArrayAdapter {
|
|
|
+public:
|
|
|
+ enum { DATA_DIMENSION = 1 };
|
|
|
+
|
|
|
+ explicit GridArrayAdapter(const cv::Mat& grid) : grid_(grid) {}
|
|
|
+
|
|
|
+ void GetValue(const int row, const int column, double* const value) const {
|
|
|
+ if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
|
|
|
+ column >= NumCols() - kPadding) {
|
|
|
+ *value = kMaxCorrespondenceCost;
|
|
|
+ } else {
|
|
|
+ *value = static_cast<double>(grid_.at<double >(row - kPadding, column - kPadding));
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ int NumRows() const {
|
|
|
+ return grid_.rows + 2 * kPadding;
|
|
|
+ }
|
|
|
+
|
|
|
+ int NumCols() const {
|
|
|
+ return grid_.cols + 2 * kPadding;
|
|
|
+ }
|
|
|
+private:
|
|
|
+ const cv::Mat& grid_;
|
|
|
+};
|
|
|
+
|
|
|
+class CostFunctor {
|
|
|
+private:
|
|
|
+ cv::Mat m_map;
|
|
|
+ double m_scale;
|
|
|
+ pcl::PointCloud<pcl::PointXYZ> m_left_front_cloud; //左前点云
|
|
|
+ pcl::PointCloud<pcl::PointXYZ> m_right_front_cloud; //右前点云
|
|
|
+ pcl::PointCloud<pcl::PointXYZ> m_left_rear_cloud; //左后点云
|
|
|
+ pcl::PointCloud<pcl::PointXYZ> m_right_rear_cloud; //右后点云
|
|
|
+
|
|
|
+public:
|
|
|
+ CostFunctor(pcl::PointCloud<pcl::PointXYZ> left_front,pcl::PointCloud<pcl::PointXYZ> right_front,
|
|
|
+ pcl::PointCloud<pcl::PointXYZ> left_rear,pcl::PointCloud<pcl::PointXYZ> right_rear,
|
|
|
+ cv::Mat& map,double scale)
|
|
|
+ {
|
|
|
+ m_map=map;
|
|
|
+ m_scale=scale;
|
|
|
+ m_left_front_cloud=left_front;
|
|
|
+ m_right_front_cloud=right_front;
|
|
|
+ m_left_rear_cloud=left_rear;
|
|
|
+ m_right_rear_cloud=right_rear;
|
|
|
+ }
|
|
|
+ template <typename T>
|
|
|
+ bool operator()(const T* const variable, T* residual) const {
|
|
|
+ T cx = variable[0];
|
|
|
+ T cy = variable[1];
|
|
|
+ T theta = variable[2];
|
|
|
+ T length = variable[3];
|
|
|
+ T width = variable[4];
|
|
|
+ T theta_front = variable[5];
|
|
|
+
|
|
|
+ //整车旋转
|
|
|
+ Eigen::Rotation2D<T> rotation(theta);
|
|
|
+ Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
|
|
|
+
|
|
|
+ //左前偏移
|
|
|
+ Eigen::Matrix<T, 2, 1> wheel_center_normal_left_front(length / 2.0, width / 2.0);
|
|
|
+ //右前偏移
|
|
|
+ Eigen::Matrix<T, 2, 1> wheel_center_normal_right_front(length / 2.0, -width / 2.0);
|
|
|
+ //左后偏移
|
|
|
+ Eigen::Matrix<T, 2, 1> wheel_center_normal_left_rear(-length / 2.0, width / 2.0);
|
|
|
+ //右后偏移
|
|
|
+ Eigen::Matrix<T, 2, 1> wheel_center_normal_right_rear(-length / 2.0, -width / 2.0);
|
|
|
+
|
|
|
+ //前轮旋转
|
|
|
+ Eigen::Rotation2D<T> rotation_front(theta_front);
|
|
|
+ Eigen::Matrix<T, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
|
|
|
+
|
|
|
+
|
|
|
+ const GridArrayAdapter adapter(m_map);
|
|
|
+ ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
|
|
|
+
|
|
|
+ double rows = m_map.rows;
|
|
|
+ double cols = m_map.cols;
|
|
|
+ //左前轮
|
|
|
+ int left_front_num = m_left_front_cloud.size();
|
|
|
+ for (int i = 0; i < m_left_front_cloud.size(); ++i) {
|
|
|
+ const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud[i].x) - cx),
|
|
|
+ (T(m_left_front_cloud[i].y) - cy));
|
|
|
+ //减去经过车辆旋转计算的左前中心
|
|
|
+ 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),
|
|
|
+ &residual[i]);
|
|
|
+
|
|
|
+ }
|
|
|
+ //右前轮
|
|
|
+ int right_front_num = m_right_front_cloud.size();
|
|
|
+ for (int i = 0; i < m_right_front_cloud.size(); ++i) {
|
|
|
+ const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud[i].x) - cx),
|
|
|
+ (T(m_right_front_cloud[i].y) - cy));
|
|
|
+ //减去经过车辆旋转计算的左前中心
|
|
|
+ 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),
|
|
|
+ &residual[left_front_num + i]);
|
|
|
+
|
|
|
+ }
|
|
|
+ //左后轮
|
|
|
+ int left_rear_num = m_left_rear_cloud.size();
|
|
|
+
|
|
|
+ for (int i = 0; i < m_left_rear_cloud.size(); ++i) {
|
|
|
+ const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud[i].x) - cx),
|
|
|
+ (T(m_left_rear_cloud[i].y) - cy));
|
|
|
+ //减去经过车辆旋转计算的左前中心
|
|
|
+ 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),
|
|
|
+ &residual[left_front_num + right_front_num + i]);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ //右后轮
|
|
|
+
|
|
|
+ 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),
|
|
|
+ &residual[left_front_num + right_front_num + left_rear_num + i]);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+
|
|
|
+private:
|
|
|
+
|
|
|
+};
|
|
|
+
|
|
|
+detect_wheel_ceres::detect_wheel_ceres()
|
|
|
+{
|
|
|
+ /////创建地图
|
|
|
+ int cols=800;
|
|
|
+ int rows=200;
|
|
|
+ int L=std::min(rows,cols);
|
|
|
+ cv::Mat map=cv::Mat::ones(L,L,CV_64F);
|
|
|
+ //map=map*255;
|
|
|
+ cv::Point center(L/2,L/2);
|
|
|
+
|
|
|
+
|
|
|
+ float K=L*0.08;
|
|
|
+ for(int n=0;n<L;n+=2)
|
|
|
+ {
|
|
|
+ cv::Size size(n+2,n+2);
|
|
|
+ cv::Rect rect(center-cv::Point(n/2,n/2),size);
|
|
|
+ if(n<K*2)
|
|
|
+ cv::rectangle(map,rect,3.0*float(K*2-n)/float(L));
|
|
|
+ else
|
|
|
+ cv::rectangle(map,rect,float(n-K*2)/float(L));
|
|
|
+
|
|
|
+ }
|
|
|
+ cv::resize(map,map,cv::Size(cols,rows));
|
|
|
+ cv::GaussianBlur(map,m_map,cv::Size(7,7),3,3);
|
|
|
+}
|
|
|
+detect_wheel_ceres::~detect_wheel_ceres(){}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec,
|
|
|
+ double& cx,double& cy,double& theta,double& wheel_base,double& width,double& front_theta)
|
|
|
+{
|
|
|
+ //清理点云
|
|
|
+ m_left_front_cloud.clear();
|
|
|
+ m_right_front_cloud.clear();
|
|
|
+ m_left_rear_cloud.clear();
|
|
|
+ m_right_rear_cloud.clear();
|
|
|
+ //重新计算点云,按方位分割
|
|
|
+ //第一步,计算整体中心,主轴方向
|
|
|
+ pcl::PointCloud<pcl::PointXYZ> cloud_all;
|
|
|
+ for(int i=0;i<cloud_vec.size();++i)
|
|
|
+ {
|
|
|
+ cloud_all+=cloud_vec[i];
|
|
|
+ }
|
|
|
+
|
|
|
+ if(cloud_all.size()<20)
|
|
|
+ return false;
|
|
|
+
|
|
|
+ Eigen::Vector4f centroid;
|
|
|
+ pcl::compute3DCentroid(cloud_all, centroid);
|
|
|
+ double center_x=centroid[0];
|
|
|
+ double center_y=centroid[1];
|
|
|
+ //计算外接旋转矩形
|
|
|
+ std::vector<cv::Point2f> points_cv;
|
|
|
+ for(int i=0;i<cloud_all.size();++i)
|
|
|
+ {
|
|
|
+ points_cv.push_back(cv::Point2f(cloud_all[i].x,cloud_all[i].y));
|
|
|
+ }
|
|
|
+ cv::RotatedRect rotate_rect=cv::minAreaRect(points_cv);
|
|
|
+ //计算旋转矩形与X轴的夹角
|
|
|
+ cv::Point2f vec;
|
|
|
+ cv::Point2f vertice[4];
|
|
|
+ rotate_rect.points(vertice);
|
|
|
+
|
|
|
+ float len1 = pow(vertice[0].x - vertice[1].x, 2.0) + pow(vertice[0].y - vertice[1].y, 2.0);
|
|
|
+ float len2 = pow(vertice[1].x - vertice[2].x, 2.0) + pow(vertice[1].y - vertice[2].y, 2.0);
|
|
|
+ // 寻找长边,倾角为长边与x轴夹角
|
|
|
+ if (len1 > len2)
|
|
|
+ {
|
|
|
+ vec.x = vertice[0].x - vertice[1].x;
|
|
|
+ vec.y = vertice[0].y - vertice[1].y;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ vec.x = vertice[1].x - vertice[2].x;
|
|
|
+ vec.y = vertice[1].y - vertice[2].y;
|
|
|
+ }
|
|
|
+ float angle_x = 180.0 / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
|
|
|
+ //printf("rect theta: %.3f\n",angle_x);
|
|
|
+ //第二步, 将没分点云旋转回去,计算点云重心所在象限
|
|
|
+ for(int i=0;i<cloud_vec.size();++i)
|
|
|
+ {
|
|
|
+ pcl::PointCloud<pcl::PointXYZ> cloud=cloud_vec[i];
|
|
|
+ Eigen::Affine3f traslation = Eigen::Affine3f::Identity();//初始化变换矩阵为单位矩阵
|
|
|
+ // 平移
|
|
|
+ traslation.translation() << -center_x, -center_y, 0.0;
|
|
|
+ pcl::PointCloud<pcl::PointXYZ> translate_cloud;
|
|
|
+ pcl::transformPointCloud(cloud, translate_cloud, traslation);
|
|
|
+
|
|
|
+ // 旋转; Z 轴上旋转angle_x 弧度,Y轴上旋转0弧度
|
|
|
+ Eigen::Affine3f rotation = Eigen::Affine3f::Identity();
|
|
|
+ rotation.rotate(Eigen::AngleAxisf(-angle_x*M_PI/180.0, Eigen::Vector3f::UnitZ()));
|
|
|
+
|
|
|
+ pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
|
|
|
+ pcl::transformPointCloud(translate_cloud, transformed_cloud, rotation);
|
|
|
+
|
|
|
+ //计算重心
|
|
|
+ Eigen::Vector4f centroid;
|
|
|
+ pcl::compute3DCentroid(transformed_cloud, centroid);
|
|
|
+ double x=centroid[0];
|
|
|
+ double y=centroid[1];
|
|
|
+ //计算象限
|
|
|
+ if(x>0&&y>0)
|
|
|
+ {
|
|
|
+ //第一象限
|
|
|
+ m_left_front_cloud=cloud;
|
|
|
+ }
|
|
|
+ if(x>0 && y<0)
|
|
|
+ {
|
|
|
+ //第四象限
|
|
|
+ m_right_front_cloud=cloud;
|
|
|
+ }
|
|
|
+ if(x<0 && y>0)
|
|
|
+ {
|
|
|
+ //第二象限
|
|
|
+ m_left_rear_cloud=cloud;
|
|
|
+ }
|
|
|
+ if(x<0 && y<0)
|
|
|
+ {
|
|
|
+ //第三象限
|
|
|
+ m_right_rear_cloud=cloud;
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ cx=center_x;
|
|
|
+ cy=center_y;
|
|
|
+ theta=-angle_x*M_PI/180.0;
|
|
|
+ return Solve(cx,cy,theta,wheel_base,width,front_theta);
|
|
|
+}
|
|
|
+
|
|
|
+bool detect_wheel_ceres::Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta)
|
|
|
+{
|
|
|
+ double SCALE=300.0;
|
|
|
+ double cx=x;
|
|
|
+ double cy=y;
|
|
|
+ double init_theta=theta;
|
|
|
+ double init_wheel_base=2.7;
|
|
|
+ double init_width=1.55;
|
|
|
+ double init_theta_front=0*M_PI/180.0;
|
|
|
+
|
|
|
+ double variable[] = {cx,cy,init_theta,init_wheel_base,init_width,init_theta_front};
|
|
|
+
|
|
|
+ // 第二部分:构建寻优问题
|
|
|
+ ceres::Problem problem;
|
|
|
+ //使用自动求导,将之前的代价函数结构体传入,第一个1是输出维度,即残差的维度,第二个1是输入维度,即待寻优参数x的维度。
|
|
|
+ ceres::CostFunction* cost_function =new
|
|
|
+ ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 6>(
|
|
|
+ new CostFunctor(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_map,SCALE),
|
|
|
+ m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
|
|
|
+ problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项,本问题比较简单,添加一个就行。
|
|
|
+
|
|
|
+
|
|
|
+ //第三部分: 配置并运行求解器
|
|
|
+ ceres::Solver::Options options;
|
|
|
+ options.use_nonmonotonic_steps=false;
|
|
|
+ options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
|
|
|
+ options.max_num_iterations=20;
|
|
|
+ options.num_threads=1;
|
|
|
+ options.minimizer_progress_to_stdout = false;//输出到cout
|
|
|
+ ceres::Solver::Summary summary;//优化信息
|
|
|
+ ceres::Solve(options, &problem, &summary);//求解!!!
|
|
|
+
|
|
|
+ //std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
|
|
|
+
|
|
|
+ /*printf("x:%.3f,y:%.3f,theta:%.3f\nlength:%.3f,width:%.3f\ntheta_front:%.3f\n",
|
|
|
+ x[0],x[1],x[2]*180.0/M_PI,x[3],x[4],x[5]*180.0/M_PI);*/
|
|
|
+
|
|
|
+ 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());
|
|
|
+
|
|
|
+
|
|
|
+ x=variable[0];
|
|
|
+ y=variable[1];
|
|
|
+ theta=(-variable[2])*180.0/M_PI;
|
|
|
+ wheel_base=variable[3];
|
|
|
+ width=variable[4];
|
|
|
+ front_theta=-(variable[5]*180.0/M_PI);
|
|
|
+
|
|
|
+ if(theta>180.0)
|
|
|
+ theta=theta-180.0;
|
|
|
+ if(theta<0)
|
|
|
+ theta+=180.0;
|
|
|
+
|
|
|
+ //检验
|
|
|
+ if(loss>0.03)
|
|
|
+ return false;
|
|
|
+ if (width < 1.350 || width > 2.000 || wheel_base > 3.000 || wheel_base < 2.200)
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ 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);
|
|
|
+ return true;
|
|
|
+
|
|
|
+}
|