joint_calib

[fengguibinyn]

` struct ReprojectionError { ReprojectionError(const Eigen::Vector2d &imgpts, const Eigen::Vector2d &boardpts) : _img_pts(imgpts), _board_pts(boardpts) {}

template <typename T>
bool operator()(const T *const instrinsics_, const T *const k_,
                const T *const rt_, // 6 : angle axis and translation
                T *residuls) const {
  //    Eigen::Vector3d hom_w(_board_pts(0), _board_pts(1), T(1.));
  T hom_w_t[3];
  hom_w_t[0] = T(_board_pts(0));
  hom_w_t[1] = T(_board_pts(1));
  hom_w_t[2] = T(1.);
  T hom_w_trans[3];
  ceres::AngleAxisRotatePoint(rt_, hom_w_t, hom_w_trans);
  hom_w_trans[0] += rt_[3];
  hom_w_trans[1] += rt_[4];
  hom_w_trans[2] += rt_[5];

  T c_x = hom_w_trans[0] / hom_w_trans[2];
  T c_y = hom_w_trans[1] / hom_w_trans[2];
  // T xd = c_x;
  // T yd = c_y;
  // distortion

  T r2 = c_x * c_x + c_y * c_y;
  T r4 = r2 * r2;
  T r_coeff = (T(1) + k_[0] * r2 + k_[1] * r4);
  T xd = c_x * r_coeff;
  T yd = c_y * r_coeff;`

这里去畸变是不是有问题？棋盘格角点是有畸变，棋盘格世界做标映射到图像上应该就可以了，为什么还要去畸变