CalibrationCalc.js

import {
  CalcRealCoor,
  CalcImgCoor,
  Mat_Multiply,
  CalcInvK,
  CalcK
} from "./Camera_Parameters.js";

const PI = 3.14159265;
const CameraParameter = {
  m_fTiltAngle: "", //倾斜角度，摄像头与水平面夹角，单位度
  m_fRollAngle: "", //滚动角度，摄像头“滚”的角度，单位度 （备用参数，可能不需要）
  m_fPanAngle: "", //旋转角度，摄像头“摇”的角度，单位度 （备用参数，可
  m_fCCDWidth: "", //ccd宽度＝图像宽度×ccd水平像素距离，（单位：毫米）
  m_fCCDHeight: "", //ccd高度＝图像高度×ccd垂直像素距离，（单
  m_fFocusX: "", //中间数据
  m_fFo: "",
  m_fHeight: "", //相机高度
  m_fCameraFoucs: "", //摄像头的焦
  m_dblK: [], //成像矩阵，以相机为原点，x轴向右，y轴向下，z轴向前
  m_dblInvK: []
};

var CameraParam = CameraParameter;

var CarLength = 4.5,
  CarWidth = 1.7,
  CarHeight = 1.5;
var PedLength = 0.3,
  PedWidth = 0.5,
  PedHeight = 1.7;
var ModelLength = 4.5,
  ModelWidth = 1.7,
  ModelHeight = 1.5;
export var changeModelData = function(type) {
  if (type == "car") {
    ModelLength = CarLength;
    ModelWidth = CarWidth;
    ModelHeight = CarHeight;
  }
  if (type == "person") {
    ModelLength = PedLength;
    ModelWidth = PedWidth;
    ModelHeight = PedHeight;
  }
};
var CalcCrossPoint = function(p1, p2, p3, p4) {
  var fDetM1 = "",
    fDetM2 = "",
    fa = "";
  var p5 = {};

  fDetM1 = (p2.x - p1.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p2.y - p1.y);

  if (fDetM1 < 0.000001 && fDetM1 > -0.000001) {
    fDetM1 = 0.000001;
  }
  fDetM2 = (p3.x - p1.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p3.y - p1.y);
  fa = fDetM2 / fDetM1;

  p5.x = p1.x + fa * (p2.x - p1.x);
  p5.y = p1.y + fa * (p2.y - p1.y);
  return p5;
};

/**
 * 根据平行线和一个标定距离计算相机标定
 * @param {float} fHeight //相机高度
 * @param {float} fRatioW2H //图像宽高比
 * @param {Array} Points //标定的6个点的相对坐标，前四个是一组平行线；最后两点是标定好的距离
 * @param {float} fDist //标定最后两点的距离
 */
export var scRTEngineCalcBaseon6Points = function(
  fHeight,
  fRatioW2H,
  Points,
  fDist
) {
  var VanishPoint,
    fTanTilt,
    fy,
    fMinError = 1000;
  var copyCameraParameter = JSON.stringify(CameraParameter);
  var TempParam = JSON.parse(copyCameraParameter);

  if (fHeight <= 0 || fDist <= 0 || fRatioW2H <= 0) return;

  VanishPoint = CalcCrossPoint(Points[0], Points[1], Points[2], Points[3]);

  if (VanishPoint.y < -100000 || VanishPoint.y > 100000)
    //标定线平行则放弃
    return;

  TempParam.m_fHeight = fHeight;
  CameraParam.m_fHeight = fHeight;
  //搜索error最小的标定
  for (
    fTanTilt = 0.01;
    fTanTilt < 1;
    fTanTilt += 0.005 //范围45~89度
  ) {
    var fTiltAngle; //fTiltAngle为相机与水平线的夹角
    if (VanishPoint.y <= 0.5) {
      //如果标定线交点在图像上方，认为其是地面平行线相交的无穷远点，夹角取值范围0~45度
      fTiltAngle = (Math.atan(fTanTilt) * 180) / PI;
      fy = (0.5 - VanishPoint.y) / fTanTilt;
    } //否则认为是纵向平行线相交的地心点，此时fTiltAngle的取值范围45~89度，fTanTilt可认为是ctg(tilt)，地心点的图像坐标y=fy*ctg(tilt)
    else {
      fTiltAngle = (Math.atan(1.0 / fTanTilt) * 180) / PI;
      fy = (VanishPoint.y - 0.5) / fTanTilt;
    }

    TempParam.m_fTiltAngle = fTiltAngle;
    TempParam.m_fFocusX = fy / fRatioW2H;
    TempParam.m_fFocusY = fy;

    if (TempParam.m_fFocusX != 0) {
      var fTanPan =
        (-(VanishPoint.x - 0.5) *
          Math.cos((TempParam.m_fTiltAngle / 180) * PI)) /
        TempParam.m_fFocusX; //pan角度，根据消失点计算
      TempParam.m_fPanAngle = (Math.atan(fTanPan) * 180) / PI;
    } else {
      TempParam.m_fPanAngle = 0;
    }

    TempParam.m_dblK = CalcK(
      TempParam.m_dblK,
      TempParam.m_fFocusX,
      TempParam.m_fFocusY,
      TempParam.m_fTiltAngle,
      0,
      TempParam.m_fPanAngle
    );
    TempParam.m_dblInvK = CalcInvK(
      TempParam.m_dblInvK,
      TempParam.m_fFocusX,
      TempParam.m_fFocusY,
      TempParam.m_fTiltAngle,
      0,
      TempParam.m_fPanAngle
    );
    //计算头两点的距离，搜索error最小的标定

    {
      var fCurrrError = "";
      var P0 = "",
        P1 = "";
      P0 = CalcRealCoor(Points[4], TempParam.m_dblInvK, TempParam.m_fHeight);
      P1 = CalcRealCoor(Points[5], TempParam.m_dblInvK, TempParam.m_fHeight);
      fCurrrError = Math.abs(
        Math.sqrt(
          (P0.x - P1.x) * (P0.x - P1.x) + (P0.z - P1.z) * (P0.z - P1.z)
        ) - fDist
      );

      if (fCurrrError < fMinError) {
        fMinError = fCurrrError;
        var a = JSON.stringify(TempParam);
        var b = JSON.parse(a);
        CameraParam = b;
      }
    }
  }
};

/**
 * 计算车或人的模型框点位
 * @param {*} P1 模型线点
 * @param {*} P2 模型线点
 * @param {*} CarOutline 传入参数
 */
export var CalcCarOutline = function(P1, P2, CarOutline) {
  var P3d1 = {},
    P3d2 = {};
  var Xdire = {},
    Zdire = {};
  var ftempN;
  P3d1 = CalcRealCoor(P1, CameraParam.m_dblInvK, CameraParam.m_fHeight, P3d1);
  P3d2 = CalcRealCoor(P2, CameraParam.m_dblInvK, CameraParam.m_fHeight, P3d2);
  Zdire.x = P3d2.x - P3d1.x;
  Zdire.y = P3d2.z - P3d1.z;
  ftempN = Math.sqrt(Zdire.x * Zdire.x + Zdire.y * Zdire.y);
  Zdire.x /= ftempN;
  Zdire.y /= ftempN;
  Xdire.x = Zdire.y;
  Xdire.y = -Zdire.x;

  {
    //左上点
    P3d2.x = P3d1.x - (ModelWidth / 2) * Xdire.x;
    P3d2.y = CameraParam.m_fHeight - ModelHeight;
    P3d2.z = P3d1.z - (ModelWidth / 2) * Xdire.y;
    CalcImgCoor(CarOutline[0], CameraParam.m_dblK, P3d2);
    //右上点
    P3d2.x = P3d1.x + (ModelWidth / 2) * Xdire.x;
    P3d2.y = CameraParam.m_fHeight - ModelHeight;
    P3d2.z = P3d1.z + (ModelWidth / 2) * Xdire.y;
    CalcImgCoor(CarOutline[1], CameraParam.m_dblK, P3d2);
    //右下点
    P3d2.x = P3d1.x + (ModelWidth / 2) * Xdire.x;
    P3d2.y = CameraParam.m_fHeight;
    P3d2.z = P3d1.z + (ModelWidth / 2) * Xdire.y;
    CalcImgCoor(CarOutline[2], CameraParam.m_dblK, P3d2);
    //左下点
    P3d2.x = P3d1.x - (ModelWidth / 2) * Xdire.x;
    P3d2.y = CameraParam.m_fHeight;
    P3d2.z = P3d1.z - (ModelWidth / 2) * Xdire.y;
    CalcImgCoor(CarOutline[3], CameraParam.m_dblK, P3d2);
    //前左上点
    P3d2.x = P3d1.x - (ModelWidth / 2) * Xdire.x + ModelLength * Zdire.x;
    P3d2.y = CameraParam.m_fHeight - ModelHeight;
    P3d2.z = P3d1.z - (ModelWidth / 2) * Xdire.y + ModelLength * Zdire.y;
    CalcImgCoor(CarOutline[4], CameraParam.m_dblK, P3d2);
    //前右上点
    P3d2.x = P3d1.x + (ModelWidth / 2) * Xdire.x + ModelLength * Zdire.x;
    P3d2.y = CameraParam.m_fHeight - ModelHeight;
    P3d2.z = P3d1.z + (ModelWidth / 2) * Xdire.y + ModelLength * Zdire.y;
    CalcImgCoor(CarOutline[5], CameraParam.m_dblK, P3d2);
    //前右下点
    P3d2.x = P3d1.x + (ModelWidth / 2) * Xdire.x + ModelLength * Zdire.x;
    P3d2.y = CameraParam.m_fHeight;
    P3d2.z = P3d1.z + (ModelWidth / 2) * Xdire.y + ModelLength * Zdire.y;
    CalcImgCoor(CarOutline[6], CameraParam.m_dblK, P3d2);
    //前左下点
    P3d2.x = P3d1.x - (ModelWidth / 2) * Xdire.x + ModelLength * Zdire.x;
    P3d2.y = CameraParam.m_fHeight;
    P3d2.z = P3d1.z - (ModelWidth / 2) * Xdire.y + ModelLength * Zdire.y;
    CalcImgCoor(CarOutline[7], CameraParam.m_dblK, P3d2);
  }
  return CarOutline;
};