精华内容
下载资源
问答
  • 单目视觉测距原理与模型的介绍,重点介绍单目视觉测距原理
  • 单目测距原理与实现

    万次阅读 多人点赞 2018-10-29 17:44:37
    Opencv3实现单目视觉测距一、写在前面的话二、单目测距原理三、实现代码四、运行结果 一、写在前面的话 刚刚接触Opencv没多久,为了检验自己最近学习的内容,准备做一下单目视觉测距。网上有很多关于单目测距的...

    一、写在前面的话

    刚刚接触Opencv没多久,为了检验自己最近学习的内容,准备做一下单目视觉测距。网上有很多关于单目测距的文章,我这里主要借鉴的是OpenCV学习笔记(二十一)——简单的单目视觉测距尝试单目摄像机测距(python+opencv)两篇文章,在这里特别作出说明。
    工作环境:Ubuntu16.04 + Opencv3.4.0 + Pycharm
    摄像头:本着一切从简的原则(好吧就是穷),选用了一款物美价廉的摄像头PS3专用PlayStation Eye, 只要27.5人民币。(这里贴一下淘宝链接,不是打广告,大家完全可以采用其他的摄像头或者工业相机。)

    二、单目测距原理

    单目相机测距常用或者说实用的方法就是相似三角形法,为了让大家更好地理解程序,这里简单说一下相似三角形法。

    举个栗子,假设现在我们有一张A4纸(8.27in x 11.69in), in代表英寸,1in = 25.4mm。纸张宽度W=11.69in,相机距离纸张的距离D = 32in,此时拍下的照片中A4纸的像素宽度为P=192px(我的相机实际测量得到的值)。
    此时我们可以算出焦距F=(192x30)/11.69。
    当我们将摄像头远离或者靠近A4纸时,就可以用相似三角形得到相机距离物体的距离。
    此时的距离: D’ = (W’ x F ) / P’
    (注意:这里测量的距离是相机到物体的垂直距离,产生夹角,测量的结果就不准确了。)

    三、实现代码

    #!/usr/bin/python3
    # -*- coding: utf-8 -*-
    # Date: 18-10-29
    
    import numpy as np      # 导入numpy库
    import cv2              # 导入Opencv库
    
    KNOWN_DISTANCE = 32   # 这个距离自己实际测量一下
    
    KNOWN_WIDTH = 11.69     # A4纸的宽度
    KNOWN_HEIGHT = 8.27
    
    IMAGE_PATHS = ["Picture1.jpg", "Picture2.jpg", "Picture3.jpg"]   # 将用到的图片放到了一个列表中
    
    
    # 定义目标函数
    def find_marker(image):
        gray_img = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)  # 将彩色图转化为灰度图
        gray_img = cv2.GaussianBlur(gray_img, (5, 5), 0)    # 高斯平滑去噪
        edged_img = cv2.Canny(gray_img, 35, 125)     # Canny算子阈值化
        cv2.imshow("降噪效果图", edged_img)          # 显示降噪后的图片
        # 获取纸张的轮廓数据
        img, countours, hierarchy = cv2.findContours(edged_img.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
        # print(len(countours))
        c = max(countours, key=cv2.contourArea)    # 获取最大面积对应的点集
        rect = cv2.minAreaRect(c)       # 最小外接矩形
        return rect
    
    
    # 定义距离函数
    def distance_to_camera(knownWidth, focalLength, perWidth):
        return (knownWidth * focalLength) / perWidth
    
    
    # 计算摄像头的焦距(内参)
    def calculate_focalDistance(img_path):
        first_image = cv2.imread(img_path)      # 这里根据准备的第一张图片,计算焦距
        # cv2.imshow('first image', first_image)
        marker = find_marker(first_image)       # 获取矩形的中心点坐标,长度,宽度和旋转角度
        focalLength = (marker[1][0] * KNOWN_DISTANCE) / KNOWN_WIDTH  # 获取摄像头的焦距
        # print(marker[1][0])
        print('焦距(focalLength) = ', focalLength)        # 打印焦距的值
        return focalLength
    
    
    # 计算摄像头到物体的距离
    def calculate_Distance(image_path, focalLength_value):
        image = cv2.imread(image_path)
        # cv2.imshow("原图", image)
        marker = find_marker(image)     # 获取矩形的中心点坐标,长度,宽度和旋转角度, marke[1][0]代表宽度
        distance_inches = distance_to_camera(KNOWN_WIDTH, focalLength_value, marker[1][0])
        box = cv2.boxPoints(marker)
        # print("Box = ", box)
        box = np.int0(box)
        print("Box = ", box)
        cv2.drawContours(image, [box], -1, (0, 255, 0), 2)      # 绘制物体轮廓
        cv2.putText(image, "%.2fcm" % (distance_inches * 2.54), (image.shape[1] - 300, image.shape[0] - 20),
                    cv2.FONT_HERSHEY_SIMPLEX, 2.0, (0, 255, 0), 3)
        cv2.imshow("单目测距", image)
    
    if __name__ == "__main__":
        img_path = "Picture1.jpg"
        focalLength = calculate_focalDistance(img_path)
    
        for image_path in IMAGE_PATHS:
            calculate_Distance(image_path, focalLength)
            cv2.waitKey(0)
        cv2.destroyAllWindows()

    四、运行结果

    第一张照片:

    第二张照片:

    第三张照片:

    后面准备做一下双目测距,各位朋友可以在评论区留言,大家相互交流。

    展开全文
  • 摄像头单目测距原理及实现

    千次阅读 2020-05-03 16:24:04
    摄像头单目测距原理及实现 一.测距原理 空间的深度或距离等数据的摄像头。 人的眼睛长在头部的前方,两只眼的视野范围重叠,两眼同时看某一物体时,产生的视觉称为双眼视觉。 双眼视觉的优点是可以弥补单眼视野中的...

    摄像头单目测距原理及实现

    一.测距原理

    空间的深度或距离等数据的摄像头。

    人的眼睛长在头部的前方,两只眼的视野范围重叠,两眼同时看某一物体时,产生的视觉称为双眼视觉。

    双眼视觉的优点是可以弥补单眼视野中的盲区缺损,扩大视野,并产生立体视觉。

    也就是说,假如只有一只眼睛,失去立体视觉后,人判断距离的能力将会下降。

    这也就是单目失明的人不能考取驾照的原因。

    在这里插入图片描述

    单纯的单目视觉测距,必须已知一个确定的长度。
    在这里插入图片描述

    f为摄像头的焦距,c为镜头光心。物体发出的光经过相机的光心,然后成像于图像传感器或者也可以说是像平面上,如果设物体所在平面与相机平面的距离为d,物体实际高度为H,在传感器上的高度为h,H一定要是已知的,我们才能求得距离d。

    假设我们有一个宽度为 W 的目标或者物体。然后我们将这个目标放在距离我们的相机为 D 的位置。我们用相机对物体进行拍照并且测量物体的像素宽度 P 。这样我们就得出了相机焦距的公式:

    F = (P x D) / W

    例如,假设现在我们有一张A4纸(8.27in x 11.69in), in代表英寸,1in = 25.4mm。纸张宽度W=11.69in,相机距离纸张的距离D = 32in,此时拍下的照片中A4纸的像素宽度为P=192px(我的相机实际测量得到的值)。

    此时我们可以算出焦距F=(192x30)/11.69。

    当我们将摄像头远离或者靠近A4纸时,就可以用相似三角形得到相机距离物体的距离。

    此时的距离: D’
    = (W’ x F ) / P’。

    (注意:这里测量的距离是相机到物体的垂直距离,产生夹角,测量的结果就不准确了。)

    二.测距步骤:

    1. 使用摄像机采集道路前方的图像;

    2. 在道路区域对物体进行检测,通过矩形框将物体形状框出来。

    3. 结合矩形框信息,找到该矩形框底边的两个像平面坐标,分别记为(u1,v1)和(u2,v2);

    4. 使用几何关系推导法,由像平面坐标点(u1, v1)、(u2, v2)推导出道路平面坐标(x1,y1)、(x2, y2);(投影到地面上,z轴为0)

    5. 通过欧氏距离公式计算出d。

    三.难点整理:

    1.图像畸变矫正模型的理解;

    (标定参数,内参矩阵,畸变矩阵,外参矩阵(平移、旋转向量矩阵))

    2.像素坐标与世界坐标公式的推导及验证;

    3.测距方法,对于检测物体在摄像头前方、左侧、右侧的判别思路;

    4.弄清反畸变;对于畸变矫正后的图像中的检测框中的点进行反畸变处理。

    四.相机镜头畸变矫正–>得到相机的内外参数、畸变参数矩阵

    1. 外参数矩阵。世界坐标经过旋转和平移,然后落到另一个现实世界点(摄像机坐标)上。

    2. 内参数矩阵。告诉你上述那个点在1的基础上,是如何继续经过摄像机的镜头、并通过针孔成像和电子转化而成为像素点的。

    3. 畸变矩阵。告诉你为什么上面那个像素点并没有落在理论计算该落在的位置上,还产生了一定的偏移和变形.

    五.实现代码

    #!/usr/bin/python3

    -- coding: utf-8 --

    Date: 18-10-29

    import numpy as np

    导入numpy库

    import cv2

    导入Opencv库

    KNOWN_DISTANCE = 32

    这个距离自己实际测量一下

    KNOWN_WIDTH = 11.69

    A4纸的宽度

    KNOWN_HEIGHT = 8.27

    IMAGE_PATHS = [“Picture1.jpg”, “Picture2.jpg”,
    “Picture3.jpg”] # 将用到的图片放到了一个列表中

    定义目标函数

    def find_marker(image):

    gray_img = cv2.cvtColor(image,
    

    cv2.COLOR_BGR2GRAY) # 将彩色图转化为灰度图

    gray_img = cv2.GaussianBlur(gray_img,
    

    (5, 5), 0) # 高斯平滑去噪

    edged_img = cv2.Canny(gray_img,
    

    35, 125) # Canny算子阈值化

    cv2.imshow("降噪效果图", edged_img)          # 显示降噪后的图片
    
    # 获取纸张的轮廓数据
    
    img, countours,
    

    hierarchy = cv2.findContours(edged_img.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)

    #
    

    print(len(countours))

    c = max(countours,
    

    key=cv2.contourArea) # 获取最大面积对应的点集

    rect = cv2.minAreaRect(c)       # 最小外接矩形
    
    return rect
    

    定义距离函数

    def distance_to_camera(knownWidth, focalLength, perWidth):

    return (knownWidth
    
    • focalLength) / perWidth

    计算摄像头的焦距(内参)

    def calculate_focalDistance(img_path):

    first_image =
    

    cv2.imread(img_path) # 这里根据准备的第一张图片,计算焦距

    #
    

    cv2.imshow(‘first image’, first_image)

    marker =
    

    find_marker(first_image) # 获取矩形的中心点坐标,长度,宽度和旋转角度

    focalLength = (marker[1][0]
    
    • KNOWN_DISTANCE) / KNOWN_WIDTH # 获取摄像头的焦距

    print(marker[1][0])

    print('焦距(focalLength) = ', focalLength)        # 打印焦距的值
    
    return
    

    focalLength

    计算摄像头到物体的距离

    def calculate_Distance(image_path, focalLength_value):

    image = cv2.imread(image_path)
    
    #
    

    cv2.imshow(“原图”, image)

    marker =
    

    find_marker(image) # 获取矩形的中心点坐标,长度,宽度和旋转角度, marke[1][0]代表宽度

    distance_inches
    

    = distance_to_camera(KNOWN_WIDTH, focalLength_value, marker[1][0])

    box = cv2.boxPoints(marker)
    
    #
    

    print("Box = ", box)

    box = np.int0(box)
    
    print("Box
    

    = ", box)

    cv2.drawContours(image,
    

    [box], -1, (0, 255, 0), 2) # 绘制物体轮廓

    cv2.putText(image,
    

    “%.2fcm” % (distance_inches * 2.54), (image.shape[1] - 300, image.shape[0]

    • 20),

                cv2.FONT_HERSHEY_SIMPLEX,
      

    2.0, (0, 255, 0), 3)

    cv2.imshow("单目测距", image)
    

    if name == “main”:

    img_path = "Picture1.jpg"
    
    focalLength =
    

    calculate_focalDistance(img_path)

    for image_path in
    

    IMAGE_PATHS:

    calculate_Distance(image_path, focalLength)

        cv2.waitKey(0)
    
    cv2.destroyAllWindows()
    

    在这里插入图片描述

    展开全文
  • 转载:https://www.cnblogs.com/wujianming-110117/p/12822331.html

    转载:https://www.cnblogs.com/wujianming-110117/p/12822331.html

    展开全文
  • 单目测距 视觉测距

    千次阅读 2021-04-22 17:02:37
    文章目录Apollo3.0单目测距原理代码注释其他-基于相似三角形的单目测距算法原理代码参考资料 Apollo3.0单目测距 原理 主要的思想就是借鉴3D Bounding Box Estimation Using Deep Learning and Geometry论文进行实现...

    单目测距

    在kitti数据集中的测试结果

    在这里插入图片描述在这里插入图片描述
    在这里插入图片描述

    C++工程

    C++工程代码下载地址。

    在这里插入图片描述

    原理

    主要的思想就是借鉴3D Bounding Box Estimation Using Deep Learning and Geometry论文进行实现。

    使用yolo进行2D图像目标检测+目标大小姿态估计网络+目标3D中心点解算模块实现单目测距。其中2D目标检测和大小姿态估计Apollo使用caffe框架进行构建深度学习模型,这部分的代码Apollo未开源,不在本文的讨论范围,本文主要说明的是Apollo的单目测距原理。

    其算法流程图如下:
    在这里插入图片描述
    Apollo经过deep learning模块之后将会得到目标2D box, 目标类型,目标的宽度w,目标高度h,目标长度l。算法的解算依据如下:
    在这里插入图片描述

    代码注释

    流程图1中的代码:

    /* 进行2D转3D */
    bool GeometryCameraConverter::Convert(
        std::vector<std::shared_ptr<VisualObject>> *objects) {
      if (!objects) return false;
    
      for (auto &obj : *objects) {
        Eigen::Vector2f trunc_center_pixel = Eigen::Vector2f::Zero();
        // 检测截断,在进行目标检测时有进行目标的截断
        CheckTruncation(obj, &trunc_center_pixel);
        // 根据目标的类型判断目标的大小是否越界,若是越界则将目标的大小重新设定
        CheckSizeSanity(obj);
        
        // 目标框
        float deg_alpha = obj->alpha * 180.0f / M_PI; //对象的观测角度
        Eigen::Vector2f upper_left(obj->upper_left.x(), obj->upper_left.y()); // 左上角:x1, y1
        Eigen::Vector2f lower_right(obj->lower_right.x(), obj->lower_right.y());// 右下角:x2, y2
    
        // 计算目标的距离distance和像素中心mass_center_pixel
        float distance = 0.0;
        Eigen::Vector2f mass_center_pixel = Eigen::Vector2f::Zero();
        if (obj->trunc_height < 0.25f) {
          // No truncation on 2D height 在二维高度上没有截断
          ConvertSingle(obj->height, obj->width, obj->length, deg_alpha, upper_left,
                        lower_right, false, &distance, &mass_center_pixel);
        } else if (obj->trunc_width < 0.25f && obj->trunc_height > 0.25f) {
          // 2D height truncation and no width truncation 二维高度截断,没有宽度截断
          ConvertSingle(obj->height, obj->width, obj->length, deg_alpha, upper_left,
                        lower_right, true, &distance, &mass_center_pixel);
        } else {
          // truncation on both sides 两边截断
          // Give fix values for detected box with both side and bottom truncation 给出被检测框的边和底截断的固定值
          distance = 10.0f;
          // Estimation of center pixel due to unknown truncation ratio 由于截断率未知,中心像素的估计
          mass_center_pixel = trunc_center_pixel;
        }
    
        // 反投影变换,计算目标的质心在相机坐标系下鸟瞰的坐标
        obj->distance = distance;
        Eigen::Vector3f camera_ray = camera_model_.unproject(mass_center_pixel);
        // 相机坐标系: 计算目标质心与光心的夹角
        DecideAngle(camera_ray, obj);
    
        // Center (3D Mass Center of 3D BBox),3D质心坐标去掉前面计算的尺度变换问题
        float scale = obj->distance / sqrt(camera_ray.x() * camera_ray.x() +
                                           camera_ray.y() * camera_ray.y() +
                                           camera_ray.z() * camera_ray.z());
        obj->center = camera_ray * scale;
    
        // Set 8 corner pixels,像素坐标系:计算8个顶点,
        SetBoxProjection(obj);
      }
    
      return true;
    }
    
    // 根据目标类型对目标的物理长、宽、高进行修正
    void GeometryCameraConverter::CheckSizeSanity(
        std::shared_ptr<VisualObject> obj) const {
      if (obj->type == ObjectType::VEHICLE) {
        obj->length = std::max(obj->length, 3.6f);
        obj->width = std::max(obj->width, 1.6f);
        obj->height = std::max(obj->height, 1.5f);
      } else if (obj->type == ObjectType::PEDESTRIAN) {
        obj->length = std::max(obj->length, 0.5f);
        obj->width = std::max(obj->width, 0.5f);
        obj->height = std::max(obj->height, 1.7f);
      } else if (obj->type == ObjectType::BICYCLE) {
        obj->length = std::max(obj->length, 1.8f);
        obj->width = std::max(obj->width, 1.2f);
        obj->height = std::max(obj->height, 1.5f);
      } else {
        obj->length = std::max(obj->length, 0.5f);
        obj->width = std::max(obj->width, 0.5f);
        obj->height = std::max(obj->height, 1.5f);
      }
    }
    
    // 检测截断,在进行目标检测时有进行目标的截断
    void GeometryCameraConverter::CheckTruncation(
        std::shared_ptr<VisualObject> obj,
        Eigen::Matrix<float, 2, 1> *trunc_center_pixel) const {
      auto width = camera_model_.get_width();
      auto height = camera_model_.get_height();
    
      // Ad-hoc 2D box truncation binary determination 二次确定特殊的2D框截断
      if (obj->upper_left.x() < 30.0f || width - 30.0f < obj->lower_right.x()) {
        obj->trunc_width = 0.5f;
    
        if (obj->upper_left.x() < 30.0f) {
          trunc_center_pixel->x() = obj->upper_left.x();
        } else {
          trunc_center_pixel->x() = obj->lower_right.x();
        }
      }
    
      if (obj->upper_left.y() < 30.0f || height - 30.0f < obj->lower_right.y()) {
        obj->trunc_height = 0.5f;
        trunc_center_pixel->x() =
            (obj->upper_left.x() + obj->lower_right.x()) / 2.0f;
      }
    
      trunc_center_pixel->y() = (obj->upper_left.y() + obj->lower_right.y()) / 2.0f;
    }
    
    void GeometryCameraConverter::DecideAngle(
        const Eigen::Vector3f &camera_ray,
        std::shared_ptr<VisualObject> obj) const {
      float beta = std::atan2(camera_ray.x(), camera_ray.z());
    
      // Orientation is not reliable in these cases (DL model specific issue)  定位在这些情况下是不可靠的(DL模型特定的问题)
      if (obj->distance > 50.0f || obj->trunc_width > 0.25f) {
        obj->theta = -1.0f * M_PI_2;
        obj->alpha = obj->theta - beta;
        if (obj->alpha > M_PI) {
          obj->alpha -= 2 * M_PI;
        } else if (obj->alpha < -M_PI) {
          obj->alpha += 2 * M_PI;
        }
      } else {  // Normal cases 正常的情况下 
        float theta = obj->alpha + beta;
        if (theta > M_PI) {
          theta -= 2 * M_PI;
        } else if (theta < -M_PI) {
          theta += 2 * M_PI;
        }
        obj->theta = theta;
      }
    }
    
    void GeometryCameraConverter::SetBoxProjection(
        std::shared_ptr<VisualObject> obj) const {
      obj->pts8.resize(16);
      if (obj->trunc_width < 0.25f && obj->trunc_height < 0.25f) {  // No truncation
        for (int i = 0; i < 8; i++) {
          obj->pts8[i * 2] = pixel_corners_[i].x();
          obj->pts8[i * 2 + 1] = pixel_corners_[i].y();
        }
      }
    }
    
    

    流程图第2部分代码:

    bool GeometryCameraConverter::LoadCameraIntrinsics(
        const std::string &file_path) {
      YAML::Node node = YAML::LoadFile(file_path);
    
      // 获取相机内参
      Eigen::Matrix3f intrinsic_k;
      for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
          int index = i * 3 + j;
          intrinsic_k(i, j) = node["K"][index].as<float>();
        }
      }
    
      // 获取畸变系数
      Eigen::Matrix<float, 5, 1> intrinsic_d;
      for (int i = 0; i < 5; i++) {
        intrinsic_d(i, 0) = node["D"][i].as<float>();
      }
      // 获取图像的宽高
      float height = node["height"].as<float>();
      float width = node["width"].as<float>();
      camera_model_.set(intrinsic_k, width, height);
      camera_model_.set_distort_params(intrinsic_d);
    
      return true;
    }
    /*
    h: height, 三维定向包围框的高度的物理大小
    w: width, 三维定向包围框的宽度的物理大小
    l: length, 三维定向包围框的长度的物理大小
    alpha_deg: 对象的观测角度,像素
    upper_left: 左上角:x1, y1
    lower_right:右下角:x2, y2
    use_width: 是否使用宽度
    istance: 距离
    mass_center_pixel: 质量中心,像素坐标
    */
      
      
    bool GeometryCameraConverter::ConvertSingle(
        const float h, const float w, const float l, const float alpha_deg,
        const Eigen::Vector2f &upper_left, const Eigen::Vector2f &lower_right,
        bool use_width, float *distance, Eigen::Vector2f *mass_center_pixel) {
      // Target Goals: Projection target
      //获取像素宽高
      int pixel_width = static_cast<int>(lower_right.x() - upper_left.x());
      int pixel_height = static_cast<int>(lower_right.y() - upper_left.y());
      int pixel_length = pixel_height;
      if (use_width) pixel_length = pixel_width;
    
      // Target Goals: Box center pixel
      // 获取像素目标框的中心
      Eigen::Matrix<float, 2, 1> box_center_pixel;
      box_center_pixel.x() = (lower_right.x() + upper_left.x()) / 2.0f;
      box_center_pixel.y() = (lower_right.y() + upper_left.y()) / 2.0f;
    
      // Generate alpha rotated 3D template here. Corners in Camera space:
      // Bottom: FL, FR, RR, RL => Top: FL, FR, RR, RL
      // 构建3D框,并根据角度进行旋转
      float deg_alpha = alpha_deg;
      float h_half = h / 2.0f;
      float w_half = w / 2.0f;
      float l_half = l / 2.0f;
    
      std::vector<Eigen::Vector3f> corners;
      corners.resize(8);
      corners[0] = Eigen::Vector3f(l_half, h_half, w_half);
      corners[1] = Eigen::Vector3f(l_half, h_half, -w_half);
      corners[2] = Eigen::Vector3f(-l_half, h_half, -w_half);
      corners[3] = Eigen::Vector3f(-l_half, h_half, w_half);
      corners[4] = Eigen::Vector3f(l_half, -h_half, w_half);
      corners[5] = Eigen::Vector3f(l_half, -h_half, -w_half);
      corners[6] = Eigen::Vector3f(-l_half, -h_half, -w_half);
      corners[7] = Eigen::Vector3f(-l_half, -h_half, w_half);
      Rotate(deg_alpha, &corners);
      corners_ = corners;
      pixel_corners_.clear();
      pixel_corners_.resize(8);
    
      // Try to get an initial Mass center pixel and vector 尝试得到一个初始质心像素和向量
      // 防止目标box超出图片的大小,给后续计算带来误差
      Eigen::Matrix<float, 3, 1> middle_v(0.0f, 0.0f, 20.0f);
      // camera_model_.project:在图像上投影一个3D点
      Eigen::Matrix<float, 2, 1> center_pixel = camera_model_.project(middle_v);
    
      // 将物理的3D框投影到图像中,得到最大外接框
      float max_pixel_x = std::numeric_limits<float>::min();
      float min_pixel_x = std::numeric_limits<float>::max();
      float max_pixel_y = std::numeric_limits<float>::min();
      float min_pixel_y = std::numeric_limits<float>::max();
      for (size_t i = 0; i < corners.size(); ++i) {
        Eigen::Vector2f point_2d = camera_model_.project(corners[i] + middle_v);
        min_pixel_x = std::min(min_pixel_x, point_2d.x());
        max_pixel_x = std::max(max_pixel_x, point_2d.x());
        min_pixel_y = std::min(min_pixel_y, point_2d.y());
        max_pixel_y = std::max(max_pixel_y, point_2d.y());
      }
      
      /* 初步计算目标在图像中的质心 */
      // 初始质心像素 与 投影到图像的3D-BOX的像素距离
      float relative_x =
          (center_pixel.x() - min_pixel_x) / (max_pixel_x - min_pixel_x);
      float relative_y =
          (center_pixel.y() - min_pixel_y) / (max_pixel_y - min_pixel_y);
    
      // 计算box质量中心,像素坐标
      mass_center_pixel->x() =
          (lower_right.x() - upper_left.x()) * relative_x + upper_left.x();
      mass_center_pixel->y() =
          (lower_right.y() - upper_left.y()) * relative_y + upper_left.y();
          
          
      //  像素坐标系转到3D坐标系的投影,其中z设为1
      Eigen::Matrix<float, 3, 1> mass_center_v =
          camera_model_.unproject(*mass_center_pixel);
      // 计算单位尺寸,就算每个维度与距离的比值,也就是目标质心的x,y,z坐标与距离是成正比的
      mass_center_v = MakeUnit(mass_center_v);
    
      // Distance search 使用二分法进行距离搜索,150.0f为相机的可视长度,0.1为起始距离。主要精度为0.1
      *distance =
          SearchDistance(pixel_length, use_width, mass_center_v, 0.1f, 150.0f);
      for (size_t i = 0; i < 1; ++i) {
        // Mass center search 质量中心搜索,与SearchDistance算法思想一样,更新mass_center_pixel
        SearchCenterDirection(box_center_pixel, *distance, &mass_center_v,
                              mass_center_pixel);
        // Distance search,提高距离的精度
        *distance = SearchDistance(pixel_length, use_width, mass_center_v,
                                   0.9f * (*distance), 1.1f * (*distance));
      }
    
      return true;
    }
    
    void GeometryCameraConverter::Rotate(
        const float alpha_deg, std::vector<Eigen::Vector3f> *corners) const {
      Eigen::AngleAxisf yaw(alpha_deg / 180.0f * M_PI, Eigen::Vector3f::UnitY());
      Eigen::AngleAxisf pitch(0.0, Eigen::Vector3f::UnitX());
      Eigen::AngleAxisf roll(0.0, Eigen::Vector3f::UnitZ());
      Eigen::Matrix3f rotation = yaw.toRotationMatrix() * pitch.toRotationMatrix() *
                                 roll.toRotationMatrix();
    
      Eigen::Matrix4f transform;
      transform.setIdentity();
      transform.block(0, 0, 3, 3) = rotation;
    
      for (auto &corner : *corners) {
        Eigen::Vector4f temp(corner.x(), corner.y(), corner.z(), 1.0f);
        temp = transform * temp;
        corner = Eigen::Vector3f(temp.x(), temp.y(), temp.z());
      }
    }
    
    /**
        主要原理:使用二分法进行物理距离的搜索,主要评判指标为:物理坐标系投影到图像中的长度 等于 像素box的长度  
    **/
    float GeometryCameraConverter::SearchDistance(
        const int pixel_length, const bool &use_width,
        const Eigen::Matrix<float, 3, 1> &mass_center_v, float close_d,
        float far_d) {
      float curr_d = 0.0f;
      int depth = 0;
      while (close_d <= far_d && depth < kMaxDistanceSearchDepth_) {
        curr_d = (far_d + close_d) / 2.0f;
        Eigen::Vector3f curr_p = mass_center_v * curr_d;
    
        float min_p = std::numeric_limits<float>::max();
        float max_p = 0.0f;
        for (size_t i = 0; i < corners_.size(); ++i) {
          Eigen::Vector2f point_2d = camera_model_.project(corners_[i] + curr_p);
    
          float curr_pixel = 0.0f;
          if (use_width) {
            curr_pixel = point_2d.x();
          } else {
            curr_pixel = point_2d.y();
          }
    
          min_p = std::min(min_p, curr_pixel);
          max_p = std::max(max_p, curr_pixel);
        }
    
        int curr_pixel_length = static_cast<int>(max_p - min_p);
        if (curr_pixel_length == pixel_length) {
          break;
        } else if (pixel_length < curr_pixel_length) {
          close_d = curr_d + 0.1f;
        } else {  // pixel_length > curr_pixel_length
          far_d = curr_d - 0.1f;
        }
    
        // Early break for 0.1m accuracy
        float next_d = (far_d + close_d) / 2.0f;
        if (std::abs(next_d - curr_d) < 0.1f) {
          break;
        }
    
        ++depth;
      }
    
      // Only copy the last projection out
      Eigen::Vector3f curr_p = mass_center_v * curr_d;
      for (size_t i = 0; i < corners_.size(); ++i) {
        Eigen::Vector2f point_2d = camera_model_.project(corners_[i] + curr_p);
        pixel_corners_[i] = point_2d;
      }
    
      return curr_d;
    }
    
    void GeometryCameraConverter::SearchCenterDirection(
        const Eigen::Matrix<float, 2, 1> &box_center_pixel, const float curr_d,
        Eigen::Matrix<float, 3, 1> *mass_center_v,
        Eigen::Matrix<float, 2, 1> *mass_center_pixel) const {
      int depth = 0;
      while (depth < kMaxCenterDirectionSearchDepth_) {
        Eigen::Matrix<float, 3, 1> new_center_v = *mass_center_v * curr_d;
    
        float max_pixel_x = std::numeric_limits<float>::min();
        float min_pixel_x = std::numeric_limits<float>::max();
        float max_pixel_y = std::numeric_limits<float>::min();
        float min_pixel_y = std::numeric_limits<float>::max();
        for (size_t i = 0; i < corners_.size(); ++i) {
          Eigen::Vector2f point_2d =
              camera_model_.project(corners_[i] + new_center_v);
          min_pixel_x = std::min(min_pixel_x, point_2d.x());
          max_pixel_x = std::max(max_pixel_x, point_2d.x());
          min_pixel_y = std::min(min_pixel_y, point_2d.y());
          max_pixel_y = std::max(max_pixel_y, point_2d.y());
        }
    
        Eigen::Matrix<float, 2, 1> current_box_center_pixel;
        current_box_center_pixel.x() = (max_pixel_x + min_pixel_x) / 2.0;
        current_box_center_pixel.y() = (max_pixel_y + min_pixel_y) / 2.0;
    
        // Update mass center
        *mass_center_pixel += box_center_pixel - current_box_center_pixel;
        *mass_center_v = camera_model_.unproject(*mass_center_pixel);
        *mass_center_v = MakeUnit(*mass_center_v);
    
        if (std::abs(mass_center_pixel->x() - box_center_pixel.x()) < 1.0 &&
            std::abs(mass_center_pixel->y() - box_center_pixel.y()) < 1.0) {
          break;
        }
    
        ++depth;
      }
    
      return;
    }
    /*单位距离时,x,y,z的比例*/
    Eigen::Matrix<float, 3, 1> GeometryCameraConverter::MakeUnit(
        const Eigen::Matrix<float, 3, 1> &v) const {
      Eigen::Matrix<float, 3, 1> unit_v = v;
      float to_unit_scale =
          std::sqrt(unit_v.x() * unit_v.x() + unit_v.y() * unit_v.y() +
                    unit_v.z() * unit_v.z());
      unit_v /= to_unit_scale;
      return unit_v;
    }
    
    

    流程图的第3和第四部分:

    /******************************************************************************
     * Copyright 2018 The Apollo Authors. All Rights Reserved.
     *
     * Licensed under the Apache License, Version 2.0 (the "License");
     * you may not use this file except in compliance with the License.
     * You may obtain a copy of the License at
     *
     * http://www.apache.org/licenses/LICENSE-2.0
     *
     * Unless required by applicable law or agreed to in writing, software
     * distributed under the License is distributed on an "AS IS" BASIS,
     * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
     * See the License for the specific language governing permissions and
     * limitations under the License.
     *****************************************************************************/
    
    #ifndef MODULES_PERCEPTION_OBSTACLE_CAMERA_COMMON_CAMERA_H_
    #define MODULES_PERCEPTION_OBSTACLE_CAMERA_COMMON_CAMERA_H_
    
    #include <Eigen/Core>
    #include <Eigen/Dense>
    
    #include <algorithm>
    
    namespace apollo {
    namespace perception {
    
    template <typename T>
    class CameraModel;
    template <typename T>
    class CameraDistort;
    
    /**@brief Print the matrix.*/
    template <typename T>
    std::ostream& operator<<(std::ostream& cout, const CameraModel<T>& camera);
    
    template <typename T>
    std::ostream& operator<<(std::ostream& cout, const CameraDistort<T>& camera);
    
    /**@brief camera intrinsic of pin-hole camera model*/
    template <typename T>
    class CameraModel {
     public:
      CameraModel() {
        focal_length_x_ = 1;
        focal_length_y_ = 1;
        center_x_ = 0;
        center_y_ = 0;
        intrinsic_(0, 0) = 1;
        intrinsic_(0, 1) = 0;
        intrinsic_(0, 2) = 0;
        intrinsic_(1, 0) = 0;
        intrinsic_(1, 1) = 1;
        intrinsic_(1, 2) = 0;
        intrinsic_(2, 0) = 0;
        intrinsic_(2, 1) = 0;
        intrinsic_(2, 2) = 1;
        width_ = 1;
        height_ = 1;
      }
    
      void set(const Eigen::Matrix<T, 3, 3>& params, T w, T h) {
        intrinsic_ = params;
        focal_length_x_ = intrinsic_(0, 0);
        focal_length_y_ = intrinsic_(1, 1);
        center_x_ = intrinsic_(0, 2);
        center_y_ = intrinsic_(1, 2);
        width_ = w;
        height_ = h;
      }
    
      void set(T focal_length_x, T focal_length_y, T center_x, T center_y, T w,
               T h) {
        focal_length_x_ = focal_length_x;
        focal_length_y_ = focal_length_y;
        center_x_ = center_x;
        center_y_ = center_y;
        width_ = w;
        height_ = h;
        intrinsic_(0, 0) = focal_length_x_;
        intrinsic_(1, 1) = focal_length_y_;
        intrinsic_(0, 2) = center_x_;
        intrinsic_(1, 2) = center_y_;
      }
    
      /**@brief Project a 3D point on an image.  在图像上投影一个3D点,去掉了世界坐标到相机坐标系的变换。默认输入的数据就是相机坐标系的数据*/
      virtual Eigen::Matrix<T, 2, 1> project(
          const Eigen::Matrix<T, 3, 1>& pt3d) const {
        Eigen::Matrix<T, 2, 1> pt2d;
    
        pt2d[0] = pt3d[0] / pt3d[2];
        pt2d[1] = pt3d[1] / pt3d[2];
    
        return pixel_denormalize(pt2d);
      }
    
      /**@brief Unproject a pixel to 3D point on a given XY plane, where z = 1   在给定的XY平面上像素到3D点的投影,其中z为1*/
      virtual Eigen::Matrix<T, 3, 1> unproject(
          const Eigen::Matrix<T, 2, 1>& pt2d) const {
        Eigen::Matrix<T, 3, 1> pt3d;
    
        Eigen::Matrix<T, 2, 1> pt2d_tmp = pixel_normalize(pt2d);
    
        pt3d[0] = pt2d_tmp[0];
        pt3d[1] = pt2d_tmp[1];
        pt3d[2] = 1;
    
        return pt3d;
      }
    
      /**@brief Project a 3D point on an image. */
      virtual Eigen::Matrix<T, 2, 1> project(
          const Eigen::Matrix<T, 4, 4>& transform,
          const Eigen::Matrix<T, 3, 1>& pt3d) const {
        Eigen::Matrix<T, 3, 1> local_pt3d;
        local_pt3d[0] = transform(0, 0) * pt3d[0] + transform(0, 1) * pt3d[1] +
                        transform(0, 2) * pt3d[2] + transform(0, 3);
        local_pt3d[1] = transform(1, 0) * pt3d[0] + transform(1, 1) * pt3d[1] +
                        transform(1, 2) * pt3d[2] + transform(1, 3);
        local_pt3d[2] = transform(2, 0) * pt3d[0] + transform(2, 1) * pt3d[1] +
                        transform(2, 2) * pt3d[2] + transform(2, 3);
    
        return project(local_pt3d);
      }
    
      /**@brief Check if a 3D point is in the view*/
      bool is_in_view(const Eigen::Matrix<T, 4, 4>& transform,
                      const Eigen::Matrix<T, 3, 1>& pt3d) const {
        Eigen::Matrix<T, 3, 1> local_pt3d;
        local_pt3d[0] = transform(0, 0) * pt3d[0] + transform(0, 1) * pt3d[1] +
                        transform(0, 2) * pt3d[2] + transform(0, 3);
        local_pt3d[1] = transform(1, 0) * pt3d[0] + transform(1, 1) * pt3d[1] +
                        transform(1, 2) * pt3d[2] + transform(1, 3);
        local_pt3d[2] = transform(2, 0) * pt3d[0] + transform(2, 1) * pt3d[1] +
                        transform(2, 2) * pt3d[2] + transform(2, 3);
        if (local_pt3d[2] <= 0) {
          return false;
        }
    
        Eigen::Matrix<T, 2, 1> pt2d = project(local_pt3d);
        if (pt2d[0] > 0 && pt2d[0] < width_ && pt2d[1] > 0 && pt2d[1] < height_) {
          return true;
        }
        return false;
      }
    
      /**@brief Get the x focal length. */
      inline T get_focal_length_x() const { return focal_length_x_; }
      /**@brief Get the y focal length. */
      inline T get_focal_length_y() const { return focal_length_y_; }
      /**@brief Get the optical center x. */
      inline T get_center_x() const { return center_x_; }
      /**@brief Get the optical center y. */
      inline T get_center_y() const { return center_y_; }
      /**@brief Get the intrinsic matrix K. */
      inline const Eigen::Matrix<T, 3, 3>& get_intrinsic() const {
        return intrinsic_;
      }
      /**@brief Get the intrinsic matrix K. */
      inline Eigen::Matrix<T, 3, 3>& get_intrinsic() { return intrinsic_; }
      /**@brief Get the image width */
      inline T get_width() const { return width_; }
      /**@brief Get the image height */
      inline T get_height() const { return height_; }
    
      friend std::ostream& operator<<<>(std::ostream& out,
                                        const CameraModel<T>& camera);
    
     protected:
      /**@brief Normalize a 2D pixel. Convert a 2D pixel as if the image is taken
       * with a camera,
       * whose K = identity matrix. */
      virtual Eigen::Matrix<T, 2, 1> pixel_normalize(
          const Eigen::Matrix<T, 2, 1>& pt2d) const {
        Eigen::Matrix<T, 2, 1> p;
        p[0] = (pt2d[0] - center_x_) / focal_length_x_;
        p[1] = (pt2d[1] - center_y_) / focal_length_y_;
    
        return p;
      }
    
      /**@brief Denormalize a 2D pixel. Convert a 2D pixel as if the image is taken 非规格化2D像素。转换一个2D像素,就像图像是用一个相机拍摄的,其K = intrinsic_。(不考虑畸变系数)
       * with a camera,
       * whose K = intrinsic_. */
      virtual Eigen::Matrix<T, 2, 1> pixel_denormalize(
          const Eigen::Matrix<T, 2, 1>& pt2d) const {
        Eigen::Matrix<T, 2, 1> p;
        p[0] = pt2d[0] * focal_length_x_ + center_x_;
        p[1] = pt2d[1] * focal_length_y_ + center_y_;
    
        return p;
      }
    
     protected:
      /**@brief The camera intrinsic matrix. */
      Eigen::Matrix<T, 3, 3> intrinsic_; 
      /**@brief The focal length x. */
      T focal_length_x_;
      /**@brief The focal length y. */
      T focal_length_y_;
      /**@brief The optical center x. */
      T center_x_;
      /**@brief The optical center y. */
      T center_y_;
      /**@brief Image width */
      T width_;
      /**@brief Image height */
      T height_;
    };
    
    /**@brief camera intrinsic of pin-hole camera model with distortion*/
    template <typename T>
    class CameraDistort : public CameraModel<T> {
     public:
      /**@brief The default constructor. */
      CameraDistort() {
        distort_params_[0] = 0;
        distort_params_[1] = 0;
        distort_params_[2] = 0;
        distort_params_[3] = 0;
        distort_params_[4] = 0;
      }
    
      /**@brief Project a 3D point on an image. */
      virtual Eigen::Matrix<T, 2, 1> project(
          const Eigen::Matrix<T, 3, 1>& pt3d) const {
        Eigen::Matrix<T, 2, 1> pt2d;
        pt2d[0] = pt3d[0] / pt3d[2];
        pt2d[1] = pt3d[1] / pt3d[2];
        return pixel_denormalize(pt2d);
      }
    
      /**@brief Unproject a pixel to 3D point on a given XY plane, where z = 1 */
      virtual Eigen::Matrix<T, 3, 1> unproject(
          const Eigen::Matrix<T, 2, 1>& pt2d) const {
        Eigen::Matrix<T, 3, 1> pt3d;
    
        Eigen::Matrix<T, 2, 1> pt2d_tmp = pixel_normalize(pt2d);
    
        pt3d[0] = pt2d_tmp[0];
        pt3d[1] = pt2d_tmp[1];
        pt3d[2] = 1;
    
        return pt3d;
      }
    
      /**@brief Project a 3D point on an image. */
      virtual Eigen::Matrix<T, 2, 1> project(
          const Eigen::Matrix<T, 4, 4>& transform,
          const Eigen::Matrix<T, 3, 1>& pt3d) const {
        Eigen::Matrix<T, 3, 1> local_pt3d;
        local_pt3d[0] = transform(0, 0) * pt3d[0] + transform(0, 1) * pt3d[1] +
                        transform(0, 2) * pt3d[2] + transform(0, 3);
        local_pt3d[1] = transform(1, 0) * pt3d[0] + transform(1, 1) * pt3d[1] +
                        transform(1, 2) * pt3d[2] + transform(1, 3);
        local_pt3d[2] = transform(2, 0) * pt3d[0] + transform(2, 1) * pt3d[1] +
                        transform(2, 2) * pt3d[2] + transform(2, 3);
    
        return project(local_pt3d);
      }
    
      /**@brief Set the distortion parameters. */
      void set_distort_params(T d0, T d1, T d2, T d3, T d4) {
        distort_params_[0] = d0;
        distort_params_[0] = d1;
        distort_params_[0] = d2;
        distort_params_[0] = d3;
        distort_params_[0] = d4;
      }
    
      /**@brief Set the distortion parameters. */
      inline void set_distort_params(const Eigen::Matrix<T, 5, 1>& params) {
        distort_params_ = params;
      }
    
      /**@brief Get the distortion parameters. */
      inline const Eigen::Matrix<T, 5, 1>& get_distort_params() const {
        return distort_params_;
      }
    
      /**@brief Get the distortion parameters. */
      inline Eigen::Matrix<T, 5, 1>& get_distort_params() {
        return distort_params_;
      }
    
      friend std::ostream& operator<<<>(std::ostream& out,
                                        const CameraDistort<T>& camera);
    
     protected:
      /**@brief Normalize a 2D pixel. Convert a 2D pixel as if the image is taken
       * with a camera,
       * whose K = identity matrix. */
      virtual Eigen::Matrix<T, 2, 1> pixel_normalize(
          const Eigen::Matrix<T, 2, 1>& pt2d) const {
        Eigen::Matrix<T, 2, 1> pt2d_distort = CameraModel<T>::pixel_normalize(pt2d);
    
        Eigen::Matrix<T, 2, 1> pt2d_undistort = pt2d_distort;  // Initial guess
        for (unsigned int i = 0; i < 20; ++i) {
          T r_sq = pt2d_undistort[0] * pt2d_undistort[0] +
                   pt2d_undistort[1] * pt2d_undistort[1];
          T k_radial = 1.0 + distort_params_[0] * r_sq +
                       distort_params_[1] * r_sq * r_sq +
                       distort_params_[4] * r_sq * r_sq * r_sq;
          T delta_x_0 =
              2 * distort_params_[2] * pt2d_undistort[0] * pt2d_undistort[1] +
              distort_params_[3] *
                  (r_sq + 2 * pt2d_undistort[0] * pt2d_undistort[0]);
          T delta_x_1 =
              distort_params_[2] *
                  (r_sq + 2 * pt2d_undistort[1] * pt2d_undistort[1]) +
              2 * distort_params_[3] * pt2d_undistort[0] * pt2d_undistort[1];
          pt2d_undistort[0] = (pt2d_distort[0] - delta_x_0) / k_radial;
          pt2d_undistort[1] = (pt2d_distort[1] - delta_x_1) / k_radial;
        }
        return pt2d_undistort;
      }
    
      /**@brief Denormalize a 2D pixel. Convert a 2D pixel as if the image is taken. 非规格化2D像素。转换一个2D像素,就像图像是生成。(考虑畸变系数)
       * with a camera,
       * whose K = intrinsic_. */
      virtual Eigen::Matrix<T, 2, 1> pixel_denormalize(
          const Eigen::Matrix<T, 2, 1>& pt2d) const {
        // Add distortion
        T r_sq = pt2d[0] * pt2d[0] + pt2d[1] * pt2d[1];
        Eigen::Matrix<T, 2, 1> pt2d_radial =
            pt2d *
            (1 + distort_params_[0] * r_sq + distort_params_[1] * r_sq * r_sq +
             distort_params_[4] * r_sq * r_sq * r_sq);
        Eigen::Matrix<T, 2, 1> dpt2d;
        dpt2d[0] = 2 * distort_params_[2] * pt2d[0] * pt2d[1] +
                   distort_params_[3] * (r_sq + 2 * pt2d[0] * pt2d[0]);
        dpt2d[1] = distort_params_[2] * (r_sq + 2 * pt2d[1] * pt2d[1]) +
                   2 * distort_params_[3] * pt2d[0] * pt2d[1];
    
        Eigen::Matrix<T, 2, 1> pt2d_undistort;
        pt2d_undistort[0] = pt2d_radial[0] + dpt2d[0];
        pt2d_undistort[1] = pt2d_radial[1] + dpt2d[1];
        // Add intrinsic K
        return CameraModel<T>::pixel_denormalize(pt2d_undistort);
      }
    
     protected:
      /**@brief The distortion parameters.
       *
       * See here for the definition of the parameters:
       * http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html
       */
      Eigen::Matrix<T, 5, 1> distort_params_;
    };
    
    template <typename T>
    std::ostream& operator<<(std::ostream& cout, const CameraModel<T>& camera) {
      cout << camera.intrinsic_ << "\n [" << camera.width_ << "," << camera.height_
           << "]\n";
      return cout;
    }
    
    template <typename T>
    std::ostream& operator<<(std::ostream& cout, const CameraDistort< camera) {
      cout << camera.intrinsic_ << "\n [" << camera.width_ << "," << camera.height_
           << "]\n";
      cout << camera.distort_params_;
    
      return cout;
    }
    
    typedef CameraModel<double> CameraD;
    typedef CameraDistort<double> CameraDistortD;
    
    }  // namespace perception
    }  // namespace apollo
    
    #endif  // MODULES_PERCEPTION_OBSTACLE_CAMERA_COMMON_CAMERA_H_
    

    目标属性部分:

    struct alignas(16) VisualObject {
      // Per-frame object id, assigned from detection 每帧对象id,从检测开始分配
      int id = 0;
      // Confidence of objectness, ranging as [0, 1] 对象的可信度,范围为[0,1]
      float score = 0.0f;
    
      // [pixel] 2D bounding box [像素]2D边框
      // upper-left corner: x1, y1 左上角:x1, y1
      Eigen::Vector2f upper_left; 
      // lower-right corner: x2, y2 右下角:x2, y2
      Eigen::Vector2f lower_right;
    
      // front box upper-left corner: x1, y1 前框左上角:x1, y1
      Eigen::Vector2d front_upper_left;
      // front box  lower-right corner: x2, y2 前框右下角:x2, y2
      Eigen::Vector2d front_lower_right;
    
      // front box upper-left corner: x1, y1 后框左上角:x1, y1
      Eigen::Vector2d back_upper_left;
      // front box  lower-right corner: x2, y2 后框右下角:x2, y2
      Eigen::Vector2d back_lower_right;
    
      // 2Dto3D, pts8.resize(16), x, y...
      std::vector<float> pts8;
    
      // 2D bounding box truncation ratio, for out-of-image objects 非图像对象的2D边框截断率
      float trunc_width = 0.0f;
      float trunc_height = 0.0f;
    
      // Object type from detection 来自检测的对象类型
      ObjectType type = ObjectType::UNKNOWN;
      // Probability of each object type 每个对象类型的概率
      std::vector<float> type_probs;
    
      // ROI pooling feature from layers of deep learning detection model. 深度学习检测模型的ROI pooling特征。单个物体的特征维度,这里是576
      std::vector<float> object_feature;
    
      // Internal object classification type. 内部对象分类类型
      InternalObjectType internal_type;
      // Internal probability of each type, used for track type. 每种类型的内部概率,用于跟踪类型
      float internal_type_probs[INT_MAX_OBJECT_TYPE];
    
      // [meter] physical size of 3D oriented bounding box 。[物理]三维定向包围框的物理大小
      // length is the size in the main direction 。长度是主要方向的尺寸
      float length = 0.0f;
      float width = 0.0f;
      float height = 0.0f;
    
      // [radian] observation angle of object, ranging as [-pi, pi] 。 [radian]对象的观测角度,范围为[-pi, pi]
      float alpha = 0.0f;
    
      // [radian] Rotation around the vertical axis, ranging as [-pi, pi]  。[radian]绕垂直轴旋转,范围为[-pi, pi]
      // the yaw angle, theta = 0.0f means direction = (1, 0, 0) 。偏航角,theta = 0.0f表示方向= (1,0,0)
      float theta = 0.0f;
      // main direction 主要方向
      Eigen::Vector3f direction = Eigen::Vector3f(1.0f, 0.0f, 0.0f);
    
      // [meter] center of the object。 [物理] 物体的中心
      Eigen::Vector3f center = Eigen::Vector3f::Zero();
      // [meter] distance to object physical center from camera origin  [meter]从相机原点到物体物理中心的距离
      float distance = 0.0f;
      // [meter / second] physical velocity of the object, (vx, vy, vz) [米/秒]物体的物理速度,(vx, vy, vz)
      Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
      // kalman filter state uncertainty set by different sensor type  根据不同的传感器类型设置卡尔曼滤波器的状态不确定度
      // each sensor need to model individually  每个传感器需要单独建模
      Eigen::Matrix<double, 4, 4> state_uncertainty =
          Eigen::Matrix<double, 4, 4>::Identity();
      // globally unique tracking id for camera visual objects  摄像头视觉对象的全局唯一跟踪id
      int track_id = 0;
      // [second] age of the tracked object  [second]被跟踪对象的年龄
      double track_age = 0.0;
      // [second] the last observed timestamp  [second]最后观察到的时间戳
      double last_track_timestamp = 0.0;
    };
    

    其他视觉测距算法-基于相似三角形的单目测距

    算法原理

    我们将使用相似三角形来计算相机到一个已知的物体或者目标的距离。 相似三角形就是这么一回事:假设我们有一个宽度为 W 的目标或者物体。然后我们将这个目标放在距离我们的相机为 D 的位置。我们用相机对物体进行拍照并且测量物体的像素宽度 P 。这样我们就得出了相机焦距的公式:F = (P x D) / W
    举个例子,假设我在离相机距离 D = 24 英寸的地方放一张标准的 8.5 x 11 英寸 A4 纸(横着放;W = 11)并且拍下一张照片。我测量出照片中 A4 纸的像素宽度为 P = 249 像素。因此我的焦距 F 是:
    F = (248px x 24in) / 11in = 543.45
    当我继续将我的相机移动靠近或者离远物体或者目标时,我可以用相似三角形来计算出物体离相机的距离:D’ = (W x F) / P
    从以上的解释中,我们可以看到,要想得到距离,我们就要知道摄像头的焦距和目标物体的大小,这两个已知条件根据公式:D’ = (W x F) / P 
    得出目标到摄像机的距离D,其中P是指像素距离,W是A4纸的宽度,F是摄像机焦距。

    代码

    import numpy as np
    import cv2
    # 找到目标函数
    def find_marker(image):
        # convert the image to grayscale, blur it, and detect edges
        #将图像转换成灰度、模糊和检测边缘
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        gray = cv2.GaussianBlur(gray, (5, 5), 0)
        edged = cv2.Canny(gray, 35, 125)
    
        # find the contours in the edged image and keep the largest one;
        #在边缘图像中找到轮廓并保持最大的轮廓
        # we'll assume that this is our piece of paper in the image
        #我们假设这是我们在图像中的一张纸
        (_, cnts, _) = cv2.findContours(edged.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
        # 求最大面积
        c = max(cnts, key = cv2.contourArea)
    
        # compute the bounding box of the of the paper region and return it
        #计算纸张区域的边界框并返回它
        # cv2.minAreaRect() c代表点集,返回rect[0]是最小外接矩形中心点坐标,
        # rect[1][0]是width,rect[1][1]是height,rect[2]是角度
        return cv2.minAreaRect(c)
    
    # 距离计算函数
    def distance_to_camera(knownWidth, focalLength, perWidth):
        # compute and return the distance from the maker to the camera
        #计算并返回从目标到相机的距离
        return (knownWidth * focalLength) / perWidth
    
    # initialize the known distance from the camera to the object, which
    # in this case is 24 inches
    #初始化已知距离从相机到对象,在这种情况下是24英寸
    KNOWN_DISTANCE = 24.0
    
    # initialize the known object width, which in this case, the piece of
    # paper is 11 inches wide
    #初始化已知的物体宽度,在这种情况下,纸是11英寸宽。
    # A4纸的长和宽(单位:inches)
    KNOWN_WIDTH = 11.69
    KNOWN_HEIGHT = 8.27
    
    # initialize the list of images that we'll be using
    #初始化我们将要使用的图像列表
    IMAGE_PATHS = ["Picture1.jpg", "Picture2.jpg", "Picture3.jpg"]
    
    # load the furst image that contains an object that is KNOWN TO BE 2 feet
    # from our camera, then find the paper marker in the image, and initialize
    # the focal length
    #加载包含一个距离我们相机2英尺的物体的第一张图像,然后找到图像中的纸张标记,并初始化焦距
    #读入第一张图,通过已知距离计算相机焦距
    image = cv2.imread("E:\\lena.jpg") #应使用摄像头拍的图
    marker = find_marker(image)
    focalLength = (marker[1][0] * KNOWN_DISTANCE) / KNOWN_WIDTH  #  D’ = (W x F) / P
    
    #通过摄像头标定获取的像素焦距
    #focalLength = 811.82
    print('focalLength = ',focalLength)
    
    #打开摄像头
    camera = cv2.VideoCapture(0)
    
    while camera.isOpened():
        # get a frame
        (grabbed, frame) = camera.read()
        marker = find_marker(frame)
        if marker == 0:
        print(marker)
        continue
        inches = distance_to_camera(KNOWN_WIDTH, focalLength, marker[1][0])
    
        # draw a bounding box around the image and display it
        #在图像周围绘制一个边界框并显示它
        box = cv2.boxPoints(marker)
        box = np.int0(box)
        cv2.drawContours(frame, [box], -1, (0, 255, 0), 2)
    
        # inches 转换为 cm
        cv2.putText(frame, "%.2fcm" % (inches *30.48/ 12),
                 (frame.shape[1] - 200, frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX,
             2.0, (0, 255, 0), 3)
    
        # show a frame
        cv2.imshow("capture", frame)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
    camera.release()
    cv2.destroyAllWindows()
    

    参考资料

    单目摄像机测距

    毕业课题项目——基于单目摄像头的距离测量

    展开全文
  • 本论文主要基于图像的针孔成像原理设计了一种单目测距算法,它是一种结合了图像处理的许多基本流程,是一个从采集的图像中检测并提取目标特征的一种测距方法。其中,算法的实现主要是基于一个开源计算机视觉库OpenCV...
  • 摄像机的高度为 H ,光轴与地面夹角为α ,目标点 P与光心 的连线与Y轴的夹角为β ,与光轴的夹角为γ ,点 P 在成像平面上的投影点为P ‘,位置满足小孔成像原理,点 P 在世界坐标系下的坐标为P (0,Y ) ,投影点 P...
  • MATLAB的单目视觉车辆测距技术研究
  • python程序,在opencv下使用单目摄像头,测量人到摄像头的距离,行人检测。
  • 单目测距C++工程代码和原理说明文档。 在kitti数据集中进行测试。 详细说明请阅读: https://blog.csdn.net/hgz_gs/article/details/116021280
  • 【机器视觉】 单目视觉测距

    万次阅读 多人点赞 2018-01-25 15:54:47
    单目视觉测距 2016-03-05 23:43 1800人阅读 评论(0) 收藏 举报  分类: 视觉(6)  版权声明:本文为博主原创文章,未经博主允许不得转载。 1、引言 近年来,由于无人机、
  • 构建单目测距系统模型。利用小孔成像原理建立测距的理论模型,根据模型的需要对摄像机进行标定,实现摄像机内参数的配置。
  • 单目视觉测距系统

    2018-07-29 10:31:38
    视觉可以帮助人类粗略的感知...难像人类一样高效的利用视觉进行导航的,而视觉导航(尤其是单目视觉) 相对于传统的利用传感器进行导航的方式,其在低成本、实时性以及高精确 度等方面表现出来的优势增加了它的研究价值
  • 双目测距的基本原理是相似三角形原理理,具体理论可以参考大佬博客。根据相似三角形原理,见图如下图所示。 假设虚线为镜头平面,左边为成像平面,右边为物体所在的平面,H为物体的长度,h为图像的像素长度,f为...
  • 本文还是在传统机器视觉的基础上讨论单目测距,深度学习直接估计深度图不属于这个议题,主要通过mobileye的论文管中窥豹,相信离实际工程应用还有很远。 mobileye2003年的论文:Vision-based ACC with a Single ...
  • OpenCV 单目测距实现

    万次阅读 多人点赞 2018-12-23 00:57:29
    最近要做一个小项目,要完成相机的测距实现... 于是就想试试单目测距的效果怎么样,通过参考网络上的各种资料,加上以前玩过三角激光测距,所以也算比较顺利的写出来个简易实现单目测距的代码,精度还算差强人意。 ...
  • 单目测距

    千次阅读 2019-03-13 15:44:58
     本项目是使用单目摄像头实现距离的测量,首先单目摄像头与kinect等深度摄像头最大 的区别是无法有效获取深度信息,那就首先从这方面入手,尝试通过图像获取摄像头与人的距 离。 在网上看了几天关于摄像头标定和...
  • 单目视觉定位测距的两种方式

    万次阅读 多人点赞 2017-05-05 16:12:55
    单目定位和双目定位的选择,我觉得主要还是成本和时间的考虑。之前也尝试过双目定位,感觉要更精准些,但双目测距需要对两幅图像进行图像变换和极线匹配,稍微耗时了一些。这几天尝试了一下单摄像头进行测距定位,...
  • 单目视觉测量中,由于模型自身的限制,沿摄像...最后,分别对单目摄像机激光测距传感器系统的测量原理、参数标定以及测量过程中的数据融合等方面进行了理论推导与实验研究,实验数据表明了系统方案的可行性和有效性。
  • python opencv单目测距 小孔成像原理

    千次阅读 2020-09-16 12:45:57
    python opencv单目测距 小孔成像原理小孔成像原理代码 opencv>3.x 小孔成像原理 一 用相似三角形计算物体或者目标到相机的距离 我们将使用相似三角形来计算相机到一个已知的物体或者目标的距离。 相似三角形...
  • 机器视觉测量主要分为:单目视觉测量、双目视觉测量、结构光视觉测量等。结构光由于光源的限制,应用的场合比较固定;双目视觉难点在于特征点的匹配,影响了测量的精度和效率,其理论研究的重点集中于特征的匹配上;...
  • 基于OPENCV的单目测距

    万次阅读 多人点赞 2018-08-23 10:12:57
     最近研究了一下单目测距,关于单目测距原理有各位大神的讲解,这里只写一些自已使用上的记录,使用环境为windows10+opencv3.1+vs2015。  买了一个摄像头(笔记本的定焦摄像头也可以),不知道具体参数,想用它...
  • 单目测距算法

    千次阅读 2020-05-16 17:04:04
    单目测距算法 相似三角形 用相似三角形计算物体或者目标到相机的距离,将使用相似三角形来计算相机到一个已知的物体或者目标的距离。 假设有一个宽度为 W 的目标或者物体。然后将这个目标放在距离的相机为 D 的位置...
  • 作者:CV_Community 来源:计算机视觉社区本文还是在传统机器视觉的基础上讨论单目测距,深度学习直接估计深度图不属于这个议题,主要通过mobileye的论文管中窥豹,相信离实际工...
  • 我们可以想到在单目测量中我们在图像中找到某个目标以后可以获得的是一组像素的坐标,对于这样的一组像素并没有与实际空间中的转换关系,所以我们使用视觉传感的第一步就是尝试进行视觉标定,将图像中的坐标转换为...
  • 双目视觉测距原理,数学推导及三维重建资源

    万次阅读 多人点赞 2018-01-09 21:15:20
    先说一下单/双目的测距原理区别:单目测距原理:先通过图像匹配进行目标识别(各种车型、行人、物体等),再通过目标在图像中的大小去估算目标距离。这就要求在估算距离之前首先对目标进行准确识别,是汽车还是行人...
  • 相机模型、相机标定及基于yolov5的单目测距实现

    千次阅读 热门讨论 2021-07-17 21:44:34
    相机模型、相机标定及基于yolov5的单目测距实现1 前言2 相机模型及单目测距原理3 相机参数标定3.1 内参矩阵3.2 内参标定 1 前言 在摄像头成像过程中,物体反射的光线通过摄像头的凸透镜打在成像器件上,形成一张图片...
  • 单目视觉测距

    万次阅读 2019-04-23 10:09:25
    单目视觉测距 一、单目视觉测距背景 单目视觉测距一般采用对应点标定法来获取图像的深度信息,对应点标定法是指通过不同坐标系中对应点的对应坐标求解坐标系的转换关系,但对应点标定法,在标定过程中,由于受器材...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 907
精华内容 362
关键字:

单目测距原理