常识-eigen-opencv-ceres

最新推荐文章于 2024-01-04 09:41:21 发布

机械不学习

最新推荐文章于 2024-01-04 09:41:21 发布

阅读量378

点赞数

分类专栏： opencv 基础知识文章标签： opencv c++ 数学建模

本文链接：https://blog.csdn.net/lqql2012/article/details/108150388

版权

基础知识同时被 2 个专栏收录

2 篇文章 0 订阅

订阅专栏

opencv

1 篇文章 0 订阅

订阅专栏

1.0 Eigen 常识

1.1.1 储存方式

eigen Matrix创建的矩阵默认是按列存储，Eigen在处理按列存储的矩阵时会更加高效，如使用 Eigen::RowMajor 强制储存方式为行方式，但这里影响的主要是储存方式，对下标、矩阵运算并无影响;

在 slam 系统中 RowMajor 常用于指针初始化 jacobian 矩阵

Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor> > J_se3(jacobians[0]);

注意：下面的的测试代码中的两种初始化方式，以及使用指针 RowMajor 方式和默认方式初始化是相同的


	Matrix3d eigMat << 
        1, 2, 3,
        4, 5, 6,
        7, 8, 9;
	cout << "init eigMat = \n";    cout << eigMat << endl;

	double* eigMatptr = eigMat.data();
	cout << "array = [ "; for (int i = 0; i < 9; ++i) cout << eigMatptr[i] << " "; cout << "]" << endl;

	cout << "eigen matrix is "<<eigMat(0,0)<<" "<<eigMat(0,1)<<" "<<eigMat(0,2)<< endl<< endl;




	double dKdppx_[9] = {1.0, 2.0, 3.0,
                      	     4.0, 5.0, 6.0,
                             7.0, 8.0, 9.0};
	Eigen::Matrix<double, 3, 3, Eigen::RowMajor> eigMatRowTest(dKdppx_);
	Eigen::Matrix<double, 3, 3, Eigen::ColMajor> eigMatColTest(dKdppx_);

	cout << "init eigMatRowTest = \n";    cout << eigMatRowTest << endl;
	cout << "init eigMatColTest = \n";    cout << eigMatColTest << endl;

	double* eigMatptrRowTest = eigMatRowTest.data();
	double* eigMatptrColTest = eigMatColTest.data();

	cout << "eigMatRowTest array = [ "; for (int i = 0; i < 9; ++i) cout << eigMatptrRowTest[i] << " "; cout << "]" << endl;
	cout << "eigMatptrRow array = [ "; for (int i = 0; i < 9; ++i) cout << eigMatptrColTest[i] << " "; cout << "]" << endl;


	cout << "eigMatRowTest is "<<eigMatRowTest(0,0)<<" "<<eigMatRowTest(0,1)<<" "<<eigMatRowTest(0,2)<< endl;
	cout << "eigMatColTest is "<<eigMatColTest(0,0)<<" "<<eigMatColTest(0,1)<<" "<<eigMatColTest(0,2)<< endl;

输出：

init eigMat = 
1 2 3
4 5 6
7 8 9
array = [ 1 4 7 2 5 8 3 6 9 ]
eigen matrix is 1 2 3

init eigMatRowTest = 
1 2 3
4 5 6
7 8 9
init eigMatColTest = 
1 4 7
2 5 8
3 6 9
eigMatRowTest array = [ 1 2 3 4 5 6 7 8 9 ]
eigMatptrRow array = [ 1 2 3 4 5 6 7 8 9 ]
eigMatRowTest is 1 2 3
eigMatColTest is 1 4 7

1.1 opencv 常识

1.1.1 矩阵操作

1）python 中 im.shape 为（高 y，宽 x，通道数），分别对应width ，height

python转c++代码时用的上

2）使用 Mat(3, 1, CV_32F, test).clone(); 方式构建的Mat，是垂直保存的，在做矩阵和向量的乘法时，就不用转置了

float test[] = { 1, 2, 3 };
float test22[] = { 2, 4, 6 };

Mat testMat = Mat(3, 1, CV_32F, test).clone();
Mat testMat22 = Mat(3, 1, CV_32F, test22).clone();
Vec3f testV(1,2,3);

cv::Mat c = testMat.mul(testV);
cout << testMat22 << endl;
cout << testV << endl;
cout << homo << endl;

3) opencv 作为矩阵来操作一定要注意类型的转换不然打印时和操作时会出现很多莫名其妙的问题

以下代码也可以直接使用double 对应 CV_64F 会省不少力气

void PrintMat(Mat A)
{
    for(int i=0;i<A.rows;i++)
    {
        for(int j=0;j<A.cols;j++)
        {
            cout<<A.at<float>(i,j)<<' ';
        }
        cout<<endl;
    }
    cout<<endl;
    cout<<endl;

}    
    

void test()
{

    float tmp[3][3] = {{9.36395260e-01 , 4.31284837e-03 , 6.40885442e+02},
                   {-4.17751778e-02 , 1.00906647e+00 ,-2.21342885e+01},
                   {-3.04980078e-05 , 1.27734868e-05,  1.00000000e+00},};
    Mat homogra = Mat(3, 3, CV_32F, tmp).clone();
    PrintMat(homogra);

    Mat homo = homogra/homogra.at<float>(2,2);
    homo = homo.inv();
    homo.convertTo(homo,CV_32F);
    PrintMat(homo);

    float base_p1[] = { 0.0, 0.0, 1.0 };
    float base_p2[] = { (float) x, 0, 1 };
    float base_p3[] = { 0, (float) y, 1 };
    float base_p4[] = { (float) x, (float) y, 1 };

    Mat hp = homo*Mat(3, 1, CV_32F, base_p1);
    PrintMat(hp);

    hp = hp/homogra.at<float>(0,2);

    PrintMat(hp);
}

1.1.2 矩阵求导

1）矩阵的逆求导

1.2 ceres

1.2.1 ceres 基础参数

AutoDiffCostFunction的模板参数顺序：代价函数、残差块中残差项的数量、各参数块中参数的数量

（注意参数代表优化参数的数量，如 new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3 >分别对应相机参数和3d点）

自定义代价函数(cost function)的参数顺序：参数块、残差块、雅克比(如果有的话)

template <typename CostFunctor,
          int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.
          int N0,       // Number of parameters in block 0.
          int N1 = 0,   // Number of parameters in block 1.
          int N2 = 0,   // Number of parameters in block 2.
          int N3 = 0,   // Number of parameters in block 3.
          int N4 = 0,   // Number of parameters in block 4.
          int N5 = 0,   // Number of parameters in block 5.
          int N6 = 0,   // Number of parameters in block 6.
          int N7 = 0,   // Number of parameters in block 7.
          int N8 = 0,   // Number of parameters in block 8.
          int N9 = 0>   // Number of parameters in block 9.

class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals,
                                                      N0, N1, N2, N3, N4,
                                                      N5, N6, N7, N8, N9>
{
...
}

1.2.2 ceres的常见用法总结

costfunction 添加观测变量，在定义事指定观测变量和优化变量的维度

AddParameterBlock 添加 costfunction 和优化变量，其中雅可比矩阵以行优先的方式储存，x方向为优化变量的维度，y方向是观测变量的维度

ceres 使用自动求导时 AutoDiffCostFunction 时，通常在 struct 中重载() ，在 operator() 中计算loss；也可以在class 中重载 Evaluate() 函数，然后计算雅可比矩阵

ceres的使用过程基本可以总结为:
1、创建优化问题与损失核函数。

ceres::Problem problem;
ceres::LossFunction *loss_function;                           // 损失核函数 
//loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0);                   // 柯西核函数

2、再创建的problem中添加优化问题的参数 problem.AddParameterBlock()

void AddParameterBlock(double* values, int size);
void AddParameterBlock(double* values,
                         int size,
                         LocalParameterization* local_parameterization);

区别是若添加的参数更新方式需要另外定义，如四元数，则需要用ceres::LocalParameterization对象重新定义该参数更新的广义加法并作为参数传入AddParameterBlock()。
例如 VINS中的POSE，首先对Pose重新参数化-
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
然后再添加
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);

bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
    Eigen::Map<const Eigen::Vector3d> _p(x);
    Eigen::Map<const Eigen::Quaterniond> _q(x + 3);

    Eigen::Map<const Eigen::Vector3d> dp(delta);

    Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));

    Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
    Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);

    p = _p + dp;
    q = (_q * dq).normalized();

    return true;
}
bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const
{
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
    j.topRows<6>().setIdentity();
    j.bottomRows<1>().setZero();

    return true;
}

3、添加残差块
一个优化问题可以看成通过调整参数将一大堆各种各样的残差降到最小，因此，残差的提供是至关重要的，一个残差的构建离不开残差的数学定义以及关联的参数，ceres添加残差块通过 AddResidualBlock()完成，有两个重载貌似最为常用

template <typename... Ts>
  ResidualBlockId AddResidualBlock(CostFunction* cost_function,
                                   LossFunction* loss_function,
                                   double* x0,
                                   Ts*... xs)
ResidualBlockId AddResidualBlock(
      CostFunction* cost_function,
      LossFunction* loss_function,
      const std::vector<double*>& parameter_blocks);

也就是需要提供三种参数 —— cost_function对象、鲁棒核函数对象、该残差的关联参数。
其中重点是cost_function对象的给定，它有两种常见的提供方式：
(1)、创建一个派生于CostFunction的Factor对象, 如 IMUFactor，然后需要自己实现Evaluate()函数，直接完成jacobian的运算，参考VINS代码。
(2)、采用ceres::AutoDiffCostFunction对象，即自动数值求导对象，AutoDiffCostFunction对象也是CostFunction的子类, 自定义一个Factor对象，并且重载operator()，在其中完成残差的计算，然后将该Factor对象作为参数即可构造ceres::AutoDiffCostFunction对象。这样的好处是不需要自己计算jacobian，但是效率可能会低点，下面是实例-
创建 ceres::AutoDiffCostFunction对象

// 创建 ceres::AutoDiffCostFunction 对象并返回  
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,const Eigen::Vector3d last_point_b_, const double s_)
{   
    // 自动求导     残差维度 3   优化变量维度  7 
    return (new ceres::AutoDiffCostFunction<LidarEdgeFactor, 3, 4, 3>(new         
               LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
}

4、设置优化参数，ceres::Solve求解即可。

问题：

1）AddParameterBlock中的 parameter_blocks 添加参数和 AddResidualBlock 添加参数的区别？

AddResidualBlock后面的参数是待优化变量

Problem::AddParameterBlock，有两个重载，有一个会进行参数化。添加一个适当的大小参数块，并对问题进行参数化。重复调用相同的参数会被忽略。重复调用相同的double指针，但是不同的大小会导致未定义的行为，重新定义该参数更新的广义加法

而 AddResidualBlock 传入的参数将用于构造 jacobians 矩阵，即隐性的关联观测量计算的 error 和某些待优化变量

2) jacobians[ ] 对应关系和维度如何确定？

如下面的 jacobians[0]、jacobians[1]对应什么？

virtual bool Evaluate(double const* const* parameters,
                          double* residuals,
                          double** jacobians) const;


template<>
bool ReprojectionErrorSE3XYZ<6>::Evaluate(const double * const *parameters, double *residuals, double **jacobians) const
{
    Eigen::Quaterniond quaterd = toQuaterniond(Eigen::Map<const Vector3d>(parameters[0]));
    Eigen::Map<const Eigen::Vector3d> trans(parameters[0] + 3);
    Eigen::Map<const Eigen::Vector3d> point(parameters[1]);

    Eigen::Vector3d p = quaterd * point + trans;

    double f_by_z = f / p[2];
    residuals[0] = f_by_z * p[0] + cx - _observation_x;
    residuals[1] = f_by_z * p[1] + cy - _observation_y;

    Eigen::Matrix<double, 2, 3, Eigen::RowMajor> J_cam;
    double f_by_zz = f_by_z / p[2];
    J_cam << f_by_z, 0, - f_by_zz * p[0],
            0, f_by_z, - f_by_zz * p[1];

    if(jacobians != NULL)
    {
        if(jacobians[0] != NULL)
        {
            Eigen::Map<Eigen::Matrix<double, 2, 6, Eigen::RowMajor> > J_se3(jacobians[0]);
            J_se3.block<2,3>(0,0) = - J_cam * skew(p);
            J_se3.block<2,3>(0,3) = J_cam;
        }
        if(jacobians[1] != NULL)
        {
            Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor> > J_point(jacobians[1]);
            J_point = J_cam * quaterd.toRotationMatrix();
        }
    }

    return true;
}

计算残差和Jacobain矩阵

consider a function f(x1,...,xk) that depends on parameter blocks [x1,...,xk].
parameters 参数是指向数组的指针数组，其中包含各种参数块。参数的元素数量与CostFunction::parameter_block_sizes_相同，参数块的顺序与 CostFunction::parameter_block_sizes_相同。
residuals 是num_residuals_大小的数组。
residuals 是costFunction::parameter_block_sizes_大小的数组，包含指向每个参数块对应的Jacobain矩阵的指针。Jacobain矩阵与CostFunction::parameter_block_sizes_的顺序是相同的。，每个jacobians矩阵都以行优先顺序存储，例如：

jacobians[i]是一个数组包含 num_residuals_ x parameter_block_sizes_ [i]元素

jacobians[i][r * parameter_block_size_[i] + c] =

jacobians 的矩阵的 row数由残差维度num_residuals_决定，cols数由 parameter_block_sizes_[i] 决定，即i * 参数块中的变量个数，综上所述 ceres 中 jacobians 矩阵不需要人为的去初始化;

如果jacobians 是 NULL，就不返回导数，当只计算cost时才会这样。如果jacobians[i]是空的，那么jacobians矩阵对应的第i个参数块就不能返回，这就是当一个参数块被标记为常量时的情况

参考：ceres的常见用法总结

1.2.3 GlobalSize 和 LocalSize

对于四元数或者旋转矩阵这种使用过参数化表示旋转的方式，它们是不支持广义的加法（因为使用普通的加法就会打破其 constraint，比如旋转矩阵加旋转矩阵得到的就不再是旋转矩阵）所以我们在使用ceres对其进行迭代更新的时候就需要自定义其更新方式了，具体的做法是实现一个参数本地化的子类，需要继承于LocalParameterization，LocalParameterization是纯虚类，所以我们继承的时候要把所有的纯虚函数都实现一遍才能使用该类生成对象.

这里以四元数为例子，解释如何实现参数本地化，需要注意的是，QuaternionParameterization中表示四元数中四个量在内存中的存储顺序是[w, x, y, z]，而Eigen内部四元数在内存中的存储顺序是[x, y, z, w]，但是其构造顺序是[w, x, y, z]（不要被这个假象给迷惑），所以就要使用另一种参数本地化类，即EigenQuaternionParameterization，下面就以QuaternionParameterization为例子说明，如下：

class CERES_EXPORT QuaternionParameterization : public LocalParameterization {
 public:
  virtual ~QuaternionParameterization() {}
  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const;
  virtual bool ComputeJacobian(const double* x,
                               double* jacobian) const;
  virtual int GlobalSize() const { return 4; }
  virtual int LocalSize() const { return 3; }
};

GlobalSize 就是表示他真正的维数是一个4维的,表示参数 x 的自由度（可能有冗余），比如四元数的自由度是4，旋转矩阵的自由度是9

LocalSize表示 Δx 所在的正切空间（tangent space）的自由度,是告诉Ceres他表示的东西是一个几维的，然后他定义了一个“Plus”函数

如SO3 空间是属于非欧式空间的，但是没关系，我们只要保证旋转的局部是欧式空间，就可以使用流形进行优化了. 比如用四元数表示的3D旋转是属于非欧式空间的，那么我们取四元数的向量部分作为优化过程中的微小增量（因为是小量，所以不存在奇异性）

1.2.4 为什么 LocalParameterization 的 ComputeJacobian 由单位矩阵和零构成？

这个其实是实现上的一个trick。前面说过优化中需要计算两个雅克比的乘积来作为最终的雅克比矩阵。这个比较低效。既然我们最终需要的是关于扰动的雅克比一个 trick 是在CostFunction的雅克比中直接计算出，它的左上角设置为关于扰动的雅克比，其它部分为0。LocalParameterization中的雅克比左上角设置为单位矩阵𝐼，其它部分为0。这样两者的乘积就是关于扰动的雅克比。写成公式：

VINS-MONO ProjectionFactor代码分析及公式推导

1.2.5 边缘化次序　ParameterBlockOrdering　

ParameterBlockOrdering　设置那些优化变量在求解增量方程时优先被边缘化，一般会将较多的地图点先边缘化，不设置ceres会自动决定边缘化次序，这在SLAM里面常用于指定Sliding Window的范围．

在线性求解器中消除变量的顺序对方法的效率和准确性有很大的影响。例如，当做稀疏的Cholesky 分解时，有一个矩阵，一个好的排序将会给出一个带有O(n)的存储，一个坏的排序将导致一个完全密集的因子

有了这样的排序，Ceres确保最低编号的消除组的参数块首先被消除，然后消除下一个最低编号的消除组，依次下去。在每个消除组中，Ceres可以自由地按照它所选择的参数块顺序来排序。例如，线性系统

x+y=3

2x+3y=7

有两种方法可以解决这个问题。首先从两个方程中消去x，然后再用x代替解出y，或者先消除y，解出x，然后再用y替换。用户可以在这里构造三个排序。

{0:x},{1:y}{0:x},{1:y}:先求解x
{0:y},{1:x}{0:y},{1:x}:先求解y
{0:x,y}{0:x,y}:求解器决定消除顺序

参考：Ceres-Solver学习笔记(6)

1.2.5 协方差估计

鉴别最小二乘问题返回值好坏的一个方法是分析解的协方差，考虑非线性回归问题如下：

观察值 y是一个对变量x的具有单位协方差的随机非线性函数，然后，对于观测y，x的最大似然估计是非线性最小二乘问题:

x* 的协方差如下

如果J(x*)是列非满秩的。那么协方差矩阵C(x*)也是非满秩的，由 Moore-Penrose pseudo inverse给出。

注意上面我们假设y的协方差矩阵是单位矩阵。这是一个很重要的假设。如果不符合这个条件，那么

Ceres-Solver学习笔记(9)

机械不学习

关注

0
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
常识-eigen-opencv-ceres

11）python 中 im.shape 为（高 y，宽 x，通道数），分别对应width ，heightpython转c++代码时用的上2）使用Mat(3, 1, CV_32F, test).clone(); 方式构建的Mat，是垂直保存的，在做矩阵和向量的乘法时，就不用转置了float test[] = { 1, 2, 3 };float test22[] = { 2, 4, 6 };Mat testMat = Mat(3, 1, CV_32F, test)....
复制链接

扫一扫