复现—张正友

先上流程：

张正友求内参分为两步，首先是在理想状态下求（即不考虑畸变）仅用二维点与对应的三维点通过公式求解出相机内参。

第二步是利用所求内参求得变换矩阵，再将图一的点与变化矩阵做乘积，将图一的点模拟旋转到图二上。因为存在误差，图二识别出来的特征点与计算出来模拟特征点会存在坐标误差。对这些误差做最小二乘，所得的结果即为最终结果。

了解到一种新的循环判断方式: for(const auto &A: B){}

其效果是 A会依次复制B中的元素（B中有多个元素如：数组，vector向量）

循环内部可以运用A来进行数值运算等等，这样可以不用去了解B中具体的元素个数，当A遍历完B中所有的元素就会停止循环。

 具体的计算方程在论文中都写的十分详细，但是在具体实现时会有一些小问题，比如如果用OpenCV自带的SVD求解来求解类似AX=0(X中的值对应着单应矩阵H的值)的结果时所求得的结果需要进行归一化，这点在论文中并没有写出来。

3.31 

终于是结束复现 

大概写一写中间遇到的一些问题。

Ceres库使用的问题：

一、这种通过点的误差来优化参数的，最好是使用BA优化，可以一次性全部优化完。

二、需要优化的参数，需要通过指针传入，在重载“括号”<即（）>时 将这些指针 以const T *const XXX 的形式传入。并且这些指针会被转化为数组的形式，在接下来利用这些指针对应的数据进行运算时，需要以数组的形式进行引用。数组的维数（即对应了每一个循环传入的对应数据的个数）通俗来讲，我想优化位移矩阵，位移矩阵为3*3的矩阵 那么我就可以将指针const T *const T_matrix 定义为9维数组。注意，“定义为数组”这只是一种比喻 ，实际上还是要传入指针。当然在使用数据的时候需要用数组形式。

三、“指针对应的数组维数”是在ceres::AutoDiffCostFunction 中定义的 例如在我写的代码中< ComputeKError , 2, 4,9,2> 第一个数指的是残差数量，后面三个对应的是传入的指针对应“数组”维数。

bool operator()( const T *const K_compute , const T *const camera , const T *Aberration,
                  T *residuals) 

例如我是这么重载的括号，那么第二个数对应的是K_compute 指针的数组维数为4，以此类推。

 bool operator()( const T *const K_compute,const T *const camera ,const T *Aberration,
                  T *residuals) const {      //K为内参,camera 存外参,Aberration 为畸变参数
                  // 重载操作符（）
                 
         const T &alpha = K_compute[0];
         const T &beta = K_compute[1];
         const T &u0 = K_compute[2];
         const T &v0 = K_compute[3];
         const T &k1 = Aberration[0];
          const T &k2 = Aberration[1];
        Eigen::Matrix<T,3,3> K;
        Eigen::Matrix<T,3,3> com_T;
        Eigen::Matrix<T,3,1> P1;
        Eigen::Matrix<T,3,1> P2;
        T  x=static_cast <T>(observed_x_3d);
        T  y=static_cast <T>(observed_y_3d);
        
        T r2 =x*x+y*y;
  
        P2<<
                    static_cast <T>(observed_x_3d),
                    static_cast <T>(observed_y_3d),
                   //P2_x,
                   //P2_y,
                    T(1);
       
        K<<
                alpha,T(0),u0,
                T(0),beta,v0,
                T(0),T(0),T(1);
        
        com_T<<
                  camera[0],camera[1],camera[2],
                  camera[3],camera[4],camera[5],
                  camera[6],camera[7],camera[8];
            
        //  P1=K*com_T*P2;
          P1=com_T*P2;
          T not_distortion_x=P1(0,0)/P1(2,0);
          T not_distortion_y=P1(1,0)/P1(2,0);
          T P2_x = not_distortion_x + not_distortion_x*(k1 * r2 + k2 * r2 * r2);
          T P2_y = not_distortion_y+ not_distortion_y*(k1 * r2 + k2 * r2 * r2);
          
          T distortion_x=u0+alpha*P2_x;
          T distortion_y= v0+beta*P2_y;
        
          residuals[0]=distortion_x - static_cast <T>(observed_x_2d);
          residuals[1]=distortion_y - static_cast <T>(observed_y_2d);

 通过代码段来理解指针对应的数组。

四、误差来源

1.首先是图像畸变，如果不考虑相机的拍照畸变会导致最终的优化结果很差。

2.旋转矩阵R,如果使用SVD分解得到的R会产生误差，还需要进行一步变换。

3.在进行最小二乘优化的时候，传入的参数是三维点坐标与对应的二维点像素坐标。其中二维点像素坐标作为orbserve_2d，即观测点。三维点坐标是可以用来和旋转矩阵，内参相乘来得到二维像素点的估计值，但是这种估计方法是存在很大的误差的，具体的原因可能是三维点本来是理想状态下的坐标，所以会导致误差。

解决的方法是通过变换矩阵T的第三列（本应是第四列，但是由于三维点的Z=0,所以T的第三列也即旋转矩阵R的第三列没有实际意义，所以将R的第三列去除）位移向量t来表示X坐标（因为第一点为原点，所以位移值也就是横纵坐标），通过公式u=alpha*X+u0 ;v=beta*Y+v0; 来计算二维像素坐标的估计值。注意到，在算式中我们需要得到相机的焦距f，然后令alpha*f=fa ; beta*f=fb。然而在本实验中我们不考虑焦距f，即fa=alpha ,fb=beta（alpha与beta都是通过张正友论文中提供的公式计算得知的）。