深蓝学院自主泊车第5次作业-建图

YMWM_

已于 2025-03-15 22:10:41 修改

阅读量344

点赞数 5

分类专栏：自主泊车-深蓝学院文章标签：自主泊车

于 2025-03-01 19:36:11 首次发布

本文链接：https://blog.youkuaiyun.com/YMWM_/article/details/145951583

版权

自主泊车-深蓝学院专栏收录该内容

7 篇文章

订阅专栏

1 题目

(1)补全avp_mapping.cc中的ipmPlane2Global()函数。
(2)补全slot.cc中的detectSlot()函数。
(3)解释图像处理操作在extractSlot()中的作用，调整它们的参数，并尝试设计你自己的图像处理方法，你的结果是否更好？

2 解答

2.1 坐标转换函数

ipm坐标，转到车体系中心下，然后转到车体rear系下，最后转到全局系下。代码如下，

Eigen::Vector3d AvpMapping::ipmPlane2Global(const TimedPose &T_world_vehicle, const cv::Point2f &ipm_point){
  Eigen::Vector3d pt_global;
   TODO: transform ipm pixel to point in global ///
  
  //计算车体中心系下的坐标pt_vehicle_center
  Eigen::Vector3d pt_vehicle_center(-(500.0 - ipm_point.x) * 0.02,
                                    (500.0 - ipm_point.y) * 0.02,
                                    0.0);
  //计算车体后面系的坐标pt_vehicle_rear
  Eigen::Vector3d pt_vehicle_rear = pt_vehicle_center;
  pt_vehicle_rear[1] += 1.32;

  //计算全局系下的坐标pt_global 
  Eigen::Vector3d t = T_world_vehicle.t_;
  Eigen::Matrix3d R = T_world_vehicle.R_.toRotationMatrix();
  pt_global = R * pt_vehicle_rear + t;

  return pt_global;
}

2.2 检测车位

分两大步骤，分别是检测角点和复原车位。

检测角点重要步骤为：
(1)两两直线求交点。
(2)两直线距离不能过远。
(3)两直线的夹角需要在90度附近。
(4)交点的标签需要是库位线。
(5)需要合并相近的交点。使用并查集来实现。

复原车位重要步骤为：
(1)两两交点进行复原。
(2)两点的距离是否和短边相近。且它们的连线上有一定数量的点的label为库位线。
(3)正反计算两个垂线，垂线上是否有一定数量的点的label为库位线。

代码如下，

#include "avp_mapping/slot.h"
#include "avp_mapping/unionfind.h" 

namespace {
double computeCenterDistance(cv::Vec4i line1, cv::Vec4i line2) {
  cv::Point2f p1 ((line1[0] + line1[2]) / 2.0f,
                  (line1[1] + line1[3]) / 2.0f);
  cv::Point2f p2 ((line2[0] + line2[2]) / 2.0f,
                  (line2[1] + line2[3]) / 2.0f);
  return cv::norm(p1 - p2);
}

double computeTheta(cv::Vec4i line1, cv::Vec4i line2) {
  int x1 = line1[0], y1 = line1[1], x2 = line1[2], y2 = line1[3];
  cv::Point2f vec1(x2 - x1, y2 - y1);
  int x3 = line2[0], y3 = line2[1], x4 = line2[2], y4 = line2[3];
  cv::Point2f vec2(x4 - x3, y4 - y3);
  double cos_theta = (vec1.x * vec2.x + vec1.y * vec2.y) / cv::norm(vec1) / cv::norm(vec2);
  cos_theta = std::max(-1.0, std::min(cos_theta, 1.0));
  double theta = std::acos(cos_theta); //弧度制,[0,pi]之间
  theta *= kToDeg; //转成角度
  return theta;
}

cv::Point2f computeIntersect(cv::Vec4i line1, cv::Vec4i line2) {
  int x1 = line1[0], y1 = line1[1], x2 = line1[2], y2 = line1[3];
  int x3 = line2[0], y3 = line2[1], x4 = line2[2], y4 = line2[3];
  float denom = (float)((x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4));
  cv::Point2f intersect( (x1 * y2 - y1 * x2) * (x3 - x4) - (x1 - x2) * (x3 * y4 - y3 * x4),
                         (x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (x3 * y4 - y3 * x4));
  return intersect / denom;
}
} // end of namespace

// 保留边角、细小区域 和骨架
cv::Mat skeletonize(const cv::Mat &img) {
  cv::Mat skel(img.size(), CV_8UC1, cv::Scalar(0));
  cv::Mat element = cv::getStructuringElement(cv::MORPH_CROSS, cv::Size(3, 3));
  cv::Mat temp, eroded;
  do {
    cv::erode(img, eroded, element);
    cv::dilate(eroded, temp, element);
    cv::subtract(img, temp, temp);  // 细小区域
    cv::bitwise_or(skel, temp, skel);
    eroded.copyTo(img);
  } while (0 != cv::countNonZero(img));
  return skel;
}

void removeIsolatedPixels(cv::Mat &src, int min_neighbors) {
  CV_Assert(src.type() == CV_8UC1);  // 确保单通道灰度图
  cv::Mat dst = src.clone();
  for (int y = 1; y < src.rows - 1; ++y) {
    for (int x = 1; x < src.cols - 1; ++x) {
      if (src.at<uchar>(y, x) > 0) {  // 如果是前景图像
        int neighbor_count = 0;       // 相邻前景像素的计数器
        // 检查8邻域内的像素
        for (int ny = -1; ny <= 1; ++ny) {
          for (int nx = -1; nx <= 1; ++nx) {
            if (ny == 0 && nx == 0) continue;  // 跳过自己
            if (src.at<uchar>(y + ny, x + nx) > 0) {
              ++neighbor_count;  // 增加邻居数
            }
          }
        }
        // 如果相邻前景像素小于阈值，则认为该点是孤立的，并将其去除
        if (neighbor_count < min_neighbors) {
          dst.at<uchar>(y, x) = 0;
        }
      }
    }
  }
  src = dst;
}

bool isSlotShortLine(const cv::Point2f &point1,
                     const cv::Point2f &point2,
                     const cv::Mat &image) {
  cv::LineIterator it(image, point1, point2, 4);
  int positiveIndex = 0;
  const double len = cv::norm(point1 - point2);
  int delta = 10;
  if (std::fabs(len - kShortLinePixelDistance) < delta) {
    for (int i = 0; i < it.count; ++i, ++it) {
      int color = image.at<uchar>(std::round(it.pos().y), std::round(it.pos().x));
      if (color > 0) {
        positiveIndex++;
      }
    }
    if (positiveIndex > kShortLineDetectPixelNumberMin) {
      return true;
    }
  }
  return false;
}

bool isSlotLongLine(const cv::Mat &line_img,
                    const cv::Point2f &start,
                    const cv::Point2f &dir) {
  int cnt{0};
  cv::Point2f pt(start);
  for (int l = 1; l < kLongLinePixelDistance; ++l) {
    pt += dir;
    if (pt.y <= 0 || pt.y >= kIPMImgHeight ||
        pt.x <= 0 || pt.x >= kIPMImgWidth) {
      continue;
    }
    if (line_img.at<uchar>(pt) > 0) {
      ++cnt;
    }
  }
  return cnt > kLongLineDetectPixelNumberMin;
}


std::vector<cv::Point2f> merge_too_close_corner(const std::vector<cv::Point2f>& corners, const int distance_threshold) {
  int n = corners.size();
  UnionFind uf = UnionFind(n); //此处应该是一个聚类,勉强可以使用并查集来求解 
  for (int i = 0; i < n; ++i) {
    for (int j = i + 1; j < n; j++) {
      float dis = cv::norm(corners[i] - corners[j]);
      if (dis < static_cast<float>(distance_threshold)) {
        uf.merge(i, j);
      }
    }
  }

  //合并结点
  std::vector<cv::Point2f> res;
  std::vector<std::vector<int>> all_collections = uf.get_all_collections();
  for (auto collection : all_collections) {
    int m = collection.size();
    cv::Point2f pt(0.0, 0.0);
    for (int i : collection) {
      pt += corners[i];
    }
    pt = pt / static_cast<float>(m);
    res.emplace_back(pt);
  }
  return res;
}

bool check_slot_label(const cv::Mat &slot_img, int v, int u, int neighbor_dis) {
  int n = slot_img.rows; //slot_img的行数
  int m = slot_img.cols;

  //return false表示没有库位线标签
  for (int dx = -neighbor_dis; dx < neighbor_dis; ++dx) {
    for (int dy = -neighbor_dis; dy < neighbor_dis; ++dy) {
      int nv = v + dx;
      int nu = u + dy;
      if (nv < 0 || nv >= n || nu < 0 || nu >= m) {
        continue;
      }
      if (slot_img.at<uchar>(nv, nu) > 0) { //slot_img中只有0或254; 254才表示库位线
        return true;
      }
    }
  }
  return false;

}

std::tuple<std::vector<cv::Point2f>, std::vector<cv::Point2f>> detectSlot(
    const cv::Mat &slot_img, const std::vector<cv::Vec4i> &lines) {
  /// TODO 1 detect corners from lines ///
  std::vector<cv::Point2f> corners; // corners detected from lines

  //lines中存储线段的起点坐标和终点坐标
  int line_size = lines.size();
  for (int i = 0; i < line_size; ++i) {
    cv::Vec4i line1 = lines[i];
    for (int j = i + 1; j < line_size; ++j) {
      cv::Vec4i line2 = lines[j];

      //如果两条线距离过远,则不计算交点4.2米*6.4米 
      //对角线长度为7.66米
      //kPixelScaleInv
      if (computeCenterDistance(line1, line2) > 10.0 * kPixelScaleInv) {
        //两条直线的距离超过10.0米
        continue;
      }

      //如果两条线角度不在90度附近,则不计算交点
      double theta = computeTheta(line1, line2); //角度制,0~180.0
      if (std::fabs(theta-90.0) > 20.0) {
        //两条直线的夹角不在90度附近
        continue;
      }

      //两条直线的交点是不是库位线标签 
      cv::Point2f intersection = computeIntersect(line1, line2);
      int u = static_cast<int>(std::round(intersection.x));
      int v = static_cast<int>(std::round(intersection.y));
      // if (0 == slot_img.at<uchar>(v, u)) { //slot_img中只有0或254; 254才表示库位线
      //   continue;
      // }
      if (!check_slot_label(slot_img, v, u, 5)) {
        continue;
      }

      //那么intersection就是目标点
      corners.emplace_back(intersection);
    }
  }

  //合并距离较近的角点
  //constexpr int kNeighborDistance = 100; // 两车位点小于这个距离视为同一个车位点
  corners = merge_too_close_corner(corners, kNeighborDistance);

  /// TODO 2 detect slots in slot_img ///
  std::vector<cv::Point2f> slot_points; // Every 4 consecutive points compose a slot
  //从corners中提取车位
  int m = corners.size();
  //std::unordered_set<int> visited;
  for (int i = 0; i < m; ++i) {
    // if (visited.find(i) != visited.end()) {
    //   continue; //i已经被访问过了
    // }
    cv::Point2f pt1 = corners[i];
    for (int j = i + 1; j < m; ++j) {
      // if (visited.find(j) != visited.end()) {
      //   continue; //j已经被访问过了 
      // }

      //isSlotShortLine()函数判断了短边中是否有一定数量的点为库位线
      cv::Point2f pt2 = corners[j];
      if (!isSlotShortLine(pt1, pt2, slot_img)) {
        //不是车位短边,则跳过
        continue;
      }

      //计算短边的两条垂线 
      cv::Point2f dir1(pt2.x-pt1.x, pt2.y-pt1.y); //短边的方向
      dir1 = dir1 / cv::norm(dir1); 
      //垂线方向1
      cv::Point2f dir2(-dir1.y, dir1.x);
      if (isSlotLongLine(slot_img, pt1, dir2) && isSlotLongLine(slot_img, pt2, dir2)) {
        cv::Point2f pt3 = pt1 + dir2 * (kLongLinePixelDistance - 1);
        cv::Point2f pt4 = pt2 + dir2 * (kLongLinePixelDistance - 1);

        //注意插入顺序
        slot_points.emplace_back(pt1);
        slot_points.emplace_back(pt3);
        slot_points.emplace_back(pt4);
        slot_points.emplace_back(pt2);

        // visited.insert(i);
        // visited.insert(j);
      } else {
        //垂线方向2
        cv::Point2f dir3(dir1.y, -dir1.x);
        if (isSlotLongLine(slot_img, pt1, dir3) && isSlotLongLine(slot_img, pt2, dir3)) {
          cv::Point2f pt3 = pt1 + dir3 * (kLongLinePixelDistance - 1);
          cv::Point2f pt4 = pt2 + dir3 * (kLongLinePixelDistance - 1);

          //注意插入顺序
          slot_points.emplace_back(pt1);
          slot_points.emplace_back(pt3);
          slot_points.emplace_back(pt4);
          slot_points.emplace_back(pt2);

          // visited.insert(i);
          // visited.insert(j);
        }
      }

    }
  }

  return std::make_tuple(corners, slot_points);
}

调用的并查集头文件unionfind.h如下，

#include <iostream>
#include <algorithm>
#include <vector>
#include <unordered_map>

class UnionFind {
public:
    UnionFind(int n) {
        this->n = n;
        p.resize(n);
        for (int i = 0; i < this->n; ++i) {
            p[i] = i;
        }
    }

    int find(int x) { //找到结点x的根结点
        if (p[x] != x) p[x] = find(p[x]);
        return p[x];
    }

    void merge(int a, int b) { //合并结点a和结点b
        int pa = find(a);
        int pb = find(b);
        if (pa == pb) {
            return;
        } else {
            p[pa] = pb;
        }
    }

    std::vector<std::vector<int>> get_all_collections() {
        //返回所有的集合
        std::unordered_map<int, std::vector<int>> map_type_ids;
        for (int i = 0; i < n; ++i) {
            int pi = find(i);
            map_type_ids[pi].emplace_back(i);
        }

        std::vector<std::vector<int>> res;
        for (auto [type, ids] : map_type_ids) {
            res.emplace_back(ids);
        }
        return res;
    }


private:
    int n;
    std::vector<int> p;
    
};

结果如下：

在这里插入图片描述

2.3 调整参数

主要需要求解的是：部分角点的label值并非是库位线，可以适当扩大范围进行判断，比如5，见slot.cc文件中的bool check_slot_label(const cv::Mat &slot_img, int v, int u, int neighbor_dis)函数，内容如下，

bool check_slot_label(const cv::Mat &slot_img, int v, int u, int neighbor_dis) {
  int n = slot_img.rows; //slot_img的行数
  int m = slot_img.cols;

  //return false表示没有库位线标签
  for (int dx = -neighbor_dis; dx < neighbor_dis; ++dx) {
    for (int dy = -neighbor_dis; dy < neighbor_dis; ++dy) {
      int nv = v + dx;
      int nu = u + dy;
      if (nv < 0 || nv >= n || nu < 0 || nu >= m) {
        continue;
      }
      if (slot_img.at<uchar>(nv, nu) > 0) { //slot_img中只有0或254; 254才表示库位线
        return true;
      }
    }
  }
  return false;

}