深蓝学院-多传感器融合定位第五章作业

**

1.不需要转台标定方法推导加速度计对应残差对加速度内参的雅克比

2.使用自动求导完成标定

3.使用解析式求导完成标定

**
1.雅克比推导

2.使用自动求导完成标定
修改三处TODO

CalibratedTriad_<_T2> calib_triad( 
  //
  // TODO: implement lower triad model here
  //
  // mis_yz, mis_zy, mis_zx:
  _T2(0), _T2(0), _T2(0),
  // mis_xz, mis_xy, mis_yx:
  params[0], params[1], params[2],
  //    s_x,    s_y,    s_z:
  params[3], params[4], params[5], 
  //    b_x,    b_y,    b_z: 
  params[6], params[7], params[8] 
);


// TODO: implement lower triad model here
//
acc_calib_params[0] = init_acc_calib_.misXZ();
acc_calib_params[1] = init_acc_calib_.misXY();
acc_calib_params[2] = init_acc_calib_.misYX();


acc_calib_ = CalibratedTriad_<_T>( 
//
// TODO: implement lower triad model here
// 
0,0,0,
min_cost_calib_params[0],
min_cost_calib_params[1],
min_cost_calib_params[2],
min_cost_calib_params[3],
min_cost_calib_params[4],
min_cost_calib_params[5],
min_cost_calib_params[6],
min_cost_calib_params[7],
min_cost_calib_params[8] 
);

标定结果

3.解析求导
写一个新的MultiPosAccResidual 类

template<typename _T1>
class MultiPosAccResidual  : public ceres::SizedCostFunction<1,9>
{
     
  public:
    MultiPosAccResidual(const _T1 &g_mag, const Eigen::Matrix< _T1, 3 , 1> &sample 
    ) : g_mag_(g_mag), sample_(sample){
     }
   
   virtual ~MultiPosAccResidual() {
     }
   virtual bool Evaluate(double const* const* parameters,double* residuals,double** jacobians) const 
	{
     
    Eigen::Matrix<double, 3, 1> raw_samp( 
      double(sample_(0)), 
      double(sample_(1)), 
      double(sample_(2)) 
    );
    /* Apply undistortion transform to accel measurements
         mis_mat_ <<  _T(1)   , -mis_yz  ,  mis_zy  ,
                       mis_xz ,  _T(1)   , -mis_zx  ,  
                      -mis_xy ,  mis_yx  ,  _T(1)   ; */
    CalibratedTriad_<double> calib_triad( 
      //
      // TODO: implement lower triad model here
      //
      // mis_yz, mis_zy, mis_zx:
      double(0), double(0), double(0),
      // mis_xz, mis_xy, mis_yx:
      parameters[0][0], parameters[0][1], parameters[0][2],
      //    s_x,    s_y,    s_z:
      parameters[0][3], parameters[0][4], parameters[0][5], 
      //    b_x,    b_y,    b_z: 
      parameters[0][6], parameters[0][7], parameters[0][8] 
    );
    
    // apply undistortion transform:
    Eigen::Matrix< double, 3 , 1> calib_samp = calib_triad.unbiasNormalize( raw_samp );
    
    residuals[0] = g_mag_*g_mag_ - calib_samp.squaredNorm();
    // residuals[0] = g_mag_- calib_samp.norm();

		if(jacobians != NULL)
		{
     
			if(jacobians[0] != NULL)
			{
      
        double S1 = parameters[0][0];
        double S2 = parameters[0][1];
        double S3 = parameters[0][2];

        double K1 = parameters[0][3];
        double K2 = parameters[0][4];
        double K3 = parameters[0][5];

        double b1 = parameters[0][6];
        double b2 = parameters[0][7];
        double b3 = parameters[0][8];

        double A1 = sample_(0);
        double A2 = sample_(1);
        double A3 = sample_(2);
        
        // 向量对向量的解析版：
        Eigen::MatrixXd a(3, 1);
        
         a << K1*(A1-b1), -S1*K1*(A1-b1)+K2*(A2-b2), -S2*K1*(A1-b1)-S3*K2*(A2-b2)+K3*(A3-b3);
        Eigen::MatrixXd da_dTheta(3, 9);
        da_dTheta << 0, 0, 0, A1-b1, 0, 0, -K1, 0, 0, 
                    -K1*(A1-b1), 0, 0, -S1*(A1-b1), A2-b2, 0, S1*K1, -K2, 0,
                    0, -K1*(A1-b1), -K2*(A2-b2), -S2*(A1-b1), -S3*(A2-b2), A3-b3, S2*K1, S3*K2, -K3;

        Eigen::Map<Eigen::Matrix<double, 1, 9, Eigen::RowMajor> > Jacob(jacobians[0]);
        Jacob.setZero();

        Jacob = - 2 * calib_samp.transpose() * da_dTheta;
                  
			}
		}

		return true;
	}

static ceres::CostFunction* Create ( const _T1 &g_mag, const Eigen::Matrix< _T1, 3 , 1> &sample )
  {
     
    return ( new MultiPosAccResidual<_T1>( g_mag, sample ) );
  }
protected:
	const _T1 g_mag_;
  const Eigen::Matrix< _T1, 3 , 1> sample_;
};

修改第二处

 inline _T misYZ() const {
      return -mis_mat_(0,1); };
  inline _T misZY() const {
      return -mis_mat_(0,2); }; //加-
  inline _T misZX() const {
      return -mis_mat_(1,2); };
  inline _T misXZ() const {
      return -mis_mat_(1,0); };//加-
  inline _T misXY() const {
      return -mis_mat_(2,0); };
  inline _T misYX() const {
      return -mis_mat_(2,1); };//加-

  inline _T scaleX() const {
      return scale_mat_(0,0); };
  inline _T scaleY() const {
      return scale_mat_(1,1); };
  inline _T scaleZ() const {
      return scale_mat_(2,2); };
      
  inline _T biasX() const {
      return bias_vec_(0); };
  inline _T biasY() const {
      return bias_vec_(1); };
  inline _T biasZ() const {
      return bias_vec_(2); };

修改第三处

  mis_mat_ <<  _T(1)   , -mis_yz  ,  -mis_zy  ,
                -mis_xz ,  _T(1)   , -mis_zx  ,  
               -mis_xy , -mis_yx  ,  _T(1)   ;          

标定结果