73 {
74 if (obstacles_num != obstacles_vertices_vec.size()) {
75 AINFO <<
"obstacles_num != obstacles_vertices_vec.size()";
76 return false;
77 }
78
79 A_all->resize(obstacles_edges_num.sum(), 2);
80 b_all->resize(obstacles_edges_num.sum(), 1);
81
82 int counter = 0;
83 double kEpsilon = 1.0e-5;
84
85 for (size_t i = 0; i < obstacles_num; ++i) {
86 size_t current_vertice_num = obstacles_edges_num(i, 0);
87 Eigen::MatrixXd A_i(current_vertice_num, 2);
88 Eigen::MatrixXd b_i(current_vertice_num, 1);
89
90
91 for (size_t j = 0; j < current_vertice_num; ++j) {
92 Vec2d v1 = obstacles_vertices_vec[i][j];
93 Vec2d v2 = obstacles_vertices_vec[i][j + 1];
94
95 Eigen::MatrixXd A_tmp(2, 1), b_tmp(1, 1), ab(2, 1);
96
97 if (std::abs(v1.x() - v2.x()) < kEpsilon) {
98 if (v2.y() < v1.y()) {
99 A_tmp << 1, 0;
100 b_tmp << v1.x();
101 } else {
102 A_tmp << -1, 0;
103 b_tmp << -v1.x();
104 }
105 } else if (std::abs(v1.y() - v2.y()) < kEpsilon) {
106 if (v1.x() < v2.x()) {
107 A_tmp << 0, 1;
108 b_tmp << v1.y();
109 } else {
110 A_tmp << 0, -1;
111 b_tmp << -v1.y();
112 }
113 } else {
114 Eigen::MatrixXd tmp1(2, 2);
115 tmp1 << v1.x(), 1, v2.x(), 1;
116 Eigen::MatrixXd tmp2(2, 1);
117 tmp2 << v1.y(), v2.y();
118 ab = tmp1.inverse() * tmp2;
119 double a = ab(0, 0);
120 double b = ab(1, 0);
121
122 if (v1.x() < v2.x()) {
123 A_tmp << -a, 1;
124 b_tmp << b;
125 } else {
126 A_tmp << a, -1;
127 b_tmp << -b;
128 }
129 }
130
131
132 A_i.block(j, 0, 1, 2) = A_tmp.transpose();
133 b_i.block(j, 0, 1, 1) = b_tmp;
134 }
135
136 A_all->block(counter, 0, A_i.rows(), 2) = A_i;
137 b_all->block(counter, 0, b_i.rows(), 1) = b_i;
138 counter += static_cast<int>(current_vertice_num);
139 }
140 return true;
141 }