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;
    } else //否则认为是纵向平行线相交的地心点，此时fTiltAngle的取值范围45~89度，fTanTilt可认为是ctg(tilt)，地心点的图像坐标y=fy*ctg(tilt)
    {
      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
}