|
@@ -13,6 +13,18 @@ void YoloSegmentMqttAsyncClient::init(const std::string &file) {
|
|
|
|
|
|
}
|
|
|
|
|
|
+bool isElliptical(float x, float y, float a2, float b2) {
|
|
|
+ return (x * x / a2 + y * y / b2) < 1;
|
|
|
+}
|
|
|
+
|
|
|
+uint32_t getPixelIndex(int row, int col) {
|
|
|
+ // LOG(INFO) << "col " << col << ", row " << row;
|
|
|
+ col = MAX(0, MIN(1280, col)) % 640;
|
|
|
+ row = MAX(0, MIN(960, row)) % 480;
|
|
|
+ // LOG(INFO) << "col " << col << ", row " << row;
|
|
|
+ return row * 640 + col;
|
|
|
+}
|
|
|
+
|
|
|
// 识别结果数据包含识别结果、时间序号、具体识别信息
|
|
|
int YoloSegmentMqttAsyncClient::CloudDataArrived(void *client, char *topicName, int topicLen,
|
|
|
MQTTAsync_message *message) {
|
|
@@ -62,30 +74,53 @@ int YoloSegmentMqttAsyncClient::CloudDataArrived(void *client, char *topicName,
|
|
|
seg_results.mutable_boxes(device_index)->set_height(iter->rect.height);
|
|
|
seg_results.mutable_boxes(device_index)->set_confidence(iter->prob);
|
|
|
if (iter->prob > 0.6) {
|
|
|
- // LOG(INFO) << device_index << ": " << iter->prob;
|
|
|
- int box_contour[480][2] = {0};
|
|
|
- for (auto &pt: seg_points) { // use 7.5~9ms
|
|
|
- pt.x -= x_alpha * merge_mat.cols / 2;
|
|
|
- pt.y -= y_alpha * merge_mat.rows / 2;
|
|
|
- if (box_contour[pt.y][0] == 0 && box_contour[pt.y][1] == 0) {
|
|
|
- box_contour[pt.y][0] = pt.x;
|
|
|
- box_contour[pt.y][1] = pt.x;
|
|
|
- } else {
|
|
|
- box_contour[pt.y][0] = MIN(pt.x, box_contour[pt.y][0]);
|
|
|
- box_contour[pt.y][1] = MAX(pt.x, box_contour[pt.y][1]);
|
|
|
+ // 内外椭圆参数
|
|
|
+ float a1 = iter->rect.width * 0.5;
|
|
|
+ float b1 = iter->rect.height * 0.5;
|
|
|
+ float a2 = iter->rect.width * 0.57;
|
|
|
+ float b2 = iter->rect.height * 0.57;
|
|
|
+
|
|
|
+ //
|
|
|
+ int center_x = iter->rect.x + iter->rect.width * 0.5;
|
|
|
+ int center_y = iter->rect.y + iter->rect.height * 0.5;
|
|
|
+ int min_x = MAX(0, center_x - a2);
|
|
|
+ int min_y = MAX(0, center_y - b2);
|
|
|
+ float a12 = a1 * a1;
|
|
|
+ float b12 = b1 * b1;
|
|
|
+ float a22 = a2 * a2;
|
|
|
+ float b22 = b2 * b2;
|
|
|
+
|
|
|
+ for (int x_value = min_x; x_value < center_x; x_value++) {
|
|
|
+ auto t_x_value = x_value - center_x;
|
|
|
+ for (int y_value = min_y; y_value < center_y; y_value++) {
|
|
|
+ auto t_y_value = y_value - center_y;
|
|
|
+
|
|
|
+ if (!isElliptical(t_x_value, t_y_value, a22, b22)) {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ auto x_value_sym = 2 * center_x - x_value;
|
|
|
+ auto y_value_sym = 2 * center_y - y_value;
|
|
|
+ if (isElliptical(t_x_value, t_y_value, a12, b12)) {
|
|
|
+ seg_results.mutable_boxes(device_index)->add_wheels()->set_point(getPixelIndex(y_value, x_value));
|
|
|
+ seg_results.mutable_boxes(device_index)->add_wheels()->set_point(getPixelIndex(y_value, x_value_sym));
|
|
|
+ seg_results.mutable_boxes(device_index)->add_wheels()->set_point(getPixelIndex(y_value_sym, x_value));
|
|
|
+ seg_results.mutable_boxes(device_index)->add_wheels()->set_point(getPixelIndex(y_value_sym, x_value_sym));
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ seg_results.mutable_boxes(device_index)->add_tires()->set_point(getPixelIndex(y_value, x_value));
|
|
|
+ seg_results.mutable_boxes(device_index)->add_tires()->set_point(getPixelIndex(y_value, x_value_sym));
|
|
|
+ seg_results.mutable_boxes(device_index)->add_tires()->set_point(getPixelIndex(y_value_sym, x_value));
|
|
|
+ seg_results.mutable_boxes(device_index)->add_tires()->set_point(getPixelIndex(y_value_sym, x_value_sym));
|
|
|
}
|
|
|
}
|
|
|
- for (auto &ends: box_contour) {
|
|
|
- auto line = seg_results.mutable_boxes(device_index)->add_lines();
|
|
|
- line->set_begin(ends[0]);
|
|
|
- line->set_end(ends[1]);
|
|
|
- }
|
|
|
}
|
|
|
}
|
|
|
+ // cv::imshow("merge_mat", merge_mat);
|
|
|
+ // cv::waitKey(1);
|
|
|
|
|
|
char data[seg_results.ByteSizeLong()];
|
|
|
seg_results.SerializeToArray((void *) data, seg_results.ByteSizeLong());
|
|
|
tof_client->SendMessage("tof/seg", data, seg_results.ByteSizeLong());
|
|
|
}
|
|
|
return 1;
|
|
|
-}
|
|
|
+}
|