精华内容
下载资源
问答
  • 该算法采用金字塔Lucas-Kanade光流法,通过计算“帧平均加速度”对人群的运动进行分析。实验结果表明所提出的算法能够快速、有效地检测出人群运动的异常,而且相对于单纯运用运动速度值大小进行检测方法,检测结果更...
  • Unity3D_向量(Vector3)数学基础

    万次阅读 多人点赞 2018-03-08 11:49:34
    向量(Vector3) 在虚拟的游戏世界中,3D数学决定了游戏,...应用十分广泛,可用于描述具有大小和方向两个属性的物理量,例如物体运动的速度、加速度、摄像机观察方向、刚体受到的力等都是向量。因此向量是物理、动...

    向量(Vector3)

    在虚拟的游戏世界中,3D数学决定了游戏,如何计算和模拟出开发者以及玩家看到的每一帧画面。学习基础的3D数学知识可以帮主用户对游戏引擎产生更深刻的了解。 
    向量定义:既有大小又有方向的量叫做向量。在空间中,向量用一段有方向的线段来表示。应用十分广泛,可用于描述具有大小和方向两个属性的物理量,例如物体运动的速度、加速度、摄像机观察方向、刚体受到的力等都是向量。因此向量是物理、动画、三维图形的基础。 
    与向量相对的量成为标量:即只有大小没有方向的量。例如物体移动中的平均速率、路程。 
    :向量的长度标准化(Normalizing):保持方向不变,将向量的长度变为1. 
    单位向量:长度为1的向量。 
    零向量:各分量均为0的向量 
    向量运算——加减:向量的加法(减法)为各个分量分别相加(相减)。在物理上可以用来计算两个里的合力,或者几个速度份量的叠加。 
    这里写图片描述 
    向量运算——数乘向量一个标量相乘称为数乘。数乘可以对向量的长度进行缩放,如果标量大于0,那么向量的方向不变,若标量小于0,则向量的方向会变为反方向。 
    向量运算——点乘两个向量点乘得到一个标量数值等于两个向量长度相乘再乘以两者夹角的余弦值。如果两个向量a,b均为单位向量,那么a.b等于向量b在向量a方向上的投影的长度(或者说向量a在向量b方向上的投影)。 

    把词语拆成字逐个分析。
    “弦”代表长,也就是斜边,从“勾三股四弦五”中迁移过来。
    “正”就是正对,表示直角三角形中角的对边。
    “余”代表相邻,表示直角三角形中与角相邻的直角边。
    “切”有垂直之意,在圆的切线中有体现。
    这样一来,正弦就是对边比斜边,余弦就是邻边比斜边,正切就是对边比(与对边垂直的)邻边。
    我们常说切割,在数学里,切和割是相差很远的,比如切线和割线。所以在三角函数里,切割相反。

     

    这里写图片描述

    叉乘两个向量的叉乘得到一个新的向量新向量垂直与原来的两个向量并且长度等于原来向量长度相乘后夹角的正弦值

    叉乘不满足交换律 即a×b 不等于 b×a。 

    这里写图片描述

    • 属性

      forward Vector3(0, 0, 1)的简码,也就是向z轴。 
      right Vector3(1, 0, 0)的简码,也就是向x轴。 
      up Vector3(0, 1, 0)的简码,也就是向y轴。 
      zero Vector3(0, 0, 0)的简码。 
      one 是 Vector3(1, 1, 1)的简码。 
      Vector3.sqrMagnitude 长度平方(只读的) 
      【注】计算长度的平方而不是magnitude是非常快的。如果你是比较两个向量的长度差,你可以比较他们的平方长度。 
      向量的长度是用勾股定理计算出来,计算机计算两次方和开根的运算量比加减法要费时的多。所以如果是想比较两个向量的长度,用sqrMagnitude可以快出很多。

    • 向量运算

      向量加法 
      向量加法就是两个向量对应的x,y,z轴坐标进行加法运算 
      例如以下的代码

    Vector3 v1 = new Vector3(1, 2, 3);
    Vector3 v2 = new Vector3(4, 2, 1);
    Vector3 v3 = v1 + v2;  //v3的结果 (5.0, 4.0, 4.0)
    Debug.Log(v3);
    • 如果v1和v2都表示一个点的话,那么v3的方向是从v1开始指向v2的一个带有箭头的射线 此时v3就是一个向量 

    如果v1和v2都表示一个向量的话,那么v3是一个从v1的尾部指向v2的头部的一个带有方向箭头的一条射线 
    这里写图片描述 
    向量减法 
    向量加法就是两个向量对应的x,y,z轴坐标进行减法运算 
    例如以下的代码

    Vector3 v1 = new Vector3(1, 2, 3);
            Vector3 v2 = new Vector3(4, 2, 1);
            Vector3 v3 = v2 - v1;  //v3的结果 (3.0, 0.0, -2.0)
            Debug.Log(v3);
    • 其实就是从向量b头部指向向量a头部的一个向量 

    向量减法图片 
    向量数乘 
    实数和向量相乘的过程就是数乘 
    如果实数大于0 那么数乘后的向量的方向和原始向量的方向一致,如果实数小于0 那么数乘后的向量的方向和原始向量的方向相反  
    数乘的几何意义就是沿着原始变量的方向或者变量的相反方向放大或者缩小

    • 方法

      Vector3.Dot 点乘 (又称”点积”,”数量积”,”内积”)(Dot Product, 用*) 
      定义a·b=|a|·|b|cos< a,b> 【注:粗体小写字母表示向量,< a,b>表示向量a,b的夹角,取值范围为[0,180]】 
      几何意义是一条边向另一条边的投影乘以另一条边的长度. 
      v1和v2向量的点乘运算:相应元素的乘积的和:v1( x1, y1,z1) v2(x2, y2,z2) = x1x2 + y1y2+z1z2; 
      注意 : 结果不是一个向量,而是一个标量。 
      性质1 ab = |a||b|Cos(θ) ,θ是向量a 和向量 b之间的夹角。 
      性质2 ab = b*a 满足乘法交换律 
      Unity项目应用: 
      1.根据点乘计算两个向量的夹角。< a,b>= arccos(a·b / (|a|·|b|)) 
      2.根据点乘的正负值,得到夹角大小范围,【大于>0,则夹角(0,90)】【 小于<0,则夹角(90,180)】,可以利用这点判断一个多边形是面向摄像机还是背向摄像机。 
      3.根据点乘的大小,得到向量的投影长度,反应了向量的长度关系。 
      4.在生产生活中,点积同样应用广泛。利用点积可判断一个多边形是否面向摄像机还是背向摄像机。向量的点积与它们夹角的余弦成正比,因此在聚光灯的效果计算中,可以根据点积来得到光照效果,如果点积越大,说明夹角越小,则物理离光照的轴线越近,光照越强。物理中,点积可以用来计算合力和功。若b为单位矢量,则点积即为a在方向b的投影,即给出了力在这个方向上的分解。功即是力和位移的点积。计算机图形学常用来进行方向性判断,如两矢量点积大于0,则它们的方向朝向相近;如果小于0,则方向相反。矢量内积是人工智能领域中的神经网络技术的数学基础之一,此方法还被用于动画渲染(Animation-Rendering)。

      Vector3.Cross 叉乘 (又称”叉积”,”向量积”,”外积”)(cross product,用x) 
      定义
      c = a x b,其中a b c均为向量 
      几何意义是得到一个与这两个向量都垂直的向量,这个向量的模是以两个向量为边的平行四边形的面积 
      v1和v2向量的叉乘运算:相应元素的乘积的和:v1( x1, y1,z1) x v2(x2, y2, z2) = (y1z2 - y2z1)i+(x2z1 - x1z2)j+(x1y2-x2y1)k; 
      利用三阶行列式计算 
      |i j k| 
      |x1 y1 z1| 
      |x2 y2 z2| 
      性质1c⊥a,c⊥b,即向量c与向量a,b所在平面垂直 
      性质2模长|c| = |a||b| sin< a,b> 
      性质3(数学上)满足右手法则, a x b = -b x a,所以我们可以使用叉乘的正负值来判断

      Unity当中叉乘的左手法则 
      Unity项目应用: 
      1.根据叉乘得到a,b向量的相对位置,和顺时针或逆时针方位。 
      简单的说: 点乘判断角度,叉乘判断方向。 
      形象的说: 当一个敌人在你身后的时候,叉乘可以判断你是往左转还是往右转更好的转向敌人,点乘得到你当前的面朝向的方向和你到敌人的方向的所成的角度大小。 
      2.得到a,b夹角的正弦值,计算向量的夹角(0,90),可以配合点乘和Angle方法计算出含正负的方向。 
      3.根据叉乘大小,得到a,b向量所形成的平行四边形的面积大小,根据面积大小得到向量的相对大小。

      Vector3.Distance 距离

      void Test()
      {
         Vector3 v1 = new Vector3(0, 0, 2);
         Vector3 v2 = new Vector3(2, 0, 0);
         //求两个点之间的距离
         Debug.Log(Vector3.Distance(v1,v2));
      
      }
      • Vector3.Lerp 插值 

      obj1的位置是上一帧的位置加上(目标位置-上一帧的位置)*0.1

    obj1.transform.position = Vector3.Lerp(obj1.transform.position, obj2.transform.position, 0.1f);
    • 1

    Vector3.Normalize 规范化 
    使向量编程长度为1的单位向量 
    - Vector2和Vector4

    Vector2 二维向量 
    这个结构用于在一些地方表示2D的位置和向量(例如:网格中的纹理坐标,或者材质中的纹理偏移)。在其他情况下大多数使用Vector3。其操作基本可Vector3差不多 
    静态变量 
    one 
    Vector2(1, 1)的简写。 
    right 
    Vector2(1, 0)的简写。 
    up 
    Vector2(0, 1)的简写。 
    zero 
    Vector2(0, 0)的简写。

    Vector4 二维向量 
    表示四维向量。 
    这个结构在一些地方用来表示四维向量(例如:网格切线,着色器的参数)。在其他情况下大多数使用Vector3。 
    其他操作和Vector3雷同



    参考代码

    点乘

    using System.Collections;
    using System.Collections.Generic;
    using UnityEngine;
    
    public class Vector3Test : MonoBehaviour {
    
        // Use this for initialization
        void Start () {
            Vector3 v1 = new Vector3(1, 0, 0);
            Vector3 v2 = new Vector3(1, 0, 1);
            TestDot(v1, v2);
        }
    
        // Update is called once per frame
        void Update () {
    
        }
    
        private void TestDot(Vector3 a,Vector3 b)
        {
            //计算两个向量点乘的结果 得到的是一个数值
            //求得的是向量b在向量a方向上的投影
            float result = Vector3.Dot(a,b);
            Debug.Log("Result = "+result);
            //计算两个向量的夹角,该方法得到的是一个角度  计算出来的夹角的范围是0-180度
            float angle = Vector3.Angle(a,b);
            Debug.Log("angle = " + angle);
            //向量b方向上的单位向量在向量a方向单位向量的投影
            //计算 a、b 单位向量的点积,得到夹角余弦值,|a.normalized|*|b.normalized|=1;  
            result = Vector3.Dot(a.normalized,b.normalized);
            Debug.Log("result = " + result);
            // 通过反余弦函数获取 向量 a、b 夹角(默认为 弧度)  
            float radians = Mathf.Acos(result);
            angle = radians * Mathf.Rad2Deg;
            Debug.Log(angle);
        }
    
    }
    
    • 叉乘

     

    展开全文
  • 软件开发基本知识

    2019-07-12 20:55:00
    1.通用计算机的组成 输入设备,存储器,控制器,输出设备,计算 ...如何选择cpu:主频,缓存大小,内核数,第几代 cpu型号U的都是超薄笔记本,性能稍差 缓存的作用非常大,所以缓存比较大的c...

    1.通用计算机的组成

    输入设备,存储器,控制器,输出设备,计算

    2.了解各种硬件及其参数

    1.主板:mother board

    性能参数:芯片组  不同厂家的cpu主板不同

    cpu的数据处理速度可以达到内存存取的速度的200倍

    CPU:控制 计算

    如何选择cpu:主频,缓存大小,内核数,第几代

    cpu型号加U的都是超薄笔记本,性能稍差

    缓存的作用非常大,所以缓存比较大的cpu一定不差

    2.内存:内存对开发者非常重要 (程序员就选cpu和内存就可以了)

    两个核心指标:容量   频率

    容量:最低8G(购买内存为8G单条的电脑)然后自己升级

    频率:DDR1600 DDR2600

    频率决定内存的存取速度,与其成正比

    3,硬盘

    机械硬盘:容量大,数度慢,不能摔

    富态硬盘:能摔

    4.显卡:AI计算能力,开赛车是独显,考完驾照是集显

    强大计算能力的子系统,达到CPU的1000倍以上

    显卡是简单的小CPu很多个,但是不适合复杂任务的处理

    CPU是几个大核在一起,适合复杂任务,但总计算能力不够

    转载于:https://www.cnblogs.com/LambertCao/p/11178356.html

    展开全文
  • VINS-Mono——初始化

    千次阅读 2019-10-28 16:01:29
    初始化的原因是单目惯性紧耦合系统是一个非线性程度很高的系统,首先单目是无法获得空间中的绝对尺度,而IMU又必然存在偏置,在后面进行求解的时候还需要用到重力加速度(包括大小和方向),对于速度比较敏感的条件...

    估计器初始化

    • 初始化的原因是单目惯性紧耦合系统是一个非线性程度很高的系统,首先单目是无法获得空间中的绝对尺度,而IMU又必然存在偏置,在后面进行求解的时候还需要用到重力加速度(包括大小和方向),对于速度比较敏感的条件下,比如说无人机,又要精确的速度信息,因此,如何有效的在紧耦合系统处理之前计算出这些量,对整个紧耦合系统的鲁棒性有着重大的意义
    • 初始化要做的事其实说起来很简单,就是计算出绝对尺度s、陀螺仪偏置bg、加速度偏置ba、重力加速度G和每个IMU时刻的速度v
    • VINS-Mono 的初始化采用松组合(loosely coupled)的方式获取初始值。 首先用 SFM求解滑动窗口内所有帧的位姿(以第一帧作为参考坐标系)和所有路标点的 3D 位置。然后将 SFM 的结果与 IMU 预积分的值进行对齐,实现对陀螺仪偏置的校正,再求解每一帧对应的速度,求解重力向量方向,恢复单目相机的尺度因子。
    • 需要注意的是,在初始化过程中,并没有对加速度计的偏置进行校正,这是因为重力是初始化过程中待求的量,而加速度计偏置与重力耦合,而且系统的加速度相对于重力加速度很小,所以加速度计偏置在初始化过程中很难观测,因此初始化过程中不考虑加速度计偏置的校正。

    滑动窗口(Sliding Window)纯视觉SfM

    首先,我们检查了最新帧与之前所有帧之间的特征对应。如果我们能在滑动窗口中的最新帧和任何其他帧之间,找到稳定的特征跟踪(超过30个跟踪特征)和足够的视差(超过20个的旋转补偿像素),使用五点法恢复这两个帧之间的相对旋转和尺度平移。否则,将最新的帧保存在窗口中,并等待新的帧。如果五点算法成功的话,任意设置尺度,并对这两个帧中观察到的所有特征进行三角化。基于这些三角特征,采用PnP来估计窗口中所有其他帧的姿态。最后,应用全局光束平差法(BA)最小化所有特征观测的重投影误差。由于我们还没有任何世界坐标系的知识,我们将第一个相机坐标系()c0(·)^{c_0}设置为SfM的参考坐标系。所有帧的位姿(pˉckc0qckc0)(\bar p^{c0}_{c_k},q^{c0}_{c_k})和特征位置表示相对于()c0(·)^{c_0}。假设摄像机和IMU之间有一个粗略测量的外部参数(pcb,qcb)(p^b_c,q^b_c),我们可以将姿态从相机坐标系转换到物体(IMU)坐标系。

    纯视觉初始化时,我们采用第一帧 c0 作为基准坐标系,若要转化为从 body 坐标系到 c0坐标系,可以进行如下变换,其中s是匹配视觉结构与距离尺度的尺度参数,解出尺度参数是实现成功初始化的关键。
    在这里插入图片描述
    上式推导如下:
    在这里插入图片描述
    在这里插入图片描述

    B. 视觉惯性校准(Visual-Inertial Alignment)

    陀螺仪偏置标定

    这 一 部 分 的 内 容 对 应 于 VINS-Mono 代 码 initial_aligment.cpp 中 的
    solveGyroscopeBias()函数。
    考虑滑动窗口中连续两帧bkb_kbk+1b_{k+1},我们从视觉sfM中得到旋转qbkc0q^{c0}_{b_k}qbk+1c0q^{c0}_{b_{k+1}}​​,从IMU预积分得到的相对约束γbk+1bkγ^{b_k}_{b_{k+1}}​。
    陀螺仪的误差有两部分测量噪声和陀螺仪偏置,噪声暂时可以忽略(毕竟太小),而视觉的误差就只有观测噪声(也可以忽略不管),因此两者差值的绝对值就是陀螺仪偏置,将整个滑动窗口的所有的旋转做差构成了一个最小化误差模型:
    在这里插入图片描述
    其中B代表窗口的所有帧。
    qbkc0q^{c0}_{b_k}qbk+1c0q^{c0}_{b_{k+1}}:相机从bkb_kbk+1b_{k+1}下的相对旋转
    γbk+1bkγ^{b_k}_{b_{k+1}}:陀螺仪从bk+1b_{k+1}bkb_k下的相对旋转
    第二个式子给出了γbk+1bkγ^{b_k}_{b_{k+1}}​对陀螺仪偏置的一阶近似。

    因为四元数最小值为单位四元数 [1; 0v]T,所以
    在这里插入图片描述
    只考虑虚部,则有:
    在这里插入图片描述

    然后取最小二乘,当然也可以使用SVD分解等方法求解,得到了陀螺仪偏置bwb_w的初始校准。然后我们用新的陀螺仪偏置重新传递所有的IMU预积分项α^bk+1bkβ^bk+1bkγ^bk+1bk\hat α^{b_k}_{b_{k+1}}、\hat β^{b_k}_{b_{k+1}}、\hat γ^{b_k}_{b_{k+1}}

    速度、重力向量和尺度初始化:

    这一部分的内容对应于 VINS-Mono 代码 initial_aligment.cpp 中的 LinearAlignment()
    函数
    在陀螺仪偏置初始化后,我们继续初始化导航的其他基本状态,即速度、重力向量和尺度:
    在这里插入图片描述
    其中,vbkbkv^{b_k}_{b_k}​​是第k帧图像本体坐标系的速度,gc0g^{c_0}c0c_0坐标系中的重力向量,s是单目SfM到公制单位的尺度。
    c0c_0坐标系的预积分:
    在这里插入图片描述
    pbk+1c0p^{c_0}_{b_{k+1}}pbkc0p^{c_0}_{b_{k}}可由视觉 SFM 获得:
    在这里插入图片描述
    将此式带入上式得:
    在这里插入图片描述
    将等式中速度都转换到c0c_0 坐标系下:
    在这里插入图片描述
    将上式转换成Hx=bHx=b 的形式,这样便于利用 cholesky 进行求解,由spˉbkc0=pckc0Rckc0pcbs\bar p^{c_0}_{b_k}=p^{c_0}_{c_k}-R^{c_0}_{c_k}p_c^b,带入上式得:

    在这里插入图片描述
    联力等式:
    在这里插入图片描述
    即: H6×10XI 10×1=b6×1H^{6×10}X_I^{\ 10×1} = b^{6×1}
    H矩阵一定是一个正定对称矩阵,以采用快速的 Cholosky 分解下面方程求解XIX_I
    在这里插入图片描述
    可以得到滑动窗口中所有关键帧的本体坐标系速度,视觉参照系()c0(·)^{c_0}的重力向量,以及单目尺度因子 s 。

    修正重力矢量

    这里计算的重力吸收了重力加速度计的偏置,虽然不需要计算重力加速度计的偏置,但重力还是需要优化的,说到优化重力加速度,肯定包含两个量,大小和方向,也就是三个维度,但是一般来说大小是确定已知的(这里设为9.8),因此其实我们要做的就是优化方向,是一个两维的向量,下图是优化重力的方法以及b1,b2单位向量的方向确定模型。
    在这里插入图片描述将重力向量重新参数化:
    在这里插入图片描述
    其中g是已知的重力大小,g^ˉ\bar {\hat g}​​是表示重力方向的单位向量,b1、 b2为重力向量正切空间的一对
    正交基 ,如图所示,w1和w2分别是在b1和b2上的对应位移。
    将上式代入前面式子中,重新整理可得:
    在这里插入图片描述
    在这里插入图片描述
    这样,可以用 Cholosky 分解下面方程求解XIX_I
    在这里插入图片描述

    完成初始化:经过对重力向量的细化,通过将重力旋转到z轴上,得到世界坐标系与摄像机坐标系c0之间的旋转qc0wq^w_{c_0}​。然后我们将所有变量从参考坐标系()c0(·)^{c_0} 旋转到世界坐标系()w(·)^w。本体坐标系的速度也将被旋转到世界坐标系。视觉SfM的变换矩阵将被缩放到度量单位。此时,初始化过程已经完成,所有这些度量值都将被输入到一个紧耦合的单目VIO中。

    代码解析

    流程图

    在这里插入图片描述

    解析

    直接从estimator.cpp中的 if (solver_flag == INITIAL) 开始

        if (solver_flag == INITIAL) {
            // 滑窗中的Keyframe达到指定大小的时候,才开始优化
            if (frame_count == WINDOW_SIZE) {
                bool result = false;
                if (ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1) {
                    result = initialStructure(); //! 初始化
                    initial_timestamp = header.stamp.toSec();
                }
                if (result) {
                    solver_flag = NON_LINEAR;
                    solveOdometry();
                    slideWindow();
                    f_manager.removeFailures();
                    ROS_INFO("Initialization finish!");
                    last_R = Rs[WINDOW_SIZE];
                    last_P = Ps[WINDOW_SIZE];
                    last_R0 = Rs[0];
                    last_P0 = Ps[0];
    
                } else
                    slideWindow();
            } else
                frame_count++;
    

    initialStructure() 视觉惯性联合初始化

    • 1.通过计算线加速度的标准差,检测IMU的可观性,以进行初始化
      注意这里并没有算上all_image_frame的第一帧,所以求均值和标准差的时候要减一
        //! 通过计算预积分加速度的标准差,检测IMU的可观性
        //check imu observibility
        {
            // 计算均值
            map<double, ImageFrame>::iterator frame_it;
            Vector3d sum_g;
            for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
            {
                double sum_dt  = frame_it->second.pre_integration->sum_dt;
                Vector3d tmp_g = frame_it->second.pre_integration->delta_v / sum_dt;
                sum_g += tmp_g;
            }
            Vector3d aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);
    
            // 计算方差
            double var = 0;
            for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
            {
                double sum_dt  = frame_it->second.pre_integration->sum_dt;
                Vector3d tmp_g = frame_it->second.pre_integration->delta_v / sum_dt;
                var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);
            }
    
            // 计算标准差
            var = sqrt(var / ((int)all_image_frame.size() - 1));
            //ROS_WARN("IMU variation %f!", var);
            if(var < 0.25) //! 以标准差判断可观性
            {
                ROS_INFO("IMU excitation not enouth!");
                //return false;
            }
        }
    
    • 2.将f_manager中的所有feature保存到vector sfm_f中,SFMFeature数组中包含了特征点状态(是否被三角化),id,2d点,3d坐标以及深度,将特征管理器中的特征信息保存到SFMFeature对象sfm_f中sfm_f.push_back(tmp_feature)。
    struct SFMFeature
    {
        bool state;//状态(是否被三角化)
        int id;
        vector<pair<int,Vector2d>> observation;//所有观测到该特征点的图像帧ID和图像坐标
        double position[3];//3d坐标
        double depth;//深度
    };
    
        // 遍历滑窗内所有的Features,以vector<SFMFeature>形式保存滑窗内所有特征点
        vector<SFMFeature> sfm_f;
        for (auto &it_per_id : f_manager.feature)
        {
            int imu_j = it_per_id.start_frame - 1;
    
            SFMFeature tmp_feature;
            tmp_feature.state = false;
            tmp_feature.id = it_per_id.feature_id;
            for (auto &it_per_frame : it_per_id.feature_per_frame)
            {
                imu_j++;
                Vector3d pts_j = it_per_frame.point;
                tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}));
            }
            sfm_f.push_back(tmp_feature);
        }
    
    • 3.relativePose()恢复出R、t

      1.先通过 FeatureManager::getCorresponding()获取滑动窗口中第i帧和最后一帧的特征匹配corres
      2.如果第i帧和最后一帧的特征匹配数corres大于20,且所有匹配的特征点的平均视差大于一定阈值,通过solveRelativeRT(定义在solv_5pts.cpp类中)用五点法求本质矩阵cv::findFundamentalMat 计算出当前帧到参考帧的 T

      值得注意:relativePose得到的位姿是第l帧的,第l帧的筛选是从第一帧开始到滑动窗口所有帧中一开始满足平均视差足够大的帧,这里的第l帧会作为参考帧到下面的全局SFM使用。这样得到图像的特征点2d坐标的提取,相机第l帧和最后一帧之间的旋转和平移

        if (!relativePose(relative_R, relative_T, l))
        {
            ROS_INFO("Not enough features or parallax; Move device around");
            return false;
        }
      

      bool relativePose(relative_R, relative_T, l)
      该函数判断每帧到窗口最后一帧对应特征点的平均视差大于30,且内点数目大于12则可进行初始化,同时返回当前帧到第l帧的坐标系变换R和T

    bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l) {
        // find previous frame which contians enough correspondance and parallex with newest frame
    
        // 在滑窗内寻找与最新的关键帧共视点超过20(像素点)的关键帧
        for (int i = 0; i < WINDOW_SIZE; i++) {
            vector<pair<Vector3d, Vector3d>> corres;
            //获取第i帧和最后一帧的特征匹配corres
            corres = f_manager.getCorresponding(i, WINDOW_SIZE);
    
            // 共视的Features应该大于20
            if (corres.size() > 20) {
    
                // 求取匹配的特征点在图像上的视差和(归一化平面上)
                double sum_parallax = 0;
                for (int j = 0; j < int(corres.size()); j++) {
                    Vector2d pts_0(corres[j].first(0), corres[j].first(1));
                    Vector2d pts_1(corres[j].second(0), corres[j].second(1));
                    double parallax = (pts_0 - pts_1).norm();
                    sum_parallax = sum_parallax + parallax;
                }
    
                // 求取所有匹配的特征点的平均视差
                double average_parallax = 1.0 * sum_parallax / int(corres.size());
    
                // 视差大于一定阈值,并且能够有效地求解出变换矩阵
                if (average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T)) {
                    l = i;
                    ROS_DEBUG("average_parallax %f choose l %d and newest frame to triangulate the whole structure",
                              average_parallax * 460, l);
                    return true;
                }
            }
        }
        return false;
    }
    

    FeatureManager::getCorresponding()

    vector<pair<Vector3d, Vector3d>> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r) {
        vector<pair<Vector3d, Vector3d>> corres;
        for (auto &it : feature) {
            // 保证两帧的id大于当前特征点的起始id小于当前特征点的终止id
            if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r) {
                int idx_l = frame_count_l - it.start_frame;
                int idx_r = frame_count_r - it.start_frame;
    
                Vector3d a = it.feature_per_frame[idx_l].point;
                Vector3d b = it.feature_per_frame[idx_r].point;
    
                corres.push_back(make_pair(a, b));
            }
        }
        return corres;
    }
    

    solveRelativeRT()

    bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
    {
        if (corres.size() >= 15)
        {
            //! Step1:提取匹配完的Features
            vector<cv::Point2f> ll, rr;
            for (int i = 0; i < int(corres.size()); i++)
            {
                ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
                rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
            }
    
            cv::Mat mask;
            //! Step2:利用Ransac算法计算本质矩阵,内外点的阈值距离设定为0.3 / 460
            cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
    
            cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
    
            //! Step3:计算变换矩阵并得到内点个数
            cv::Mat rot, trans;
            int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);
            //cout << "inlier_cnt " << inlier_cnt << endl;
    
            //! 得到变换矩阵 ll ==> rr
            Eigen::Matrix3d R;
            Eigen::Vector3d T;
            for (int i = 0; i < 3; i++)
            {   
                T(i) = trans.at<double>(i, 0);
                for (int j = 0; j < 3; j++)
                    R(i, j) = rot.at<double>(i, j);
            }
    
            //! Step4:得到旋转矩阵和平移量 rr ==> ll
            Rotation    =  R.transpose();
            Translation = -R.transpose() * T;
    
            //! 判断求取的内点个数是否满足要求
            if(inlier_cnt > 12)
                return true;
            else
                return false;
        }
        return false;
    }
    
    • 4.对窗口中每个图像帧求解sfm问题,调用sfm.construct(frame_count + 1, Q, T,l,relative_R, relative_T,sfm_f, sfm_tracked_points)估计slidingwindow中所有图像帧相对于参考帧(这里以第l帧作为参考帧)的旋转四元数Q、平移向量T和特征点坐标sfm_tracked_points。
        // 三角化恢复滑窗内的Features
        GlobalSFM sfm;
        Quaterniond Q[frame_count + 1];
        Vector3d    T[frame_count + 1];
        map<int, Vector3d> sfm_tracked_points;
        if(!sfm.construct(frame_count + 1, Q, T, l, relative_R, relative_T, sfm_f, sfm_tracked_points))
        {
            ROS_DEBUG("global SFM failed!");
            marginalization_flag = MARGIN_OLD;
            return false;
        }
    

    bool GlobalSFM::construct()
    函数位于inital_sfm.cpp文件中
    纯视觉sfm,求解窗口中所有图像帧的位姿QT(相对于第l帧)和特征点坐标sfm_tracked_points
    frame_num=frame_count + 1=11,frame_num-1表示当前帧

    1.把第l帧看作参考坐标系,根据当前帧到第l帧的relative_R,relative_T,得到当前帧在参考坐标系下的位姿,之后的pose[i]表示第l帧到第i帧的变换矩阵[R|T]
    2.三角化第l帧(参考帧)与第frame_num-1帧(当前帧)的路标点
    3.pnp求解参考坐标系到第l+1开始的每一帧的变换矩阵R_initial, P_initial,保存在Pose中,并与当前帧进行三角化
    4.对第l帧与从第l+1到frame_num-2的每一帧再进行三角化
    5.PNP求解参考坐标系到从第l-1到第0帧的每一帧之间的变换矩阵,并进行三角化
    6.三角化其他未恢复的特征点。至此得到了滑动窗口中所有图像帧的位姿以及特征点的3d坐标
    7.使用cares进行全局BA优化
    8.得到的是第l帧坐标系到各帧的变换矩阵,将其转变为每一帧在第l帧坐标系上的位姿

    bool GlobalSFM::construct(int frame_num, Quaterniond* q, Vector3d* T, int l,
    			  const Matrix3d relative_R, const Vector3d relative_T,
    			  vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points)
    {
    	feature_num = sfm_f.size();
    	//cout << "set 0 and " << l << " as known " << endl;
    	// have relative_r relative_t
    	// intial two view
    	q[l].w() = 1;
    	q[l].x() = 0;
    	q[l].y() = 0;
    	q[l].z() = 0;
    	T[l].setZero();
    	q[frame_num - 1] = q[l] * Quaterniond(relative_R);
    	T[frame_num - 1] = relative_T;
    	//cout << "init q_l " << q[l].w() << " " << q[l].vec().transpose() << endl;
    	//cout << "init t_l " << T[l].transpose() << endl;
    
    	//rotate to cam frame
    	Matrix3d c_Rotation[frame_num];
    	Vector3d c_Translation[frame_num];
    	Quaterniond c_Quat[frame_num];
    	double c_rotation[frame_num][4];
    	double c_translation[frame_num][3];
    	Eigen::Matrix<double, 3, 4> Pose[frame_num];
    
    	c_Quat[l] = q[l].inverse();
    	c_Rotation[l] = c_Quat[l].toRotationMatrix();
    	c_Translation[l] = -1 * (c_Rotation[l] * T[l]);
    	Pose[l].block<3, 3>(0, 0) = c_Rotation[l];
    	Pose[l].block<3, 1>(0, 3) = c_Translation[l];
    
    	c_Quat[frame_num - 1] = q[frame_num - 1].inverse();
    	c_Rotation[frame_num - 1] = c_Quat[frame_num - 1].toRotationMatrix();
    	c_Translation[frame_num - 1] = -1 * (c_Rotation[frame_num - 1] * T[frame_num - 1]);
    	Pose[frame_num - 1].block<3, 3>(0, 0) = c_Rotation[frame_num - 1];
    	Pose[frame_num - 1].block<3, 1>(0, 3) = c_Translation[frame_num - 1];
    
    
    	//1: trangulate between l ----- frame_num - 1
    	//2: solve pnp l + 1; trangulate l + 1 ------- frame_num - 1; 
    	for (int i = l; i < frame_num - 1 ; i++)
    	{
    		// solve pnp
    		//pnp求解参考坐标系到第l+1开始的每一帧的变换矩阵R_initial, P_initial,保存在Pose中
    		if (i > l)
    		{
    			Matrix3d R_initial = c_Rotation[i - 1];
    			Vector3d P_initial = c_Translation[i - 1];
    			if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
    				return false;
    			c_Rotation[i] = R_initial;
    			c_Translation[i] = P_initial;
    			c_Quat[i] = c_Rotation[i];
    			Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
    			Pose[i].block<3, 1>(0, 3) = c_Translation[i];
    		}
    
    		// triangulate point based on the solve pnp result
    		triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);
    	}
    	//3: triangulate l-----l+1 l+2 ... frame_num -2
    	for (int i = l + 1; i < frame_num - 1; i++)
    		triangulateTwoFrames(l, Pose[l], i, Pose[i], sfm_f);
    	//4: solve pnp l-1; triangulate l-1 ----- l
    	//             l-2              l-2 ----- l
    	//PNP求解参考坐标系到从第l-1到第0帧的每一帧之间的变换矩阵,并进行三角化
    	for (int i = l - 1; i >= 0; i--)
    	{
    		//solve pnp
    		Matrix3d R_initial = c_Rotation[i + 1];
    		Vector3d P_initial = c_Translation[i + 1];
    		if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))
    			return false;
    		c_Rotation[i] = R_initial;
    		c_Translation[i] = P_initial;
    		c_Quat[i] = c_Rotation[i];
    		Pose[i].block<3, 3>(0, 0) = c_Rotation[i];
    		Pose[i].block<3, 1>(0, 3) = c_Translation[i];
    		//triangulate
    		triangulateTwoFrames(i, Pose[i], l, Pose[l], sfm_f);
    	}
    	//5: triangulate all other points
    	//三角化其他未恢复的特征点。至此得到了滑动窗口中所有图像帧的位姿以及特征点的3d坐标
    	for (int j = 0; j < feature_num; j++)
    	{
    		if (sfm_f[j].state == true)
    			continue;
    		if ((int)sfm_f[j].observation.size() >= 2)
    		{
    			Vector2d point0, point1;
    			int frame_0 = sfm_f[j].observation[0].first;
    			point0 = sfm_f[j].observation[0].second;
    			int frame_1 = sfm_f[j].observation.back().first;
    			point1 = sfm_f[j].observation.back().second;
    			Vector3d point_3d;
    			triangulatePoint(Pose[frame_0], Pose[frame_1], point0, point1, point_3d);
    			sfm_f[j].state = true;
    			sfm_f[j].position[0] = point_3d(0);
    			sfm_f[j].position[1] = point_3d(1);
    			sfm_f[j].position[2] = point_3d(2);
    			//cout << "trangulated : " << frame_0 << " " << frame_1 << "  3d point : "  << j << "  " << point_3d.transpose() << endl;
    		}		
    	}
    
    /*
    	for (int i = 0; i < frame_num; i++)
    	{
    		q[i] = c_Rotation[i].transpose(); 
    		cout << "solvePnP  q" << " i " << i <<"  " <<q[i].w() << "  " << q[i].vec().transpose() << endl;
    	}
    	for (int i = 0; i < frame_num; i++)
    	{
    		Vector3d t_tmp;
    		t_tmp = -1 * (q[i] * c_Translation[i]);
    		cout << "solvePnP  t" << " i " << i <<"  " << t_tmp.x() <<"  "<< t_tmp.y() <<"  "<< t_tmp.z() << endl;
    	}
    */
    	//full BA
    	//使用cares进行全局BA优化
    	ceres::Problem problem;
    	ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();
    	//cout << " begin full BA " << endl;
    	for (int i = 0; i < frame_num; i++)
    	{
    		//double array for ceres
    		c_translation[i][0] = c_Translation[i].x();
    		c_translation[i][1] = c_Translation[i].y();
    		c_translation[i][2] = c_Translation[i].z();
    		c_rotation[i][0] = c_Quat[i].w();
    		c_rotation[i][1] = c_Quat[i].x();
    		c_rotation[i][2] = c_Quat[i].y();
    		c_rotation[i][3] = c_Quat[i].z();
    		problem.AddParameterBlock(c_rotation[i], 4, local_parameterization);
    		problem.AddParameterBlock(c_translation[i], 3);
    		if (i == l)
    		{
    			problem.SetParameterBlockConstant(c_rotation[i]);
    		}
    		if (i == l || i == frame_num - 1)
    		{
    			problem.SetParameterBlockConstant(c_translation[i]);
    		}
    	}
    
    	for (int i = 0; i < feature_num; i++)
    	{
    		if (sfm_f[i].state != true)
    			continue;
    		for (int j = 0; j < int(sfm_f[i].observation.size()); j++)
    		{
    			int l = sfm_f[i].observation[j].first;
    			ceres::CostFunction* cost_function = ReprojectionError3D::Create(
    												sfm_f[i].observation[j].second.x(),
    												sfm_f[i].observation[j].second.y());
    
        		problem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l], 
        								sfm_f[i].position);	 
    		}
    
    	}
    	ceres::Solver::Options options;
    	options.linear_solver_type = ceres::DENSE_SCHUR;
    	//options.minimizer_progress_to_stdout = true;
    	options.max_solver_time_in_seconds = 0.2;
    	ceres::Solver::Summary summary;
    	ceres::Solve(options, &problem, &summary);
    	//std::cout << summary.BriefReport() << "\n";
    	if (summary.termination_type == ceres::CONVERGENCE || summary.final_cost < 5e-03)
    	{
    		//cout << "vision only BA converge" << endl;
    	}
    	else
    	{
    		//cout << "vision only BA not converge " << endl;
    		return false;
    	}
    	for (int i = 0; i < frame_num; i++)
    	{
    		q[i].w() = c_rotation[i][0]; 
    		q[i].x() = c_rotation[i][1]; 
    		q[i].y() = c_rotation[i][2]; 
    		q[i].z() = c_rotation[i][3]; 
    		q[i] = q[i].inverse();
    		//cout << "final  q" << " i " << i <<"  " <<q[i].w() << "  " << q[i].vec().transpose() << endl;
    	}
    	//第l帧坐标系到各帧的变换矩阵,应将其转变为每一帧在第l帧坐标系上的位姿
    	for (int i = 0; i < frame_num; i++)
    	{
    
    		T[i] = -1 * (q[i] * Vector3d(c_translation[i][0], c_translation[i][1], c_translation[i][2]));
    		//cout << "final  t" << " i " << i <<"  " << T[i](0) <<"  "<< T[i](1) <<"  "<< T[i](2) << endl;
    	}
    	for (int i = 0; i < (int)sfm_f.size(); i++)
    	{
    		if(sfm_f[i].state)
    			sfm_tracked_points[sfm_f[i].id] = Vector3d(sfm_f[i].position[0], sfm_f[i].position[1], sfm_f[i].position[2]);
    	}
    	return true;
    
    }
    
    
    1. 对于所有的图像帧,包括不在滑动窗口中的,提供初始的RT估计,然后solvePnP进行优化
        // solve pnp for all frame
        map<int, Vector3d>::iterator it;
        map<double, ImageFrame>::iterator frame_it = all_image_frame.begin( );
        for (int i = 0; frame_it != all_image_frame.end( ); frame_it++)
        {
            // provide initial guess
            if((frame_it->first) == Headers[i].stamp.toSec())
            {
                frame_it->second.is_key_frame = true;
                frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose();
                frame_it->second.T = T[i];
                i++;
                continue;
            }
            if((frame_it->first) > Headers[i].stamp.toSec())
            {
                i++;
            }
    
            // 将滑窗内第i帧的变换矩阵当做初始值
            Matrix3d R_inital = (Q[i].inverse()).toRotationMatrix();
            Vector3d P_inital = - R_inital * T[i];
            cv::Mat rvec, t, tmp_r;
            cv::eigen2cv(R_inital, tmp_r);
            cv::Rodrigues(tmp_r, rvec);
            cv::eigen2cv(P_inital, t);
    
            frame_it->second.is_key_frame = false;
    
            vector<cv::Point3f> pts_3_vector;
            vector<cv::Point2f> pts_2_vector;
            for (auto &id_pts : frame_it->second.points)
            {
                int feature_id = id_pts.first;
                for (auto &i_p : id_pts.second)
                {
                    it = sfm_tracked_points.find(feature_id);
                    if(it != sfm_tracked_points.end())
                    {
                        Vector3d world_pts = it->second;
                        cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));
                        pts_3_vector.push_back(pts_3);
    
                        Vector2d img_pts = i_p.second.head<2>();
                        cv::Point2f pts_2(img_pts(0), img_pts(1));
                        pts_2_vector.push_back(pts_2);
                    }
                }
            }
            if(pts_3_vector.size() < 6) {
                cout << "pts_3_vector size " << pts_3_vector.size() << endl;
                ROS_DEBUG("Not enough points for solve pnp !");
                return false;
            }
    
            cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
    
            cv::Mat D;
            if (! cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, true)) {
                ROS_DEBUG("solve pnp fail!");
                return false;
            }
    
            // PnP求解出的位姿要取逆
            MatrixXd R_pnp;
            MatrixXd T_pnp;
            {
                cv::Mat r;
                cv::Rodrigues(rvec, r);
                MatrixXd tmp_R_pnp;
                cv::cv2eigen(r, tmp_R_pnp);
                R_pnp = tmp_R_pnp.transpose();
    
                cv::cv2eigen(t, T_pnp);
                T_pnp = R_pnp * (-T_pnp);
            }
    
            // 转换到IMU坐标系下
            frame_it->second.R = R_pnp * RIC[0].transpose();
            frame_it->second.T = T_pnp;
        }
    
    • 6.进行视觉惯性联合初始化,imu与视觉对齐,获取绝对尺度s、陀螺仪偏置bg、加速度偏置ba、重力加速度G和每个IMU时刻的速度v
        // 视觉与IMU对齐
        if (visualInitialAlign())
            return true;
        else
        {
            ROS_INFO("misalign visual structure with IMU");
            return false;
        }
    

    bool Estimator::visualInitialAlign()
    该函数主要实现了陀螺仪的偏置校准(加速度偏置没有处理),计算速度V[0:n]、重力g、尺度s。
    同时更新了Bgs后,IMU测量量需要repropagate;得到尺度s和重力g的方向后,需更新所有图像帧在世界坐标系下的Ps、Rs、Vs。

    bool Estimator::visualInitialAlign()
    {
        TicToc t_g;
    
        VectorXd x;
    
        //solve scale
        // 要注意这个地方求解出的g是在C0坐标系下
        //1.计算陀螺仪偏置,尺度,重力加速度和速度
        bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);
        if(!result) {
            ROS_DEBUG("solve g failed!");
            return false;
        }
    
        // change state
        //2.获取所有图像帧的位姿Ps、Rs,并将其置为关键帧
        for (int i = 0; i <= frame_count; i++)
        {
            Matrix3d Ri = all_image_frame[Headers[i].stamp.toSec()].R;
            Vector3d Pi = all_image_frame[Headers[i].stamp.toSec()].T;
            Ps[i] = Pi;
            Rs[i] = Ri;
            all_image_frame[Headers[i].stamp.toSec()].is_key_frame = true;
        }
    
        //将所有特征点的深度置为-1
        VectorXd dep = f_manager.getDepthVector();
        for (int i = 0; i < dep.size(); i++)
            dep[i] = -1;
        f_manager.clearDepth(dep);
    
        //triangulat on cam pose , no tic
        //3.重新计算特征点的深度
        Vector3d TIC_TMP[NUM_OF_CAM];
        for(int i = 0; i < NUM_OF_CAM; i++)
            TIC_TMP[i].setZero();
        ric[0] = RIC[0];
        f_manager.setRic(ric);
        f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));
    
        double s = (x.tail<1>())(0);
        //4.陀螺仪的偏置bgs改变,重新计算预积分
        for (int i = 0; i <= WINDOW_SIZE; i++)
        {
            pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
        }
        //5.将Ps、Vs、depth尺度s缩放后转变为相对于第0帧图像坐标系下
        for (int i = frame_count; i >= 0; i--)
            Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);
        int kv = -1;
        map<double, ImageFrame>::iterator frame_i;
        for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)
        {
            if(frame_i->second.is_key_frame)
            {
                kv++;
                Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);
            }
        }
        for (auto &it_per_id : f_manager.feature)
        {
            it_per_id.used_num = it_per_id.feature_per_frame.size();
            if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
                continue;
            it_per_id.estimated_depth *= s;
        }
    
        //6.通过将重力旋转到z轴上,得到世界坐标系与摄像机坐标系c0之间的旋转矩阵rot_diff
        Matrix3d R0 = Utility::g2R(g);
        double yaw = Utility::R2ypr(R0 * Rs[0]).x();
        R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
        g = R0 * g;
        //Matrix3d rot_diff = R0 * Rs[0].transpose();
        Matrix3d rot_diff = R0;
        //7.所有变量从参考坐标系c0旋转到世界坐标系w
        for (int i = 0; i <= frame_count; i++)
        {
            Ps[i] = rot_diff * Ps[i];
            Rs[i] = rot_diff * Rs[i];
            Vs[i] = rot_diff * Vs[i];
        }
        ROS_DEBUG_STREAM("g0     " << g.transpose());
        ROS_DEBUG_STREAM("my R0  " << Utility::R2ypr(Rs[0]).transpose()); 
    
        return true;
    }
    

    参考:
    VINS-Mono代码解读——视觉惯性联合初始化 initialStructure sfm
    VINS理论与代码详解4——初始化

    展开全文
  • Unity基础3_3D数学基础

    2017-10-18 14:26:47
    向量(Vector3) 在虚拟的游戏世界中,3D数学决定了游戏,如何计算和...应用十分广泛,可用于描述具有大小和方向两个属性的物理量,例如物体运动的速度、加速度、摄像机观察方向、刚体受到的力等都是向量。因此向量是

    向量(Vector3)

    在虚拟的游戏世界中,3D数学决定了游戏,如何计算和模拟出开发者以及玩家看到的每一帧画面。学习基础的3D数学知识可以帮主用户对游戏引擎产生更深刻的了解。 
    向量定义:既有大小又有方向的量叫做向量。在空间中,向量用一段有方向的线段来表示。应用十分广泛,可用于描述具有大小和方向两个属性的物理量,例如物体运动的速度、加速度、摄像机观察方向、刚体受到的力等都是向量。因此向量是物理、动画、三维图形的基础。 
    与向量相对的量成为标量:即只有大小没有方向的量。例如物体移动中的平均速率、路程。 
    模:向量的长度标准化(Normalizing):保持方向不变,将向量的长度变为1. 
    单位向量:长度为1的向量。 
    零向量:各分量均为0的向量 
    向量运算——加减:向量的加法(减法)为各个分量分别相加(相减)。在物理上可以用来计算两个里的合力,或者几个速度份量的叠加。 
    这里写图片描述 
    向量运算——数乘:向量与一个标量相乘称为数乘。数乘可以对向量的长度进行缩放,如果标量大于0,那么向量的方向不变,若标量小于0,则向量的方向会变为反方向。 
    向量运算——点乘:两个向量点乘得到一个标量,数值等于两个向量长度相乘再乘以两者夹角的余弦值。如果两个向量a,b均为单位向量,那么a.b等于向量b在向量a方向上的投影的长度(或者说向量a在向量b方向上的投影)。 
    这里写图片描述 
    叉乘:两个向量的叉乘得到一个新的向量,新向量垂直与原来的两个向量,并且长度等于原来向量长度相乘后夹角的正弦值注意:叉乘不满足交换律 即a×b 不等于 b×a。 
    这里写图片描述

    • 属性

      forward Vector3(0, 0, 1)的简码,也就是向z轴。 
      right Vector3(1, 0, 0)的简码,也就是向x轴。 
      up Vector3(0, 1, 0)的简码,也就是向y轴。 
      zero Vector3(0, 0, 0)的简码。 
      one 是 Vector3(1, 1, 1)的简码。 
      Vector3.sqrMagnitude 长度平方(只读的) 
      【注】计算长度的平方而不是magnitude是非常快的。如果你是比较两个向量的长度差,你可以比较他们的平方长度。 
      向量的长度是用勾股定理计算出来,计算机计算两次方和开根的运算量比加减法要费时的多。所以如果是想比较两个向量的长度,用sqrMagnitude可以快出很多。

    • 向量运算

      向量加法 
      向量加法就是两个向量对应的x,y,z轴坐标进行加法运算 
      例如以下的代码

    Vector3 v1 = new Vector3(1, 2, 3);
    Vector3 v2 = new Vector3(4, 2, 1);
    Vector3 v3 = v1 + v2;  //v3的结果 (5.0, 4.0, 4.0)
    Debug.Log(v3);
    • 1
    • 2
    • 3
    • 4

    如果v1和v2都表示一个点的话,那么v3的方向是从v1开始指向v2的一个带有箭头的射线 此时v3就是一个向量 
    如果v1和v2都表示一个向量的话,那么v3是一个从v1的尾部指向v2的头部的一个带有方向箭头的一条射线 
    这里写图片描述 
    向量减法 
    向量加法就是两个向量对应的x,y,z轴坐标进行减法运算 
    例如以下的代码

    Vector3 v1 = new Vector3(1, 2, 3);
            Vector3 v2 = new Vector3(4, 2, 1);
            Vector3 v3 = v2 - v1;  //v3的结果 (3.0, 0.0, -2.0)
            Debug.Log(v3);
    • 1
    • 2
    • 3
    • 4

    其实就是从向量b头部指向向量a头部的一个向量 
    向量减法图片 
    向量数乘 
    实数和向量相乘的过程就是数乘 
    如果实数大于0 那么数乘后的向量的方向和原始向量的方向一致,如果实数小于0 那么数乘后的向量的方向和原始向量的方向相反 
    数乘的几何意义 
    数乘的几何意义:就是沿着原始变量的方向或者变量的相反方向放大或者缩小

    • 方法

      Vector3.Dot 点乘 
      (又称”点积”,”数量积”,”内积”)(Dot Product, 用*) 
      定义:a·b=|a|·|b|cos< a,b> 【注:粗体小写字母表示向量,< a,b>表示向量a,b的夹角,取值范围为[0,180]】 
      几何意义:是一条边向另一条边的投影乘以另一条边的长度. 
      v1和v2向量的点乘运算:相应元素的乘积的和:v1( x1, y1,z1) v2(x2, y2,z2) = x1x2 + y1y2+z1z2; 
      注意 : 结果不是一个向量,而是一个标量。 
      性质1: ab = |a||b|Cos(θ) ,θ是向量a 和向量 b之间的夹角。 
      性质2: ab = b*a 满足乘法交换律 
      Unity项目应用: 
      1.根据点乘计算两个向量的夹角。< a,b>= arccos(a·b / (|a|·|b|)) 
      2.根据点乘的正负值,得到夹角大小范围,>0,则夹角(0,90)< 0,则夹角(90,180),可以利用这点判断一个多边形是面向摄像机还是背向摄像机。 
      3.根据点乘的大小,得到向量的投影长度,反应了向量的长度关系。 
      4.在生产生活中,点积同样应用广泛。利用点积可判断一个多边形是否面向摄像机还是背向摄像机。向量的点积与它们夹角的余弦成正比,因此在聚光灯的效果计算中,可以根据点积来得到光照效果,如果点积越大,说明夹角越小,则物理离光照的轴线越近,光照越强。物理中,点积可以用来计算合力和功。若b为单位矢量,则点积即为a在方向b的投影,即给出了力在这个方向上的分解。功即是力和位移的点积。计算机图形学常用来进行方向性判断,如两矢量点积大于0,则它们的方向朝向相近;如果小于0,则方向相反。矢量内积是人工智能领域中的神经网络技术的数学基础之一,此方法还被用于动画渲染(Animation-Rendering)。

      Vector3.Cross 叉乘 
      (又称”叉积”,”向量积”,”外积”)(cross product,用x) 
      定义:c = a x b,其中a b c均为向量 
      几何意义是:得到一个与这两个向量都垂直的向量,这个向量的模是以两个向量为边的平行四边形的面积 
      v1和v2向量的叉乘运算:相应元素的乘积的和:v1( x1, y1,z1) x v2(x2, y2, z2) = (y1z2 - y2z1)i+(x2z1 - x1z2)j+(x1y2-x2y1)k; 
      利用三阶行列式计算 
      |i j k| 
      |x1 y1 z1| 
      |x2 y2 z2| 
      性质1:c⊥a,c⊥b,即向量c与向量a,b所在平面垂直 
      性质2:模长|c| = |a||b| sin< a,b> 
      性质3:(数学上)满足右手法则, a x b = -b x a,所以我们可以使用叉乘的正负值来判断

      Unity当中叉乘的左手法则 
      Unity项目应用: 
      1.根据叉乘得到a,b向量的相对位置,和顺时针或逆时针方位。 
      简单的说: 点乘判断角度,叉乘判断方向。 
      形象的说: 当一个敌人在你身后的时候,叉乘可以判断你是往左转还是往右转更好的转向敌人,点乘得到你当前的面朝向的方向和你到敌人的方向的所成的角度大小。 
      2.得到a,b夹角的正弦值,计算向量的夹角(0,90),可以配合点乘和Angle方法计算出含正负的方向。 
      3.根据叉乘大小,得到a,b向量所形成的平行四边形的面积大小,根据面积大小得到向量的相对大小。

      Vector3.Distance 距离

      void Test()
      {
         Vector3 v1 = new Vector3(0, 0, 2);
         Vector3 v2 = new Vector3(2, 0, 0);
         //求两个点之间的距离
         Debug.Log(Vector3.Distance(v1,v2));
      
      }
      • 1
      • 2
      • 3
      • 4
      • 5
      • 6
      • 7
      • 8

      Vector3.Lerp 插值 
      obj1的位置是上一帧的位置加上(目标位置-上一帧的位置)*0.1

    obj1.transform.position = Vector3.Lerp(obj1.transform.position, obj2.transform.position, 0.1f);
    • 1

    Vector3.Normalize 规范化 
    使向量编程长度为1的单位向量 
    - Vector2和Vector4

    Vector2 二维向量 
    这个结构用于在一些地方表示2D的位置和向量(例如:网格中的纹理坐标,或者材质中的纹理偏移)。在其他情况下大多数使用Vector3。其操作基本可Vector3差不多 
    静态变量 
    one 
    Vector2(1, 1)的简写。 
    right 
    Vector2(1, 0)的简写。 
    up 
    Vector2(0, 1)的简写。 
    zero 
    Vector2(0, 0)的简写。

    Vector4 二维向量 
    表示四维向量。 
    这个结构在一些地方用来表示四维向量(例如:网格切线,着色器的参数)。在其他情况下大多数使用Vector3。
    其他操作和Vector3雷同



    参考代码

    点乘

    using System.Collections;
    using System.Collections.Generic;
    using UnityEngine;
    
    public class Vector3Test : MonoBehaviour {
    
        // Use this for initialization
        void Start () {
            Vector3 v1 = new Vector3(1, 0, 0);
            Vector3 v2 = new Vector3(1, 0, 1);
            TestDot(v1, v2);
        }
    
        // Update is called once per frame
        void Update () {
    
        }
    
        private void TestDot(Vector3 a,Vector3 b)
        {
            //计算两个向量点乘的结果 得到的是一个数值
            //求得的是向量b在向量a方向上的投影
            float result = Vector3.Dot(a,b);
            Debug.Log("Result = "+result);
            //计算两个向量的夹角,该方法得到的是一个角度  计算出来的夹角的范围是0-180度
            float angle = Vector3.Angle(a,b);
            Debug.Log("angle = " + angle);
            //向量b方向上的单位向量在向量a方向单位向量的投影
            //计算 a、b 单位向量的点积,得到夹角余弦值,|a.normalized|*|b.normalized|=1;  
            result = Vector3.Dot(a.normalized,b.normalized);
            Debug.Log("result = " + result);
            // 通过反余弦函数获取 向量 a、b 夹角(默认为 弧度)  
            float radians = Mathf.Acos(result);
            angle = radians * Mathf.Rad2Deg;
            Debug.Log(angle);
        }
    
    }
    
    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39

    叉乘



    展开全文
  • Unity_3D数学基础_005

    2017-07-22 09:40:50
    向量(Vector3)在虚拟的游戏世界中,3D数学决定了游戏,...应用十分广泛,可用于描述具有大小和方向两个属性的物理量,例如物体运动的速度、加速度、摄像机观察方向、刚体受到的力等都是向量。因此向量是物理、动画、
  • Unity入门操作_向量_002

    2017-08-16 11:55:26
    在虚拟的游戏世界中,3D数学决定了游戏,...应用十分广泛,可用于描述具有大小和方向两个属性的物理量,例如物体运动的速度、加速度、摄像机观察方向、刚体受到的力等都是向量。因此向量是物理、动画、三维图形的基础。
  • Foxpro 开发答疑160问

    2014-10-07 19:55:45
    146. 如何获得和设置鼠标的双击速度 498 147. 如何屏蔽Ctrl+Alt+Del组合键 500 148. 如何打开或关闭光盘驱动器 501 149. 如何获得计算机硬盘卷序列号 502 150. 如何隐藏Windows的任务栏 504 151. 如何在系统...
  • 演示有空气阻力落体的最简方法

    千次阅读 2007-12-17 08:37:00
    如何演示计算空气阻力的落体。我认为下面的方法是最好的。...a 是落体的加速度。a = dv/dt。m*dv/dt = mg - kv 是一个微分方程。对其求解可以得到通解:mg - kv = e-kt/m-kCv0 = 0, t0=0的特解是 v=
  • C#编程经验技巧宝典

    热门讨论 2008-06-01 08:59:33
    98 <br>0153 如何自定义数字小数点左边分组位数 98 <br>0154 格式化输入数据为货币格式 99 <br>0155 如何计算两个整数的乘积 99 <br>0156 如何将二进制数转换为十进制数 100 <br>0157 如何...
  • TCP超时计算是RTOx2,这样连续丢三次包就变成RTOx8了,十分恐怖,而KCP启动快速模式后不x2,只是x1.5(实验证明1.5这个值相对比较好),提高了传输速度。 选择性重传 vs 全部重传: TCP丢包时会全部重传从丢的那个...
  • delphi 开发经验技巧宝典源码

    热门讨论 2010-08-12 16:47:23
    0075 如何计算两个日期之间的天数、周数、分钟数 49 0076 如何实现数字斜塔 50 0077 如何实现杨辉三角 51 0078 如何实现CRC算法 52 0079 如何将B转换成GB、MB和KB 52 3.3 判断及转换类算法 53 0080 ...
  • 0075 如何计算两个日期之间的天数、周数、分钟数 49 0076 如何实现数字斜塔 50 0077 如何实现杨辉三角 51 0078 如何实现CRC算法 52 0079 如何将B转换成GB、MB和KB 52 3.3 判断及转换类算法 53 0080 ...
  • 0075 如何计算两个日期之间的天数、周数、分钟数 49 0076 如何实现数字斜塔 50 0077 如何实现杨辉三角 51 0078 如何实现CRC算法 52 0079 如何将B转换成GB、MB和KB 52 3.3 判断及转换类算法 53 0080 ...
  • 0075 如何计算两个日期之间的天数、周数、分钟数 49 0076 如何实现数字斜塔 50 0077 如何实现杨辉三角 51 0078 如何实现CRC算法 52 0079 如何将B转换成GB、MB和KB 52 3.3 判断及转换类算法 53 0080 ...
  • 0075 如何计算两个日期之间的天数、周数、分钟数 49 0076 如何实现数字斜塔 50 0077 如何实现杨辉三角 51 0078 如何实现CRC算法 52 0079 如何将B转换成GB、MB和KB 52 3.3 判断及转换类算法 53 0080 ...
  • 0075 如何计算两个日期之间的天数、周数、分钟数 49 0076 如何实现数字斜塔 50 0077 如何实现杨辉三角 51 0078 如何实现CRC算法 52 0079 如何将B转换成GB、MB和KB 52 3.3 判断及转换类算法 53 0080 ...
  • Oracle如何精确计算row的大小 38 PL/SQL编程 39 数据库的分组问题 41 oracle知识 42 数据库的导入导出 42 Oracle查询中rownum与Order by查询 45 oracle9i小结 46 Oracle 数据库的聚簇技术 61 数据库、服务名、实例 ...
  • 如何计算旋转矩阵 介绍下机器学习和深度学习中常见的参数类算法和非参数类算法 随机梯度下降 神经网络训练如何解决过拟合和欠拟合 L1正则化和L2正则化区别,具体有何用途 L1正则化相比于 L2正则化为何具有稀疏...
  • 可以不参数,该程序会自动根据内存大小分配适当的内存空间作为高速缓 存),再安装Windows XP即可。另外提醒大家,这个程序在安装完Windows后 ,不要运行,否则Windows可用内存将减少。  4、Win32k.sys是什么...
  • 有限元分析

    2012-07-04 11:25:06
    现以SolidWorks三维设计软件为例,谈谈如何提高绘图速度和效率。 一、定义快捷键 通过多年使用SolidWorks软件的经验,我们体会到最能提高绘图速度的方法是手脑共用,双手配合,一手敲击键盘,一手运动鼠标。Solid...
  •  使用Windows系统平台的缺点之一就是对文件的管理不清楚,你有时根本就不知道这个文件对系统是否有用,因而Windows目录下的文件数目越来越多,容量也越来越庞大,之现在的软件都喜欢越做越大,再加上一些系统产生...
  • 你必须知道的495个C语言问题

    千次下载 热门讨论 2015-05-08 11:09:25
    6.22 如何在一个文件中判断声明为extern的数组的大小(例如,数组定义和大小在另一个文件中)?sizeof操作符似乎不行。 6.23 sizeof返回的大小是以字节计算的,怎样才能判断数组中有多少个元素呢? 第7章 内存...
  • 2、系统设置:该系统须根据控制卡实际控制的点阵大小设置,控制卡根据设置内容计算控制方法。用户可根据实际情况设置任意宽度和高度的组合,控制卡均可正常显示。如果下载数据后发现屏仅仅微亮,请改变"OE低电平...
  • word使用技巧大全

    热门讨论 2011-03-18 20:37:53
    1、如何在word文档中进参考文献的标注 70 2、省时省力——写论文时如何利用word编辑参考文献 71 3、如何在Word中编辑参考文献 72 4、关于[1][2][3]类的脚尾注格式问题,与各位分享(修改) 73 5、利用word中的交叉...
  • 为了克服第一代计算机网络的缺点,提高网络的可靠性和可用性,人们开始研究如何将多台计算机相互连接的方法。人们首先借鉴了电信部门的电路交换的思想。所谓“交换”,从通信资源的分配角度来看,就是由交换设备动态...

空空如也

空空如也

1 2 3 4 5 ... 8
收藏数 157
精华内容 62
关键字:

如何计算加速度大小