手眼标定
机器人手眼标定Ax=xB(eye to hand和eye in hand)及平面九点法标定
Ax = xB问题求解,旋转和平移分步求解法
手眼标定AX=XB求解方法(文献总结)
基于靶的方法
- 相机标定
(1) ApriTag
(2) 棋盘格:cv::findChessboardCorners
(3) 品字格 - 激光雷达标定
(1) 激光雷达反射板
动态标定
这里面的动态标定指车辆行使时广角前视相机相对路面的位姿估计
1.基于路面稀疏光流的Homography标定
前提:利用H进行优化,特征点需要位于同一个平面上,例如都是路面点的采样
首先利用opencv::calcOpticalFlowPyrLK
提取出连续两帧之间的稀疏光流
假设相机在两个不同位置拍摄同一个平面(路面)
frame1中的点可以由以下公式转换到frame2中
X1表示三维坐标点,在平面P上,因此X1沿着平面法向量n的投影距离为d
进一步推导
结合公式1我们可以得到
因此我们就得到了平面的单应性矩阵
因此x2= Hx1,其中x1和x2分别是上一帧和当前帧相机坐标系下的点(x,y,1),我们可以根据x2 - Hx1构建重投影误差来优化H
那么如何由H分解出来相机与车辆的标定矩阵呢?
H是相机之间的变化Rt_cam求得的(本质上优化H也就是优化Rt_cam),然后根据车辆坐标系的移动Rt_vcs,可以计算出相机到车辆的标定Rt,如下所示
括号里面可以理解为第一帧车辆系下两个相机之间的平移,前面乘以Rcv是把车辆坐标系转到相机坐标系
//前提:优化的变量直接是rpyxyz_v_c,可以直接分解出来rpy
ceres::CostFunction *cost_function = HomographyReprojectionError::Create(curr_pt, last_pt, R_vv, t_vv, K);
problem.AddResidualBlock(cost_function, loss_func, rpy_v_c.data(), xyz_v_c.data());
class HomographyReprojectionError {
public:
HomographyReprojectionError(const cv::Point2f &p1, const cv::Point2f &p2, const Eigen::Matrix3d &R_vv, const Eigen::Vector3d &t_vv, const Eigen::Matrix3d &K)
: x1_(p1.x), y1_(p1.y), x2_(p2.x), y2_(p2.y), R_vv_(R), t_vv_(t_vv), K_(K){}
static ceres::CostFunction *Create(const cv::Point2f &p1, const cv::Point2f &p2, const Eigen::Matrix3d &R_vv, const Eigen::Vector3d &t_vv, const Eigen::Matrix3d &K)
{
return (new ceres::AutoDiffCostFunction<HomographyReprojectionError, 2, 6>(new HomographyReprojectionError(p1, p2, R_vv, t_vv, K)));
}
template <typename T>
bool operator()(const T *const rpy_v_c, const T *const xyz_v_c, T *residuals) const {
// get extrinsics
Eigen::Matrix<T, 3, 1> rpy_data(rpy_v_c[0], rpy_v_c[1], rpy_v_c[2]);
Eigen::Matrix<T, 3, 3, Eigen::RowMajor> R_v_c = Intr_XYZ2R(rpy_data);
Eigen::Matrix<T, 3, 1> t_v_c = t_v_c_.template cast<T>();
Eigen::Matrix<T, 3, 1> xyz_data(xyz_v_c[0], xyz_v_c[1], xyz_v_c[2]);
t_v_c += xyz_data;
// camera relative pose
Eigen::Matrix<T, 3, 3, Eigen::RowMajor> R_cc = R_v_c.transpose() * R_vv * R_v_c;
Eigen::Matrix<T, 3, 1> t_cc = R_v_c.transpose() * (R_vv * t_v_c + t_vv - t_v_c);
// homography matrix
T d = T(-1.0 * t_v_c(2));
Eigen::Matrix<T, 3, 1> n_vcs(T(0), T(0), T(1));
Eigen::Matrix<T, 3, 1> n_cam = R_v_c.transpose() * n_vcs;
Eigen::Matrix<T, 3, 3, Eigen::RowMajor> H = K * (R_cc + t_cc * n_cam.transpose() / d) * K_inv;
// H reprojection error
Eigen::Matrix<T, 3, 1> p1(T(x1_), T(y1_), T(1));
Eigen::Matrix<T, 3, 1> p1_proj = H * p1;
p1_proj /= p1_proj(2);
residuals[0] = ceres::abs(T(x2_) - p1_proj(0));
residuals[1] = ceres::abs(T(y2_) - p1_proj(1));
return true;
}
private:
const double x1_, y1_, x2_, y2_;
Eigen::Matrix3d R_vv_, K_;
Eigen::Vector3d t_vv_;
};
2.基于非路面光流的VO估计yaw/pitch
前提:利用空间采样点利用E优化vo(利用E的优化不限制光流在同一个平面),然后通过vo分解出pitch和yaw
首先通过光流构建前后帧特征点之间的匹配关系,然后构建极线约束求解Essential Matrix,分解E得到位移方向t的初值
用tcc和Rcc构建本征矩阵E,然后求sampson残差
//该残差构建需要已知的rpy_vo, xyz_vo, 他们由R_v_v和需要标定的R_v_c构造
Eigen::Matrix3f R_v_c = last_calib_Rvc;
Eigen::Matrix3f R_c_c = R_v_c.transpose() * R_v_v * R_v_c;
Eigen::Vector3f t_c_c = R_v_c.transpose() * (R_v_v * t_v_c + t_v_v - t_v_c);
Eigen::Vector3d rpy_vo = Intr_R2XYZ(R_c_c).cast<double>();
Eigen::Vector3d xyz_vo = t_c_c.cast<double>();
ceres::CostFunction *cost_function = RelativePoseCostFunctionRPY::Create(pt1, pt2);
problem.AddResidualBlock(cost_function, loss_func, rpy_vo.data(), xyz_vo.data());
class RelativePoseCostFunctionRPY {
public:
RelativePoseCostFunctionRPY(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2)
: x1_(p1(0)), y1_(p1(1)), x2_(p2(0)), y2_(p2(1)) {}
static ceres::CostFunction* Create(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2)
{
return ( new ceres::AutoDiffCostFunction<RelativePoseCostFunctionRPY, 1, 3, 3>( new RelativePoseCostFunctionRPY(p1, p2)));
}
template <typename T>
bool operator()(const T* const rpy, const T* const xyz_vo, T* residuals) const {
Eigen::Matrix<T, 3, 1> rpy_data(rpy_vo[0], rpy_vo[1], rpy_vo[2]);
Eigen::Matrix<T, 3, 3, Eigen::RowMajor> R = Intr_XYZ2R(rpy_data);
// Matrix representation of the cross product t x R.
Eigen::Matrix<T, 3, 3> t_x;
t_x << T(0), -xyz_vo[2], xyz_vo[1], xyz_vo[2], T(0), -xyz_vo[0], -xyz_vo[1], xyz_vo[0],
T(0);
// Essential matrix.
const Eigen::Matrix<T, 3, 3> E = t_x * R;
// Homogeneous image coordinates.
const Eigen::Matrix<T, 3, 1> p1_h(T(x1_), T(y1_), T(1));
const Eigen::Matrix<T, 3, 1> p2_h(T(x2_), T(y2_), T(1));
// Squared sampson error.
const Eigen::Matrix<T, 3, 1> Ep1 = E * p1_h;
const Eigen::Matrix<T, 3, 1> Etp2 = E.transpose() * p2_h;
const T p2tEp1 = p2_h.transpose() * Ep1;
residuals[0] = p2tEp1 * p2tEp1 /
(Ep1(0) * Ep1(0) + Ep1(1) * Ep1(1) + Etp2(0) * Etp2(0) +
Etp2(1) * Etp2(1));
return true;
}
private:
const double x1_, y1_, x2_, y2_;
};
利用最优化方法得到更精确的t的值,多个匹配特征点,tx和R构建sampson error优化,然后求解yaw和pitch
double x = xyz_vo(0), y = xyz_vo(1), z = xyz_vo(2);
double curr_pitch = -1.0 * std::atan2(-z, std::sqrt(x * x + y * y));
double curr_yaw = -1.0 * std::atan2(y, x);
3.基于非路面光流的直接估计rpy
这个方法是最普通的方法,直接在前后两帧中提前特征点进行光流跟踪,然后剔除外点。
首先已知车辆坐标系下的移动R_vv和t_vv,最后直接利用光流跟踪值优化R_v_c提取出相机标定参数rpy
//前提:直接估计rpy_v_c
cost_function = EpipolarErrorWithPriorMotion::Create(curr_points[i], last_points[i], R_vv, t_vv);
problem.AddResidualBlock(cost_function, loss_func, rpy_v_c.data(), xyz_v_c.data());
class EpipolarErrorWithPriorMotion {
public:
EpipolarErrorWithPriorMotion(const cv::Point2f &p1, const cv::Point2f &p2, const Eigen::Matrix3d &R_vv, const Eigen::Vector3d &t_vv)
: x1_(p1.x), y1_(p1.y), x2_(p2.x), y2_(p2.y), R__vv_(R_vv), t_vv_(t_vv) {}
static ceres::CostFunction *Create(const cv::Point2f &p1, const cv::Point2f &p2, const Eigen::Matrix3d &R, const Eigen::Vector3d &t)
{
return ( new ceres::AutoDiffCostFunction<EpipolarErrorWithPriorMotion, 1, 3, 3>(new EpipolarErrorWithPriorMotion(p1, p2, R, t, sleep_us)));
}
template <typename T>
bool operator()(const T *const rpy_v_c, const T *const xyz_v_c, T *residuals) const {
// get extrinsic params
Eigen::Matrix<T, 3, 1> rpy_data(rpy_v_c[0], rpy_v_c[1], rpy_v_c[2]);
Eigen::Matrix<T, 3, 3, Eigen::RowMajor> R_v_c = Intr_XYZ2R(rpy_data);
Eigen::Matrix<T, 3, 1> t_v_c(xyz_v_c[0], xyz_v_c[1], xyz_v_c[2]);
// camera relative pose
Eigen::Matrix<T, 3, 3, Eigen::RowMajor> R_cc = R_v_c.transpose() * R_vv * R_v_c;
Eigen::Matrix<T, 3, 1> t_cc = R_v_c.transpose() * (R_vv * t_v_c + t_vv - t_v_c);
// epipolar error
Eigen::Matrix<T, 3, 3> t_x;
t_x << T(0), -t_cc[2], t_cc[1], t_cc[2], T(0), -t_cc[0], -t_cc[1], t_cc[0], T(0);
const Eigen::Matrix<T, 3, 3, Eigen::RowMajor> E = t_x * R_cc;
// Homogeneous image coordinates.
const Eigen::Matrix<T, 3, 1> p1_h(T(x1_), T(y1_), T(1));
const Eigen::Matrix<T, 3, 1> p2_h(T(x2_), T(y2_), T(1));
// Squared sampson error.
const Eigen::Matrix<T, 3, 1> Ep1 = E * p1_h;
const Eigen::Matrix<T, 3, 1> Etp2 = E.transpose() * p2_h;
const T p2tEp1 = p2_h.transpose() * Ep1;
residuals[0] = p2tEp1 * p2tEp1 /
(Ep1(0) * Ep1(0) + Ep1(1) * Ep1(1) + Etp2(0) * Etp2(0) +
Etp2(1) * Etp2(1));
return true;
}
private
const double x1_, y1_, x2_, y2_;
Eigen::Matrix3d R_vv_;
Eigen::Vector3d t_vv_;
};