咱们好呀～

概况介绍

遇到一个特别有意思的功用，雷达相机合作，监测呈现在空间的异物，雷达担任定位，相机担任拍摄。咱们要做的是将雷达测得的数据处理后给到相机，保证相机能拍摄到物体，场景如下：

雷达监测到这个空间呈现的物体，获得三个参数，雷达间隔物体的间隔distance、雷达的俯仰角hDeg、雷达与正北方违背的视点nDeg，这三个参数仅有确认了物体的位置。相机要正确的拍摄到物体也需求三个参数，间隔，俯仰角和违背正北方的视点。所以咱们要处理的问题是经过雷达获取到的数据，核算得出相机需求的参数。

雷达和相机的高度（radarHeight,cameraHeight）、雷达与相机的间隔(radarToCamera)、雷达相机地点直线违背正北方的视点(roadDeg)为已知条件。

        //初始化数据（注意视点和弧度的转换）
        //雷达相机连线与北边的夹角
        const roadDeg = ref((Math.PI / 180) * 45);
        //雷达与相机的间隔
        const radarToCamera = ref(500);
        //雷达高度
        const radarHeight = ref(400);
        //相机高度
        const cameraHeight = ref(400);

（我第一版代码用的三角函数，经过三角形三边与角的联系来核算出间隔和视点，这个能够算出来，可是有很多隐藏问题，比如需求用到的视点会着着雷达和相机的位置不同而不同，有爱好的小伙伴能够画图试试～）

所以有了第二版代码，我觉得空间坐标系是一个很好的处理方案。

核算坐标

1.雷达坐标

如上图，我以雷达底部为原点，正东方为x轴正方向，正南方为y轴正方向树立坐标，那么雷达的坐标能够表明为（0，0，radarHeight）

            //雷达坐标
            const radar = ref({ x: 0, y: 0, z: radarHeight.value });

2.相机坐标

因为已知雷达与相机的间隔(radarToCamera)，雷达相机地点直线违背正北方的视点(roadDeg)，经过正余弦定理能够得出相机x和相机y。
相机x = radarToCamera * sin(roadDeg)
相机y = radarToCamera * cos(roadDeg)
相机z = cameraHeight

这儿要注意相机x、y的正负值与相机在第几象限有关，也便是跟雷达相机地点直线违背正北方的视点有关。0-180度，x为正，180-360度，x为负；0-90度或者270-360度，y为负，90-270度，y为正;

          //相机坐标
           const camera = ref({ x: 0, y: 0, z: 0 });
        //核算相机坐标
        const getCameraCoordinate = () => {
          const x = radarToCamera.value * Math.sin(roadDeg.value).toFixed(4);
          const y = radarToCamera.value * Math.cos(roadDeg.value).toFixed(4);
          if (roadDeg.value <= Math.PI) {
            camera.value.x = Math.abs(x);
          } else {
            camera.value.x = -Math.abs(x);
          }
          if (roadDeg.value >= Math.PI / 2 && roadDeg.value <= (Math.PI * 3) / 2) {
            camera.value.y = Math.abs(y);
          } else {
            camera.value.y = -Math.abs(y);
          }
        };

3.异物坐标

接下来核算出异物的坐标，分两种状况，第一种是异物在雷达高度以下（含雷达高度），雷达俯仰角为正；第二种是异物处于比雷达高的地方，雷达俯仰角为负。

第一种状况，先经过雷达测得的俯仰角和间隔核算出异物离雷达顶部的垂直高度，为了核算更便利，此时把水平面提升至异物地点面，核算中的体现是雷达和相机的坐标z都需求减去异物到地上的高度，异物的z为0（相当于重置水平面为异物地点面）。

第二种状况，异物比雷达高，经过三角函数可求得异物坐标。

同样，这儿异物的x,y也需求根据雷达与正北方视点考虑正负～

如图：

//障碍物坐标
 const hinder = ref({ x: 0, y: 0, z: 0 });
//核算障碍物的坐标(参数需传入雷达测得的间隔、与北边的视点、俯仰角)
const getHinderCoordinate = (len, ndeg, hdeg) => {
  let x;
  let y;
  //把视点转化为弧度
  const nRadian = (ndeg % 360 * Math.PI) / 180;
  const hRadian = (hdeg * Math.PI) / 180;
  if (hRadian > 0) {
    //障碍物间隔雷达顶部的高度
    const hinderZ = len * Math.sin(hRadian);
    //将水平面进步至障碍物高度，便于核算
    radar.value.z = hinderZ;
    camera.value.z = cameraHeight.value - (radarHeight.value - hinderZ);
    hinder.value.z = 0;
    //核算雷达底部到障碍物的间隔
    const distance = Math.sqrt(Math.pow(len, 2) - Math.pow(hinderZ, 2));
    x = distance * Math.sin(nRadian).toFixed(4);
    y = distance * Math.cos(nRadian).toFixed(4);
  } else {
    radar.value.z = radarHeight.value;
    camera.value.z = cameraHeight.value;
    hinder.value.z = len * Math.sin(-hRadian) + radarHeight.value;
    x = len * Math.cos(-hRadian) * Math.sin(nRadian).toFixed(4);
    y = len * Math.cos(-hRadian) * Math.cos(nRadian).toFixed(4);
  }
  if (nRadian <= Math.PI) {
    hinder.value.x = Math.abs(x);
  } else {
    hinder.value.x = -Math.abs(x);
  }
  if (nRadian >= Math.PI / 2 && nRadian <= (Math.PI * 3) / 2) {
    hinder.value.y = Math.abs(y);
  } else {
    hinder.value.y = -Math.abs(y);
  }
};

核算获得相机需求的参数

现在雷达、相机、异物的坐标都核算出来了，有坐标了核算长度和视点还不简单，直接上代码！

1.间隔

//核算相机到障碍物的间隔
const getResultDistance = () => {
  //空间中两点之间的间隔公式：d=√[（x1-x2）^2+（y1-y2)）2+（z1-z2）^2]
  let d = Math.sqrt(
    Math.pow(camera.value.x - hinder.value.x, 2) +
      Math.pow(camera.value.y - hinder.value.y, 2) +
      Math.pow(camera.value.z - hinder.value.z, 2)
  );
  return d;
};

2.俯仰角

需求经过雷达的俯仰角判断相机的俯仰角为正还是负

//核算相机的俯仰角，参数为雷达的俯仰角
const getHorizontalDeg = (deg) => {
  let sinDeg;
  let d = getResultDistance();
  if (deg <= 0) {
    sinDeg = -(hinder.value.z - camera.value.z) / d;
  } else {
    sinDeg = camera.value.z / d;
  }
  return (Math.asin(sinDeg) * 180) / Math.PI;
};

3.相机需求相对于正北方违背视点

这儿用到了平面向量求夹角，引入指向正北方的单位向量，求异物向量与单位向量的夹角。

因为向量之间的夹角规模是0-180，可是对于相机来说，相对于北方的视点规模是0-360，所以最终需求判断一步，异物向量的x小于0时，求得的视点c虽然在0-180之间，但实践相机需求旋转360-c

//核算相机与北边的视点
const getNDeg = () => {
  //去掉z，转为平面向量求夹角，向量夹角公式cos=a*b/(|a|*|b|)
  const vectorAB = {
    x: hinder.value.x - camera.value.x,
    y: hinder.value.y - camera.value.y,
  };
  const vectorN = { x: 0, y: -1 };
  const lenAB = Math.sqrt(Math.pow(vectorAB.x, 2) + Math.pow(vectorAB.y, 2));
  const lenN = Math.sqrt(Math.pow(vectorN.x, 2) + Math.pow(vectorN.y, 2));
  const cosDeg =
    (vectorAB.x * vectorN.x + vectorAB.y * vectorN.y) / (lenAB * lenN);
  if (hinder.value.x > 0) {
    return (Math.acos(cosDeg) * 180) / Math.PI;
  } else {
    return 360 - (Math.acos(cosDeg) * 180) / Math.PI;
  }
};

执行函数

经过下面的入口函数，传入参数，就完成啦

//参数为雷达测得的间隔
const start = (len, ndeg, hdeg) => {
  getCameraCoordinate();
  getHinderCoordinate(len, ndeg, hdeg);
  let resultDistance = getResultDistance();
  let horizontalDeg = getHorizontalDeg(hdeg);
  let nDeg = getNDeg();
  //console.log('相机坐标',camera.value);
  //console.log('雷达坐标',radar.value);
  //console.log('异物坐标',hinder.value);
  console.log("相机间隔异物的间隔===>", resultDistance);
  console.log("相机的俯仰角===>", horizontalDeg);
  console.log("相机相对于正北方需求旋转的视点===>", nDeg);
};

测验

最终，便是测验环节，要怎么测验才最高效呢～

想到了一个特别简洁的方法，当雷达和相机无限近的时分，雷达测得的数据直接给相机就能够使用，所以这种状况下咱们输入和输出的数据应该无限挨近。 所以，将radarToCamera设置为0.1

测验了几组数据，如下：

结束语

又整理了一遍思路，不知道还有没有隐藏问题，老觉得没有讲清楚，欢迎咱们指正，也希望能帮助到你们～