精华内容
下载资源
问答
  • 二维装箱问题的非线性优化方法
  • SLAM的后端一般分为两种处理方法,一种是以扩展卡尔曼滤波(EKF)为代表的滤波方法,一种是以图优化为代表的非线性优化方法。不过,目前SLAM研究的主流热点几乎都是基于图优化的。 滤波方法的优缺点: 优点:在...

    SLAM的后端一般分为两种处理方法,一种是以扩展卡尔曼滤波(EKF)为代表的滤波方法,一种是以图优化为代表的非线性优化方法。不过,目前SLAM研究的主流热点几乎都是基于图优化的。

    滤波方法的优缺点:

    优点:在当时计算资源受限、待估计量比较简单的情况下,EKF为代表的滤波方法比较有效,经常用在激光SLAM中。

    缺点:它的一个大缺点就是存储量和状态量是平方增长关系,因为存储的是协方差矩阵,因此不适合大型场景。而现在基于视觉的SLAM方案,路标点(特征点)数据很大,滤波方法根本吃不消,所以此时滤波的方法效率非常低。

     

    图优化:

    历史:

           2008年左右,大家还都是用滤波方法,因为在图优化里,Bundle Adjustment(后面简称BA)起到了核心作用。但是那会SLAM的研究者们发现包含大量特征点和相机位姿的BA计算量其实很大,根本没办法实时。

           后来SLAM研究者们发现了其实在视觉SLAM中,虽然包含大量特征点和相机位姿,但其实BA是稀疏的,稀疏的就好办了,就可以加速了啊!比较代表性的就是2009年,几个大神发表了自己的研究成果《SBA:A software package for generic sparse bundle adjustment》,而且计算机硬件发展也很快,因此基于图优化的视觉SLAM也可以实时了!

    概念:

            比如一个机器人在房屋里移动,它在某个时刻 t 的位姿(pose)就是一个顶点,这个也是待优化的变量。而位姿之间的关系就构成了一个边,比如时刻 t 和时刻 t+1 之间的相对位姿变换矩阵就是边,边通常表示误差项。

    在SLAM里,图优化一般分解为两个任务:

    1、构建图。机器人位姿作为顶点,位姿间关系作为边。

    2、优化图。调整机器人的位姿(顶点)来尽量满足边的约束,使得误差最小。

    例子:

           下面就是一个直观的例子。我们根据机器人位姿来作为图的顶点,这个位姿可以来自机器人的编码器,也可以是ICP匹配得到的,图的边就是位姿之间的关系。由于误差的存在,实际上机器人建立的地图是不准的,如下图左。我们通过设置边的约束,使得图优化向着满足边约束的方向优化,最后得到了一个优化后的地图(如下图中所示),它和真正的地图(下图右)非常接近。

    g2o:General Graphic Optimization

     

     

     

     

     

     

     

     

     

     

    展开全文
  • 求解位姿变换的非线性优化方法1. 李群和李代数2. 求解位姿变换的SVD方法 1. 李群和李代数 2. 求解位姿变换的SVD方法   把三维坐标系OqO_{q}Oq​中的坐标q=(a,b,c)q=(a,b,c)q=(a,b,c)变换到三维坐标系OpO_{p}Op...

    1. 李群和李代数

    2. 求解位姿变换的非线性优化方法

    2.1 误差

      把三维坐标系OqO_{q}中的坐标q=(a,b,c)q=(a,b,c)变换到三维坐标系OpO_{p}中:
    p=exp(ξ)q=(x,y,z)(4.2.1) p' = exp(\xi ^ \land) q = (x',y',z') \tag{4.2.1}
    qqOpO_{p}中的实际坐标是p=(x,y,x)p=(x,y,x),那么上式的误差用向量表示就是:
    e=pp=[xxyyzz](4.2.2) \vec e = p - p' = \left[ \begin{matrix} x-x' \\ y-y' \\ z-z' \end{matrix} \right] \tag{4.2.2}
      qq称为观测值,pp'称为测量值,pp称为实际值。

    2.2 误差对位姿增量的导数

    • 根据向量求导法则:
      ep=[(xx)x(xx)y(xx)z(yy)x(yy)y(yy)z(zz)x(zz)y(zz)z]=[100010001](4.2.3) \frac{\partial \vec e}{\partial \vec p'} = \left[ \begin{matrix} \frac{\partial (x-x')}{\partial x'} & \frac{\partial (x-x')}{\partial y'} & \frac{\partial (x-x')}{\partial z'} \\ \frac{\partial (y-y')}{\partial x'} & \frac{\partial (y-y')}{\partial y'} & \frac{\partial (y-y')}{\partial z'} \\ \frac{\partial (z-z')}{\partial x'} & \frac{\partial (z-z')}{\partial y'} & \frac{\partial (z-z')}{\partial z'} \end{matrix} \right] = - \left[ \begin{matrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{matrix} \right] \tag{4.2.3}
    • 根据李群李代数的推论:
      pδξ=[Ip]=[1000zy010z0x001yx0](4.2.4) \frac{\partial \vec p'}{\partial \delta \xi} = \left[ \begin{matrix} I & -p'^ \land \end{matrix} \right] = \left[ \begin{matrix} 1 & 0 & 0 & 0 & z' & -y' \\ 0 & 1 & 0 & -z' & 0 & x' \\ 0 & 0 & 1 & y' & -x' & 0 \end{matrix} \right] \tag{4.2.4}
    • 根据链式求导法则:
      eδξ=eppδξ=pδξ=[1000zy010z0x001yx0](4.2.5) \frac{\partial \vec e'}{\partial \delta \xi} = \frac{\partial \vec e}{\partial \vec p'} \cdot \frac{\partial \vec p'}{\partial \delta \xi} = - \frac{\partial \vec p'}{\partial \delta \xi} = \left[ \begin{matrix} -1 & 0 & 0 & 0 & -z' & y' \\ 0 & -1 & 0 & z' & 0 & -x' \\ 0 & 0 & -1 & -y' & x' & 0 \end{matrix} \right] \tag{4.2.5}
        当然也可以在公式(4.2.2)中定义e=pp\vec e = p' - p,这样的话,公式(4.2.5)就要乘一个负号。
        公式(4.2.5)右侧的矩阵可以构成一个雅可比矩阵。表示位姿的se(3)(3)的前部是平移后部是旋转,导致公式(4.2.4)中的单位矩阵II在左边,进而公式(4.2.5)中矩阵的后三列是雅可比矩阵的前三列而其前三列是雅可比矩阵的后三列,即雅可比矩阵
      J=[0zy100z0x010yx0001](4.2.6) J = \left[ \begin{matrix} 0 & -z' & y' & -1 & 0 & 0 \\ z' & 0 & -x' & 0 & -1 & 0 \\ -y' & x' & 0 & 0 & 0 & -1 \end{matrix} \right] \tag{4.2.6}

    3. 使用G2O求解位姿

      G2O(GeneralGraphicOptimization)G2O(General Graphic Optimization)是一个基于图优化的库,广泛应用于SLAM中。第三章利用EigenEigen库进行SVDSVD分解求坐标变换,这里使用G2OG2O来求解。

    #include <iostream>
    #include <opencv2/opencv.hpp>
    #include <g2o/core/base_unary_edge.h>
    #include <g2o/core/block_solver.h>
    #include <g2o/core/optimization_algorithm_gauss_newton.h>
    #include <g2o/solvers/eigen/linear_solver_eigen.h>
    #include <g2o/types/sba/types_six_dof_expmap.h>
    
    using namespace std;
    using namespace cv;
    
    // g2o edge
    class EdgeProjectXYZRGBDPoseOnly: public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap> {
      public:
    	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    	EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d& point): _point(point) {}
    
    	virtual void computeError() {
    		const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> (_vertices[0]);
    		// measurement is p, point is p', p' = Rq + t
    		_error = _measurement - pose->estimate().map(_point);
    	}
    
    	virtual void linearizeOplus() {
    		g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
    		/* g2o::SE3Quat T(pose->estimate());
    		Eigen::Vector3d xyz_trans = T.map(_point); */
    		Eigen::Vector3d xyz_trans = pose->estimate().map(_point);
    		double x = xyz_trans[0];
    		double y = xyz_trans[1];
    		double z = xyz_trans[2];
    
    		_jacobianOplusXi(0,0) = 0;
    		_jacobianOplusXi(0,1) = -z;
    		_jacobianOplusXi(0,2) = y;
    		_jacobianOplusXi(0,3) = -1;
    		_jacobianOplusXi(0,4) = 0;
    		_jacobianOplusXi(0,5) = 0;
    
    		_jacobianOplusXi(1,0) = z;
    		_jacobianOplusXi(1,1) = 0;
    		_jacobianOplusXi(1,2) = -x;
    		_jacobianOplusXi(1,3) = 0;
    		_jacobianOplusXi(1,4) = -1;
    		_jacobianOplusXi(1,5) = 0;
    
    		_jacobianOplusXi(2,0) = -y;
    		_jacobianOplusXi(2,1) = x;
    		_jacobianOplusXi(2,2) = 0;
    		_jacobianOplusXi(2,3) = 0;
    		_jacobianOplusXi(2,4) = 0;
    		_jacobianOplusXi(2,5) = -1;
    	}
    
    	bool read(istream& in) {}
    	bool write(ostream& out) const {}
      protected:
    	Eigen::Vector3d _point;
    };
    
    void bundleAdjustment(
    	const vector< Point3f >& pts1,
    	const vector< Point3f >& pts2) {
    
    	// 初始化g2o
    	typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3> > Block;  // pose维度为 6, landmark 维度为 3
    	Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>(); // 线性方程求解器
    	Block* solver_ptr = new Block(linearSolver);      // 矩阵块求解器
    	g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);
    	g2o::SparseOptimizer optimizer;
    	optimizer.setAlgorithm(solver);
    
    	// vertex
    	g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    	pose->setId(0);
    	pose->setEstimate(g2o::SE3Quat(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0, 0, 0)));
    	optimizer.addVertex(pose);
    
    	// edges
    	int index = 1;
    	for(size_t i=0; i<pts1.size(); i++) {
    		EdgeProjectXYZRGBDPoseOnly* edge =
    			new EdgeProjectXYZRGBDPoseOnly(Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z));
    		edge->setId(index);
    		edge->setVertex(0, dynamic_cast<g2o::VertexSE3Expmap*>(pose));
    		edge->setMeasurement(Eigen::Vector3d(pts1[i].x, pts1[i].y, pts1[i].z));
    		edge->setInformation(Eigen::Matrix3d::Identity() * 1e4);
    		optimizer.addEdge(edge);
    		index++;
    	}
    
    	cout << "Before optimization:" << endl;
    	cout << "T=" << endl << fixed << setprecision(2) << Eigen::Isometry3d(pose->estimate()).matrix();
    	cout << endl << endl;
    
    	optimizer.setVerbose(true);
    	optimizer.initializeOptimization();
    	optimizer.optimize(10);
    
    	cout << endl << "After optimization:" << endl;
    	cout << "T=" << endl << fixed << setprecision(2) << Eigen::Isometry3d(pose->estimate()).matrix() << endl;
    }
    
    int main(int argc, char** argv) {
    	vector<Point3f> pts1, pts2;
    	pts1.push_back(Point3f(2, 3, 1));
    	pts1.push_back(Point3f(2, -2, 3));
    	pts1.push_back(Point3f(3, -2, 2));
    	pts1.push_back(Point3f(1, -3, 1));
    	pts1.push_back(Point3f(4, 0, 2));
    	pts1.push_back(Point3f(0, -8, 3));
    	pts2.push_back(Point3f(-4, 2, 1));
    	pts2.push_back(Point3f(1, 2, 3));
    	pts2.push_back(Point3f(1, 3, 2));
    	pts2.push_back(Point3f(2, 1, 1));
    	pts2.push_back(Point3f(-1, 4, 2 ));
    	pts2.push_back(Point3f(7, 0, 3));
    
    	bundleAdjustment(pts1, pts2);
    }
    

    3.1 G2O的边

      类EdgeProjectXYZRGBDPoseOnlyEdgeProjectXYZRGBDPoseOnly定义了图优化中的边。G2OG2O实现了常用的边,但没有实现这里我们要用的3D3D3D-3D的边,所以我们从第1313行到第5959行实现了这样的边。
      G2O的边必须的成员函数:

    1. setId(intid)setId(int id):设置该边的ID。
    2. setVertex(sizeti,Vertexv)setVertex(size_t i, Vertex* v):设置该边的ii号顶点为vv
    3. setMeasurementsetMeasurement:设置实际值。
    4. setInformationsetInformation:未知。
    5. 雅可比矩阵,即误差关于位姿增量的导数。元素来自公式(4.2.6)(4.2.6)

      定义G2O的边的步骤:

    1. 该边继承一元边g2o::BaseUnaryEdgeg2o::BaseUnaryEdge,模板参数代表观测值的维度观测值的数据类型连接的顶点类型
    2. 第15行设置数据对齐。
    3. 第16行初始化该类的成员_point\_point。由第84行可以看出使用坐标系OqO_{q}中的坐标点(观测值)初始化该成员。
    4. 第19行获取该边对应的第一个顶点_vertices[0]\_vertices[0]。因为该类定义的是一个一元边,所以它只有这一个顶点(位姿)。这个顶点在第86行通过调用自己的成员函数setVertexsetVertex被赋值。
    5. 第21行定义误差的计算方式(实际值-测量值)。_measurement\_measurement由成员函数setMeasurementsetMeasurement给定,如第87行。pose>estimate().map(point)pose->estimate().map(_point)把坐标系OqO_{q}中的坐标(观测值)按位姿posepose变换到坐标系OpO_{p}中(测量值)。
    6. 第26和27行等价于第28行,获取pp'的坐标。
    7. 第29到52行构造雅可比矩阵,即误差对位姿增量的导数。

    3.2 BABA3D3D3D-3D坐标变换

    1. 第66行是求解器,模板参数的66代表位姿变量有6维,33代表观测值有3维。
    2. 第67行定义线性求解器,有5种可选:LinearSolverCholmodLinearSolverCholmodLinearSolverCSparseLinearSolverCSparseLinearSolverPCGLinearSolverPCGLinearSolverDenseLinearSolverDenseLinearSolverEigenLinearSolverEigen
    3. 第69行定义优化算法,有三种可选:OptimizationAlgorithmGaussNewtonOptimizationAlgorithmGaussNewtonOptimizationAlgorithmLevenbergOptimizationAlgorithmLevenbergOptimizationAlgorithmDoglegOptimizationAlgorithmDogleg
    4. 第74到第77行添加一个位姿顶点。
    5. 第80到第89行创建多个边并用观测值初始化:第85行设置边的唯一顶点(位姿);第86行设置实际值。
    6. 第96行设置是否输出优化日志。

      输出结果是:

    Before optimization:
    T=
    1.00 0.00 0.00 0.00
    0.00 1.00 0.00 0.00
    0.00 0.00 1.00 0.00
    0.00 0.00 0.00 1.00

    iteration= 0 chi2= 465988.840354 time= 0.000591696 cumTime= 0.000591696 edges= 6 schur= 0
    iteration= 1 chi2= 21766.670043 time= 0.000448715 cumTime= 0.00104041 edges= 6 schur= 0
    iteration= 2 chi2= 36.238111 time= 0.000438133 cumTime= 0.00147854 edges= 6 schur= 0
    iteration= 3 chi2= 0.000005 time= 0.000431817 cumTime= 0.00191036 edges= 6 schur= 0
    iteration= 4 chi2= 0.000000 time= 0.000425022 cumTime= 0.00233538 edges= 6 schur= 0
    iteration= 5 chi2= 0.000000 time= 0.000433779 cumTime= 0.00276916 edges= 6 schur= 0
    iteration= 6 chi2= 0.000000 time= 0.000422018 cumTime= 0.00319118 edges= 6 schur= 0
    iteration= 7 chi2= 0.000000 time= 0.000455279 cumTime= 0.00364646 edges= 6 schur= 0
    iteration= 8 chi2= 0.000000 time= 0.000484232 cumTime= 0.00413069 edges= 6 schur= 0
    iteration= 9 chi2= 0.000000 time= 0.000440495 cumTime= 0.00457119 edges= 6 schur= 0

    After optimization:
    T=
    -0.00 1.00 0.00 -0.00
    -1.00 -0.00 -0.00 -1.00
    -0.00 -0.00 1.00 0.00
    0.00 0.00 0.00 1.00

      输出内容被空行分为三部分:第1部分和第3部分是优化前后的变换矩阵,可以看出优化前位姿矩阵被初始化为单位矩阵和全零向量作为旋转矩阵和平移向量,优化后的位姿矩阵和第三章的结果一样。第2部分是优化日志,chi2chi2是误差的一种表示方法,可以看出第四次迭代后chi2chi2已经为0。

    展开全文
  • 针对传统遗传算法自身存在的早熟收敛、搜索空间小以及计算效率低的问题,在保证算法收敛和最大限度地搜索模型空间的基础上,对遗传算子采取相应策略进行了改进,并通过界约束以增加解的稳定性。...
  • 这是一个用于在两种非线性优化方法之间进行比较的项目:顺序二次规划法和半光滑牛顿法。 目录树 |-- SQP-vs.-Semismooth-Newton |-- tex |-- test |-- problem_... |-- ... |-- problem_... |-- extras |-- test-...
  • Tsai提出的基于RAC约束(Radial Alignment Constraint)的两步法[2]先利用线性变换方法求解摄像机参数,再以求得的参数作为初始值,考虑畸变因素,利用非线性优化方法进一步提高标定精度。
  • 而我们知道,SLAM问题是一个已知u和z,求相机每一步的姿态x和特征点空间坐标y的问题,所以从概率学的角度,在非线性优化中,我们把所有待估计的变量放在一个“状态变量”中: x = {x1, … , xN, y1,…,yM} 对...

    关键词:最小二乘法,增量方程

    状态估计问题的表述:
    在之前我们已经讲过了经典的SLAM数学模型,由运动方程和观测方程两者所组成。如下:

    1. xk = f(xk-1, uk) + wk
    2. zk,j = h(yj, xk) + vk,j

    方程1被称为运动方程,描述相机的运动,方程2被称为观测方程,描述特征点的观测模型。

    而在相机模型中,依据内参外参矩阵,有如下方程:
    Szk,j = K exp(&^)yj
    K为相机内参,exp(&^)为相机外参的李代数,yj为空间点在世界坐标系下的三维位置坐标,K对于相机为固定值(当然此时我们还没有考虑相关的畸变模型,在之后我们会考虑畸变模型构建一个更复杂的方程)

    此处假设w和v都是均值为0的高斯噪声:
    wk ~ N(0, Rk)
    vk,j ~ N(0,Qk,j)

    而我们知道,SLAM问题是一个已知u和z,求相机每一步的姿态x和特征点空间坐标y的问题,所以从概率学的角度,在非线性优化中,我们把所有待估计的变量放在一个“状态变量”中:
    x = {x1, … , xN, y1,…,yM}

    对机器人状态的估计,就是已知输入数据u和观测数据z的条件下,求计算状态x的条件概率分布: P(x|z,u)
    当没有测量运动的传感器,而只有图像时,比如ORB-SLAM,可以简化为:P(x|z)
    依据贝叶斯法则,我们有:
    P(x|z) = P(z|x)P(x)/P(z)
    我们不知道图片分布的概率,也不知道P(x),此时就没有了先验概率,因此我们可以求解x的最大似然估计(Maximize Likelihood Estination, MLE):
    x*MLE = arg max P(z|x)
    我们想要P(x|z)最大,即我们要求出在现有输入下,变量x最可能的取值,由贝叶斯公式,我们将其转换为了求P(z|x)最大值的问题,即变为了在什么情况的x下,取z的概率最大!

    [–当然这里面的假设太多了,我不太理解为啥这样做可以,即这样的省略正确–]

    由于观测方程:
    zk,j = h(yj, xk) + vk,j
    【当然,现在看来,从P(x|z)转化为P(z|x)是为了顺应之前的相机模型方程,由三维点投影到相片是可以计算的,但是从相片反投影回三维点,只能计算反投影到标定平面上的那个点,计算不出来在三维平面的那个点,这需要更复杂的计算。】
    h表示一系列的函数运算,v为高斯分布的变量,根据概率论的知识,有:
    vk,j ~ N(0,Qk,j)
    P(zj,k | xk, yj) = N(h(yj, xk), Qk,j)
    这里相当于xk,yj为常量了!
    这仍然是一个高斯分布,为了计算使其最大化的xk,yj,我们往往通过最小化负对数的方式来求解。

    高维高斯分布模型:
    x ~ N(u,∑),概率密度展开形式为:
    P(x) = exp(-1/2 (x-u)T ∑-1 (x - u))/sqrt(2pai)N * det(∑))
    取其负对数,去掉无关项,只保留未知项,得到:
    x* = arg min( (zk,j - h(xk, yj))T Qk,j-1 (zk,j - h(xk,yj)) )

    而这样其实就把可能相关的多元变量的最小二乘法的雏形表现出来了
    意义:最小化噪声项(误差)在∑范数下的平方
    因此,我们定义数据与估计值之间的误差:
    ev,k = xk - f(xk-1, vk)
    ey,j,k = zk,j - h(xk,yj)

    并求该误差的平方和,把两式的误差分别加起来(当然,这里也要涉及到我们很头疼的一个问题,那就是权重问题,两式是独立的,因此我们将其直接加起来,这一步是我最怀疑的,也是我感觉最没有依据的一步
    当然了,同一个式子里面的权值,有噪声为依据进行加权,但问题也来了,你如何知道每一步的噪声的分布呢?难道每次都去取点计算,逼近噪音吗?

    普遍的非线性最小二乘的优化问题:
    如: min(x) 1/2 ||f(x)||^2
    其中,自变量x属于 R^n,f是任意的非线性函数,我们设它有m纬,形成一个n -> m的空间投影,解决这样一个优化问题的步骤如下:

    1. 给定某个初始值x0
    2. 对于第k次迭代,寻找一个增量delta xk,使得 ||f(xk + delta xk)||^2达到极小值
    3. 若delta xk 足够小,则停止
    4. 否则,令 xk+1 = xk + delta xk,返回第二步

    (梯度下降法)

    多元函数的泰勒展开:
    只展示前两项:
    ||f(x + delta x)||^2 ~~ ||f(x)||^2 + J(x)delta x + 1/2 delta x T H delta x
    J 是 ||f(x)||^2对于x的一阶导数(雅可比矩阵),H是二阶导数(海塞[Hessian]矩阵)

    1. 如果只保留一阶梯度,那么这就是一条直线,我们视增量解为
      delta x* = -JT(x)
      意义就是沿着反向梯度方向前进即可(在我看来就类似于一条曲线的切线,沿着下降的方式前行,部分情况下还可以增加一个布长 lamda)
    2. 如果保留二阶梯度信息,使得导数为0,对泰勒展开对于delta x求解,那么增量的解为:
      H delta x = -JT
      但是该方法还要求解H(海塞)矩阵,这样计算量较大,计算多元函数的二阶导数,J(x)为一阶矩阵

    由于我们通常想要避免H矩阵的计算,所以我们在实际中有两种更为实用的方法:高斯牛顿法和列文伯格-马夸尔特方法。

    高斯牛顿法
    先讲f(x)进行一阶泰勒展开,然后再将其平方,从而用两个一次导表示出二次导数,简化计算量。
    f(x+delta x) = f(x) + J(x)delta x
    对 1/2||f(x + delta x)||^2 进行delta x求导之后,我们简化得到:
    J(x)TJ(x)delta x = -J(x)Tf(x)
    将左侧定义为H,右侧定义为g,那么上式变为:
    Hdelta x = g
    这样计算更简单

    列文伯格-马夸尔特方法
    在高斯牛顿法的基础上进行修改,给delta x加一个信赖区间(Trust Region):把增量解限定在一个椭圆的球里面
    最初始:
    min 1/2||f(xk) + J(xk)delta xk ||^2 , s.t. || Ddelta xk||^2<= u
    u是给定的信赖区域半径,会不断的更新

    但是在SLAM十四讲中这一块并不清楚,如何将定义域转化到无约束优化问题里面,以及如何将矩阵简化为I,都不清不楚!
    这里我们简要记为:
    (H + lamda I)delta x = g
    这里的H和g和高斯牛顿法是一样的。
    有助于解决H矩阵奇异值的问题。
    当lamda较小的时候,偏向于高斯法,当lamda比较大的时候,偏向于一阶梯度法。

    当然,如何求解delta x是个很重要的问题,直接对系数矩阵求逆是很大的计算量,多亏于系数矩阵的系数性质,在下一章节我们将会讲到。
    下一章节: BA优化

    展开全文
  • 例如:可以让相机少转一些角度,而把点多移动一些 在BA中希望有尽可能多的约束,因为多次观测会带来更多的信息,能够更准确地估计每个变量 通过几个g2o的例子可以看到,g2o在求解非线性优化问题时具有很明显的通用性...

    使用g2o求解ICP的步骤:

    1、定义顶点和边的类型

    2、提取ORB特征、匹配点对

    3、定义并设置求解器

    4、定义并设置顶点和边

    5、调用求解器的 initializeOptimization 函数进行优化

     

    使用g2o进行优化,不仅可以对位姿进行估计,还可以同时对空间点进行优化(将空间点也加入优化变量中)

    如果同时考虑点和相机,整个问题变得更自由,可能会得到其他的解。例如:可以让相机少转一些角度,而把点多移动一些

    在BA中希望有尽可能多的约束,因为多次观测会带来更多的信息,能够更准确地估计每个变量

     

    通过几个g2o的例子可以看到,g2o在求解非线性优化问题时具有很明显的通用性

    使用g2o求解问题的关键在于构建图优化:

    1、定义顶点,要明确优化变量是什么

    2、定义边,要明确误差是什么、雅可比的计算公式

    3、定义求解器,完成一些配置

     

    #include <iostream>
    using namespace std;
    
    #include <opencv2/core/core.hpp>
    #include <opencv2/features2d/features2d.hpp>
    #include <opencv2/highgui/highgui.hpp>
    #include <opencv2/calib3d/calib3d.hpp>
    using namespace cv;
    
    #include <Eigen/Core>
    #include <Eigen/SVD>
    #include <Eigen/Dense>
    
    #include <chrono>
    
    
    #include <sophus/se3.hpp>
    
    #include <g2o/core/base_vertex.h>
    #include <g2o/core/base_unary_edge.h>
    #include <g2o/core/sparse_optimizer.h>
    #include <g2o/core/block_solver.h>
    #include <g2o/core/solver.h>
    #include <g2o/core/optimization_algorithm_gauss_newton.h>
    #include <g2o/solvers/dense/linear_solver_dense.h>
    
    
    //提取ORB特征并匹配
    void findAndMatchOrbFeature(const Mat &img1, const Mat &img2, 
    	vector<KeyPoint> &kp1, vector<KeyPoint> &kp2, 
    	vector<DMatch> &matches)
    {	
    	Ptr<FeatureDetector> detector = ORB::create();
    	Ptr<DescriptorExtractor> desc = ORB::create();
    	Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    	
    	//检测角点
    	detector->detect(img1, kp1);
    	detector->detect(img2, kp2);
    	
    	//计算描述子
    	Mat desc1, desc2;	
    	desc->compute(img1, kp1, desc1);
    	desc->compute(img2, kp2, desc2);
    	
    	//匹配点对
    	vector<DMatch> allMatches;
    	matcher->match(desc1, desc2, allMatches);
    	
    	//检测误匹配
    	double minDist = 10000;
    	for(auto m : allMatches)
    		if(m.distance < minDist)	minDist = m.distance;
    	
    	double useDist = max(2*minDist, 30.0);
    	for(auto m : allMatches)	
    		if(m.distance <= useDist)
    			matches.push_back(m);	
    }
    
    //将像素坐标转化为归一化坐标
    Point2d pixelToNor(const Point2d &p, const Mat & k)
    {
    	//像素坐标 = k * 归一化坐标
    	//xp = fx * xn + cx
    	return Point2d((p.x - k.at<double>(0, 2)) / k.at<double>(0, 0), (p.y - k.at<double>(1, 2)) / k.at<double>(1, 1));
    }
    
    //顶点,模板参数:优化变量维度和数据类型
    class Vertex : public g2o::BaseVertex<6, Sophus::SE3d> {
    public:
    	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    	//重置
    	virtual void setToOriginImpl() override {	//override保留字表示当前函数重写了基类的虚函数
    		_estimate = Sophus::SE3d();
    	}
    
    	//更新
    	virtual void oplusImpl(const double *update) override {
    		Eigen::Matrix<double, 6, 1> delta_r;
    		delta_r << update[0], update[1], update[2], update[3], update[4], update[5];
    		_estimate = Sophus::SE3d::exp(delta_r) * _estimate;
    	}
    
    	//存盘和读盘:留空
    	virtual bool read(istream &in) {}
    
    	virtual bool write(ostream &out) const {}
    };
    
    //边, 模板参数:观测值维度,类型,连接顶点类型
    class Edge : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, Vertex> {
    public:
    	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    	Edge(const Eigen::Vector3d &p) : BaseUnaryEdge(), _p(p) {}
    
    	//计算误差
    	virtual void computeError() override 
    	{	
    		const Vertex *v = static_cast<const Vertex *> (_vertices[0]);	//static_cast将_vertices[0]的类型强制转换为const CurveFittingVertex *
    		const Sophus::SE3d T = v->estimate();
    		_error = _measurement - T*_p;
    	}
    
    	//计算雅可比矩阵
    	virtual void linearizeOplus() override 
    	{
    		const Vertex *v = static_cast<const Vertex *> (_vertices[0]);
    		const Sophus::SE3d T = v->estimate();
    		
    		// e = p - Tp'
    		// de/d李代数
    		
    		Eigen::Vector3d p = T*_p;
    		//<>指定子块大小,()指定子块起点
    		_jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix<double, 3, 3>::Identity();
    		_jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(p);	//hat返回向量对应的反对称矩阵
    	}
    
    	virtual bool read(istream &in) {}
    
    	virtual bool write(ostream &out) const {}
    
    private:
    	Eigen::Vector3d _p;
    };
    
    //用g20进行优化
    void poseEstimate3d3d(const vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &p1s3d, 
    	const vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &p2s3d, 
    	Mat &R, Mat &t)
    {
    	//构建图优化,先设定g2o
    	typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  //优化变量维度为6,误差值维度为3
    	typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
    
    	//梯度下降方法,可以从GN, LM, DogLeg 中选
    	auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    	g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    	g2o::SparseOptimizer optimizer;     // 图模型
    	optimizer.setAlgorithm(solver);   // 设置求解器
    	optimizer.setVerbose(true);       // 打开调试输出
    
    	//往图中增加顶点
    	Vertex *v = new Vertex();
    	v->setEstimate(Sophus::SE3d());
    	v->setId(0);
    	optimizer.addVertex(v);
    
    	//往图中增加边
    	for (int i = 0; i < (int)p1s3d.size(); i++) 
    	{
    		Edge *edge = new Edge(p1s3d[i]);
    		edge->setId(i);
    		edge->setVertex(0, v);                // 设置连接的顶点
    		edge->setMeasurement(p2s3d[i]);      // 观测数值
    		edge->setInformation(Eigen::Matrix<double, 3, 3>::Identity()); //信息矩阵
    		optimizer.addEdge(edge);
    	}
    
    	//执行优化
    	optimizer.initializeOptimization();
    	optimizer.optimize(10);
    
    	//以Mat格式重构T
    	Sophus::SE3d T = v->estimate();
    	Eigen::Matrix3d R_ = T.rotationMatrix();
    	Eigen::Vector3d t_ = T.translation();	
    	R = (Mat_<double>(3, 3) << 
    		R_(0, 0), R_(0, 1), R_(0, 2),
    		R_(1, 0), R_(1, 1), R_(1, 2),
    		R_(2, 0), R_(2, 1), R_(2, 2));
    	t = (Mat_<double>(3, 1) << 
    		t_(0, 0), t_(1, 0), t_(2, 0));
    }
    
    int main(int argc, char **argv)
    {
    	if(argc != 5)
    	{
    		cout << "error!" << endl;
    		return 0;
    	}
    
    	Mat img1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    	Mat img2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
    
    	//提取ORB特征、匹配点对
    	vector<KeyPoint> kp1, kp2;
    	vector<DMatch> matches;
    	findAndMatchOrbFeature(img1, img2, kp1, kp2, matches);
    	
    		
    	//读取深度图,获得3d点对,每个像素占据16字节(单通道)
    	Mat img3 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);
    	Mat img4 = imread(argv[4], CV_LOAD_IMAGE_UNCHANGED);	
    	Mat k = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    	vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> p1s3d, p2s3d;
    	for(auto m : matches)
    	{
    		Point2d p12d = kp1[m.queryIdx].pt;
    		unsigned short pixel1 = img3.ptr<unsigned short>((int)p12d.y)[(int)p12d.x];
    		Point2d p22d = kp2[m.trainIdx].pt;
    		unsigned short pixel2 = img4.ptr<unsigned short>((int)p22d.y)[(int)p22d.x];
    		
    		if(pixel1 == 0 || pixel2 == 0)	continue;
    		
    		double depth = pixel1 / 5000.0;		
    		Point2d pNor2d = pixelToNor(p12d, k);				
    		Eigen::Vector3d p3d(pNor2d.x*depth, pNor2d.y*depth, depth);
    		p1s3d.push_back(p3d);
    		
    		depth = pixel2 / 5000.0;
    		pNor2d = pixelToNor(p22d, k);		
    		p3d = Eigen::Vector3d(pNor2d.x*depth, pNor2d.y*depth, depth);
    		p2s3d.push_back(p3d);		
    	}
    	
    	//g2o求解ICP估计问题	
    	Mat R, t;
    	poseEstimate3d3d(p1s3d, p2s3d, R, t);
    	cout << "R: " << endl << R << endl << "t: " << t.t() << endl;
    	
    	//验证点对
    	for(int i = 0; i < (int)p1s3d.size(); i++)
    	{
    		Mat p1 = (Mat_<double>(3, 1) << p2s3d[i](0, 0), p2s3d[i](1, 0), p2s3d[i](2, 0));
    		Mat p2 = (Mat_<double>(3, 1) << p1s3d[i](0, 0), p1s3d[i](1, 0), p1s3d[i](2, 0));
    		
    		cout << i << "\t:" << p1.t() << " and " << (R*p2+t).t() << endl;
    	}
    	
    	
    	return 0;
    }
    

     

     

    展开全文
  • 非线性优化方法

    2013-10-24 16:24:04
    非线性优化方法,涵盖了高斯-牛顿法,L-M算法,trust region等非线性参数优化算法
  • matlab程序非线性优化设计方法时下流行的关于非线性规划的源程序
  • 基于梯度理论的非线性优化研究,张璐,,在生产实践中,非线性优化方法应用广泛,基于梯度搜索理论的共轭梯度法是一种寻找最优解的有效方法。我们将其应用到目前同样被广
  • 非线性优化计算方法 非线性优化计算方法 非线性优化计算方法
  • 最优化计算方法,本书主要描述最优化算法,以及非线性优化算法
  • 非线性优化算法总结

    2020-07-09 22:56:35
    非线性优化方法: 1、梯度下降法: 对目标函数进行一阶泰勒近似 2、牛顿法:对目标函数二阶泰勒近似,需要计算Hessain矩阵 3、高斯牛顿法:JTJJ^{T}JJTJ近似Hessain矩阵 4、列文伯格-马夸尔特(Lenverberg-Maquardt)...
  • 非线性优化

    2019-08-16 17:14:37
    非线性优化一般采用迭代的方法,认为估计问题是一个凸函数(初值很重要),通过迭代增量参数X,不断使残差平方和最小.直到,增量X小到一定程度(因为假设凸函数,凸函数在接近极值的地方,导数很小)时,或达到迭代...
  • matlab开发-使用gmarguardt方法进行多变量非线性优化。基于马尔夸特方法的多元非线性优化
  • 本文介绍了基本的无约束非线性优化方法,理解其在SLAM状态估计问题中的应用。
  • 这个问题在高翔博士的十四讲里有详细的阐述(P244),主要对比了EKF和非线性优化方法的差异,主要包括3个方面,这篇EKF算法与非线性优化算法的比较进行了概述 其中第二点提到,从k-1时刻到k时刻,EKF只在k-1时刻做了...
  • 非线性优化计算方法是计算数学与运筹学的交叉学科。 许多其他科学领域的问题也可以归结为非线性优化问题,如信息科学中的模式识别问题。 研究高效的求解非线性优化的计算方法具有重要的理论意义和实际价值。 ...
  • 非线性最小二乘优化方法总结

    千次阅读 2020-07-07 20:27:00
    什么是非线性优化 对于最优化问题,如果目标函数是非线性的,那就是非线性优化问题,如果目标函数是凸函数,那就是凸优化问题。 首先需要说明的是,最初的问题其实是一个最优化问题。所谓的最优化问题其实就是...
  • 非线性优化方法更倾向于使用所有的历史记录. EKF的非线性误差,EKF只在x_k-1处做了一次线性优化,根据这次线性化的效果直接计算出后验概率,也就是该点处的线性化近似在后验概率处仍然是有效的,而实际上,当离工作点较...
  • 毕 业 论 文 题 目 非线性优化计算方法与算法 学 院 数学科学学院 专 业 信息与计算科学 班 级 计算 1201 学 生 陶红 学 号 20120921104 指导教师 邢顺来 二一六年五月二十五日 摘 要 非线性规划 问题是一般形式的...
  • L-BFGS-B的代码,是一种基于梯度的非线性优化方法
  • 优化方法是一种数学方法,它是研究在给定约束之下如何寻求某些因素(的量),以使某一(或某些)指标达到最优的一些学科的总称。随着学习的深入,博主越来越发现最优化方法的重要性,学习和工作中遇到的大多问题都可以...
  • SLAM问题中,机器人自身的状态估计主要有两种方式: 基于滤波器的方法(Kalman filter, Particle filter等)和基于非线性优化的方法,目前主流的SLAM方案均使用了非线性优化方法 状态估计 经典SLAM模型主要由运动模型和...
  • 这些非线性优化方法都是建立在牛顿法基础上的一个方法。牛顿法可以求方程的根以及求目标函数的极小值(优化问题)。 引1:牛顿法迭代求解方程 首先我们引入牛顿法: 例: 迭代法求函数 y=3x−2 y=3^x-2 y=3x−2 的...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 3,698
精华内容 1,479
关键字:

非线性优化方法