detect_wheel_ceres.cpp 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401
  1. //
  2. // Created by zx on 2020/7/1.
  3. //
  4. #include "detect_wheel_ceres.h"
  5. #include <ceres/cubic_interpolation.h>
  6. #include <pcl/common/transforms.h>
  7. constexpr float kMinProbability = 0.0f;
  8. constexpr float kMaxProbability = 1.f - kMinProbability;
  9. constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
  10. constexpr int kPadding = INT_MAX / 4;
  11. class GridArrayAdapter {
  12. public:
  13. enum { DATA_DIMENSION = 1 };
  14. explicit GridArrayAdapter(const cv::Mat& grid) : grid_(grid) {}
  15. void GetValue(const int row, const int column, double* const value) const {
  16. if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
  17. column >= NumCols() - kPadding) {
  18. *value = kMaxCorrespondenceCost;
  19. } else {
  20. *value = static_cast<double>(grid_.at<double >(row - kPadding, column - kPadding));
  21. }
  22. }
  23. int NumRows() const {
  24. return grid_.rows + 2 * kPadding;
  25. }
  26. int NumCols() const {
  27. return grid_.cols + 2 * kPadding;
  28. }
  29. private:
  30. const cv::Mat& grid_;
  31. };
  32. class CostFunctor {
  33. private:
  34. cv::Mat m_map;
  35. double m_scale;
  36. pcl::PointCloud<pcl::PointXYZ> m_left_front_cloud; //左前点云
  37. pcl::PointCloud<pcl::PointXYZ> m_right_front_cloud; //右前点云
  38. pcl::PointCloud<pcl::PointXYZ> m_left_rear_cloud; //左后点云
  39. pcl::PointCloud<pcl::PointXYZ> m_right_rear_cloud; //右后点云
  40. public:
  41. CostFunctor(pcl::PointCloud<pcl::PointXYZ> left_front,pcl::PointCloud<pcl::PointXYZ> right_front,
  42. pcl::PointCloud<pcl::PointXYZ> left_rear,pcl::PointCloud<pcl::PointXYZ> right_rear,
  43. cv::Mat& map,double scale)
  44. {
  45. m_map=map;
  46. m_scale=scale;
  47. m_left_front_cloud=left_front;
  48. m_right_front_cloud=right_front;
  49. m_left_rear_cloud=left_rear;
  50. m_right_rear_cloud=right_rear;
  51. }
  52. template <typename T>
  53. bool operator()(const T* const variable, T* residual) const {
  54. T cx = variable[0];
  55. T cy = variable[1];
  56. T theta = variable[2];
  57. T length = variable[3];
  58. T width = variable[4];
  59. T theta_front = variable[5];
  60. //整车旋转
  61. Eigen::Rotation2D<T> rotation(theta);
  62. Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
  63. //左前偏移
  64. Eigen::Matrix<T, 2, 1> wheel_center_normal_left_front(length / 2.0, width / 2.0);
  65. //右前偏移
  66. Eigen::Matrix<T, 2, 1> wheel_center_normal_right_front(length / 2.0, -width / 2.0);
  67. //左后偏移
  68. Eigen::Matrix<T, 2, 1> wheel_center_normal_left_rear(-length / 2.0, width / 2.0);
  69. //右后偏移
  70. Eigen::Matrix<T, 2, 1> wheel_center_normal_right_rear(-length / 2.0, -width / 2.0);
  71. //前轮旋转
  72. Eigen::Rotation2D<T> rotation_front(theta_front);
  73. Eigen::Matrix<T, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
  74. const GridArrayAdapter adapter(m_map);
  75. ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
  76. double rows = m_map.rows;
  77. double cols = m_map.cols;
  78. //左前轮
  79. int left_front_num = m_left_front_cloud.size();
  80. for (int i = 0; i < m_left_front_cloud.size(); ++i) {
  81. const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud[i].x) - cx),
  82. (T(m_left_front_cloud[i].y) - cy));
  83. //减去经过车辆旋转计算的左前中心
  84. const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
  85. //旋转
  86. const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
  87. interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
  88. point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
  89. &residual[i]);
  90. }
  91. //右前轮
  92. int right_front_num = m_right_front_cloud.size();
  93. for (int i = 0; i < m_right_front_cloud.size(); ++i) {
  94. const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud[i].x) - cx),
  95. (T(m_right_front_cloud[i].y) - cy));
  96. //减去经过车辆旋转计算的左前中心
  97. const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
  98. //旋转
  99. const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
  100. interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
  101. point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
  102. &residual[left_front_num + i]);
  103. }
  104. //左后轮
  105. int left_rear_num = m_left_rear_cloud.size();
  106. for (int i = 0; i < m_left_rear_cloud.size(); ++i) {
  107. const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud[i].x) - cx),
  108. (T(m_left_rear_cloud[i].y) - cy));
  109. //减去经过车辆旋转计算的左前中心
  110. const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
  111. interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
  112. tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
  113. &residual[left_front_num + right_front_num + i]);
  114. }
  115. //右后轮
  116. for (int i = 0; i < m_right_rear_cloud.size(); ++i) {
  117. const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud[i].x) - cx),
  118. (T(m_right_rear_cloud[i].y) - cy));
  119. //减去经过车辆旋转计算的左前中心
  120. const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
  121. interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
  122. tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
  123. &residual[left_front_num + right_front_num + left_rear_num + i]);
  124. }
  125. return true;
  126. }
  127. private:
  128. };
  129. bool detect_wheel_ceres::update_mat(int rows, int cols)
  130. {
  131. /////创建地图
  132. int L=std::min(rows,cols);
  133. cv::Mat map=cv::Mat::ones(L,L,CV_64F);
  134. //map=map*255;
  135. cv::Point center(L/2,L/2);
  136. float K=L*0.08;
  137. // 从中心开始向外画矩形
  138. for(int n=0;n<L;n+=2)
  139. {
  140. cv::Size size(n+2,n+2);
  141. cv::Rect rect(center-cv::Point(n/2,n/2),size);
  142. // LOG(INFO) << rect.tl().x<<", "<<rect.tl().y<<", "<<rect.br().x<<", "<<rect.br().y;
  143. // 对内外分别取色
  144. if(n<K*2){
  145. cv::rectangle(map,rect,3.0*float(K*2-n)/float(L));
  146. }
  147. else{
  148. cv::rectangle(map,rect,float(n-K*2)/float(L));
  149. }
  150. }
  151. cv::resize(map,map,cv::Size(cols,rows));
  152. cv::GaussianBlur(map,m_map,cv::Size(7,7),3,3);
  153. }
  154. detect_wheel_ceres::detect_wheel_ceres()
  155. {
  156. int cols=800;
  157. int rows=200;
  158. update_mat(rows, cols);
  159. }
  160. detect_wheel_ceres::~detect_wheel_ceres(){}
  161. bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec,
  162. double& cx,double& cy,double& theta,double& wheel_base,double& width,double& front_theta)
  163. {
  164. //清理点云
  165. m_left_front_cloud.clear();
  166. m_right_front_cloud.clear();
  167. m_left_rear_cloud.clear();
  168. m_right_rear_cloud.clear();
  169. //重新计算点云,按方位分割
  170. //第一步,计算整体中心,主轴方向
  171. pcl::PointCloud<pcl::PointXYZ> cloud_all;
  172. for(int i=0;i<cloud_vec.size();++i)
  173. {
  174. cloud_all+=cloud_vec[i];
  175. }
  176. if(cloud_all.size()<20)
  177. return false;
  178. Eigen::Vector4f centroid;
  179. pcl::compute3DCentroid(cloud_all, centroid);
  180. double center_x=centroid[0];
  181. double center_y=centroid[1];
  182. //计算外接旋转矩形
  183. std::vector<cv::Point2f> points_cv;
  184. for(int i=0;i<cloud_all.size();++i)
  185. {
  186. points_cv.push_back(cv::Point2f(cloud_all[i].x,cloud_all[i].y));
  187. }
  188. cv::RotatedRect rotate_rect=cv::minAreaRect(points_cv);
  189. //计算旋转矩形与X轴的夹角
  190. cv::Point2f vec;
  191. cv::Point2f vertice[4];
  192. rotate_rect.points(vertice);
  193. float len1 = pow(vertice[0].x - vertice[1].x, 2.0) + pow(vertice[0].y - vertice[1].y, 2.0);
  194. float len2 = pow(vertice[1].x - vertice[2].x, 2.0) + pow(vertice[1].y - vertice[2].y, 2.0);
  195. // 寻找长边,倾角为长边与x轴夹角
  196. if (len1 > len2)
  197. {
  198. vec.x = vertice[0].x - vertice[1].x;
  199. vec.y = vertice[0].y - vertice[1].y;
  200. }
  201. else
  202. {
  203. vec.x = vertice[1].x - vertice[2].x;
  204. vec.y = vertice[1].y - vertice[2].y;
  205. }
  206. float angle_x = 180.0 / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
  207. //printf("rect theta: %.3f\n",angle_x);
  208. //第二步, 将没分点云旋转回去,计算点云重心所在象限
  209. for(int i=0;i<cloud_vec.size();++i)
  210. {
  211. pcl::PointCloud<pcl::PointXYZ> cloud=cloud_vec[i];
  212. Eigen::Affine3f traslation = Eigen::Affine3f::Identity();//初始化变换矩阵为单位矩阵
  213. // 平移
  214. traslation.translation() << -center_x, -center_y, 0.0;
  215. pcl::PointCloud<pcl::PointXYZ> translate_cloud;
  216. pcl::transformPointCloud(cloud, translate_cloud, traslation);
  217. // 旋转; Z 轴上旋转angle_x 弧度,Y轴上旋转0弧度
  218. Eigen::Affine3f rotation = Eigen::Affine3f::Identity();
  219. rotation.rotate(Eigen::AngleAxisf(-angle_x*M_PI/180.0, Eigen::Vector3f::UnitZ()));
  220. pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
  221. pcl::transformPointCloud(translate_cloud, transformed_cloud, rotation);
  222. //计算重心
  223. Eigen::Vector4f centroid;
  224. pcl::compute3DCentroid(transformed_cloud, centroid);
  225. double x=centroid[0];
  226. double y=centroid[1];
  227. //计算象限
  228. if(x>0&&y>0)
  229. {
  230. //第一象限
  231. m_left_front_cloud=cloud;
  232. }
  233. if(x>0 && y<0)
  234. {
  235. //第四象限
  236. m_right_front_cloud=cloud;
  237. }
  238. if(x<0 && y>0)
  239. {
  240. //第二象限
  241. m_left_rear_cloud=cloud;
  242. }
  243. if(x<0 && y<0)
  244. {
  245. //第三象限
  246. m_right_rear_cloud=cloud;
  247. }
  248. }
  249. // printf("origin: x:%.3f,y:%.3f,theta:%.3f\nlength:%.3f,width:%.3f\ntheta_front:%.3f\n",
  250. // center_x, center_y, angle_x, wheel_base, width, front_theta * 180.0 / M_PI);
  251. cx=center_x;
  252. cy=center_y;
  253. double final_theta = 0;
  254. double avg_loss = 100;
  255. int map_cols = 800;
  256. bool solve_result = false;
  257. for (int j = 2; j < 5; j++)
  258. {
  259. double input_vars[] = {cx, cy, theta, wheel_base, width, front_theta};
  260. double map_rows = map_cols * 1.0 / j ;
  261. update_mat(map_rows, map_cols);
  262. // 寻找最小loss值对应的初始旋转角
  263. for (int i = -5; i < 6; i++)
  264. {
  265. double t_loss = 1;
  266. input_vars[2] = (-angle_x + i) * M_PI / 180.0;
  267. // printf("middle 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]);
  268. bool current_result = Solve(input_vars[0], input_vars[1], input_vars[2], input_vars[3], input_vars[4], input_vars[5], t_loss);
  269. // LOG(INFO) << "avg loss, current theta: " << t_loss << ", " << input_vars[2];
  270. if (avg_loss > t_loss)
  271. {
  272. avg_loss = t_loss;
  273. // final_theta = -input_vars[2] * M_PI / 180.0;
  274. cx = input_vars[0];
  275. cy = input_vars[1];
  276. theta = input_vars[2];
  277. wheel_base = input_vars[3];
  278. width = input_vars[4];
  279. front_theta = input_vars[5];// * M_PI / 180.0;
  280. solve_result = current_result;
  281. }
  282. }
  283. }
  284. // LOG(INFO) << "avg loss, final theta: "<<avg_loss <<", "<<final_theta;
  285. // theta = -final_theta * M_PI / 180.0;
  286. //printf("final x:%.3f y: %.3f wheel:%.3f width:%.3f theta : %.3f front theta: %.3f\n",cx,cy,wheel_base,width,theta,front_theta);
  287. return solve_result;//Solve(cx, cy, theta, wheel_base, width, front_theta, avg_loss);
  288. }
  289. bool detect_wheel_ceres::Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta,double &avg_loss)
  290. {
  291. double SCALE=300.0;
  292. double cx=x;
  293. double cy=y;
  294. double init_theta=theta;
  295. double init_wheel_base=2.7;
  296. double init_width=1.55;
  297. double init_theta_front=0*M_PI/180.0;
  298. double variable[] = {cx,cy,init_theta,init_wheel_base,init_width,init_theta_front};
  299. // 第二部分:构建寻优问题
  300. ceres::Problem problem;
  301. //使用自动求导,将之前的代价函数结构体传入,第一个1是输出维度,即残差的维度,第二个1是输入维度,即待寻优参数x的维度。
  302. ceres::CostFunction* cost_function =new
  303. ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 6>(
  304. new CostFunctor(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_map,SCALE),
  305. m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
  306. problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项,本问题比较简单,添加一个就行。
  307. //第三部分: 配置并运行求解器
  308. ceres::Solver::Options options;
  309. options.use_nonmonotonic_steps=false;
  310. options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
  311. options.max_num_iterations=60;
  312. options.num_threads=1;
  313. options.minimizer_progress_to_stdout = false;//输出到cout
  314. ceres::Solver::Summary summary;//优化信息
  315. ceres::Solve(options, &problem, &summary);//求解!!!
  316. // std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
  317. /*printf("x:%.3f,y:%.3f,theta:%.3f\nlength:%.3f,width:%.3f\ntheta_front:%.3f\n",
  318. x[0],x[1],x[2]*180.0/M_PI,x[3],x[4],x[5]*180.0/M_PI);*/
  319. 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());
  320. x=variable[0];
  321. y=variable[1];
  322. theta=(-variable[2])*180.0/M_PI;
  323. wheel_base=variable[3];
  324. width=variable[4];
  325. front_theta=-(variable[5]*180.0/M_PI);
  326. if(theta>180.0)
  327. theta=theta-180.0;
  328. if(theta<0)
  329. theta+=180.0;
  330. //检验
  331. if(loss>0.03)
  332. return false;
  333. if (width < 1.350 || width > 2.000 || wheel_base > 3.000 || wheel_base < 2.200)
  334. {
  335. return false;
  336. }
  337. width+=0.15;//车宽+10cm
  338. // printf(" x:%.3f y: %.3f wheel:%.3f width:%.3f theta : %.3f front theta: %.3f\n",x,y,wheel_base,width,theta,front_theta);
  339. // //added by yct
  340. avg_loss = loss; // 将loss传出
  341. return true;
  342. }