视觉十四讲：第八讲_直接法

1.直接法的推导：

考虑某个空间点P和两个时刻的相机，P的世界坐标为[X,Y,Z],它在两个相机上成像，记非齐次像素坐标为\(p_{1},p_{2}\)，目标是求第一个相机到第二个相机的相对位姿变换。

直接法的思路是根据当前相机的位姿估计值，来寻找\(p_{2}\)的位置，误差项为光度误差
\(e = I_{1}(p_{1}) - I_{2}(p_{2})\)
考虑多个空间点P，那个整个相机位姿估计问题会变成：

主要问题是求误差e相对于位姿的导数关系。使用李代数的扰动模型，给\(exp(\xi)\)左乘一个小扰动\(exp(\delta \xi)\)得：

记：\(q = \delta \xi^{\Lambda} exp(\xi^{\Lambda}) P\),
\(u = \frac{1}{Z_{2}}Kq\)
q为P在扰动之后，位于第二个相机坐标系下的坐标，而u为它的像素坐标，利用一阶泰勒展开，有：


故误差相对于李代数的雅可比矩阵为：

2.直接法分类：

1.稀疏关键点：通常使用数百个至上千个关键点，像LK光流那样，这种稀疏直接法不必计算描述子，因此速度最快，但只能计算稀疏的重构。
2.半稠密直接法：考虑图像内带有梯度的像素点，舍弃像素梯度不明显的地方，重构一个半稠密结构。
3.稠密直接法：考虑所有像素点，需要GPU加速。

3.实践直接法：

1.单层稀疏直接法：

class JacobianAccumulator {
public:JacobianAccumulator(const cv::Mat &img1_,const cv::Mat &img2_,const VecVector2d &px_ref_,const vector depth_ref_,Sophus::SE3d &T21_) :img1(img1_), img2(img2_), px_ref(px_ref_), depth_ref(depth_ref_), T21(T21_) {projection = VecVector2d(px_ref.size(), Eigen::Vector2d(0, 0));}/// accumulate jacobians in a rangevoid accumulate_jacobian(const cv::Range &range);/// get hessian matrixMatrix6d hessian() const { return H; }/// get biasVector6d bias() const { return b; }/// get total costdouble cost_func() const { return cost; }/// get projected pointsVecVector2d projected_points() const { return projection; }/// reset h, b, cost to zerovoid reset() {H = Matrix6d::Zero();b = Vector6d::Zero();cost = 0;}private:const cv::Mat &img1;const cv::Mat &img2;const VecVector2d &px_ref;const vector depth_ref;Sophus::SE3d &T21;VecVector2d projection; // projected pointsstd::mutex hessian_mutex;Matrix6d H = Matrix6d::Zero();Vector6d b = Vector6d::Zero();double cost = 0;
};
void DirectPoseEstimationSingleLayer(const cv::Mat &img1,const cv::Mat &img2,const VecVector2d &px_ref,const vector depth_ref,Sophus::SE3d &T21) {const int iterations = 10;double cost = 0, lastCost = 0;auto t1 = chrono::steady_clock::now();JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);for (int iter = 0; iter < iterations; iter++) {  //10次迭代次数jaco_accu.reset();cv::parallel_for_(cv::Range(0, px_ref.size()),  //循环2000个点std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, std::placeholders::_1));Matrix6d H = jaco_accu.hessian();Vector6d b = jaco_accu.bias();//10次迭代获取更新值Vector6d update = H.ldlt().solve(b);//更新姿态T21 = Sophus::SE3d::exp(update) * T21;cost = jaco_accu.cost_func();if (std::isnan(update[0])) {// sometimes occurred when we have a black or white patch and H is irreversiblecout << "update is nan" << endl;break;}if (iter > 0 && cost > lastCost) {cout << "cost increased: " << cost << ", " << lastCost << endl;break;}if (update.norm() < 1e-3) {// convergebreak;}lastCost = cost;cout << "iteration: " << iter << ", cost: " << cost << endl;}cout << "T21 = \n" << T21.matrix() << endl;auto t2 = chrono::steady_clock::now();auto time_used = chrono::duration_cast>(t2 - t1);cout << "direct method for single layer: " << time_used.count() << endl;// plot the projected pixels herecv::Mat img2_show;cv::cvtColor(img2, img2_show, CV_GRAY2BGR);VecVector2d projection = jaco_accu.projected_points();for (size_t i = 0; i < px_ref.size(); ++i) {auto p_ref = px_ref[i];auto p_cur = projection[i];if (p_cur[0] > 0 && p_cur[1] > 0) {cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),cv::Scalar(0, 250, 0));}}cv::imshow("current", img2_show);cv::waitKey();
}
void JacobianAccumulator::accumulate_jacobian(const cv::Range &range) {// parametersconst int half_patch_size = 1; //区块大小int cnt_good = 0;Matrix6d hessian = Matrix6d::Zero();Vector6d bias = Vector6d::Zero();double cost_tmp = 0;for (size_t i = range.start; i < range.end; i++) {//第一张图的像素坐标转相机归一化坐标，乘深度转为相机3D点Eigen::Vector3d point_ref =depth_ref[i] * Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);//将第一张图的点根据估计的位姿，转到第二张图的坐标系下Eigen::Vector3d point_cur = T21 * point_ref;if (point_cur[2] < 0)   // depth invalidcontinue;//将相机坐标系转为像素坐标系float u = fx * point_cur[0] / point_cur[2] + cx, v = fy * point_cur[1] / point_cur[2] + cy;//去除边界点if (u < half_patch_size || u > img2.cols - half_patch_size || v < half_patch_size ||v > img2.rows - half_patch_size)continue;projection[i] = Eigen::Vector2d(u, v);double X = point_cur[0], Y = point_cur[1], Z = point_cur[2],//深度平方，用于雅可比矩阵的计算Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;cnt_good++;//2个for循环主要是是为了计算图像梯度for (int x = -half_patch_size; x <= half_patch_size; x++)for (int y = -half_patch_size; y <= half_patch_size; y++) {//重投影光度误差，double error = GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) -GetPixelValue(img2, u + x, v + y);Matrix26d J_pixel_xi;Eigen::Vector2d J_img_pixel;//计算李代数雅可比矩阵J_pixel_xi(0, 0) = fx * Z_inv;J_pixel_xi(0, 1) = 0;J_pixel_xi(0, 2) = -fx * X * Z2_inv;J_pixel_xi(0, 3) = -fx * X * Y * Z2_inv;J_pixel_xi(0, 4) = fx + fx * X * X * Z2_inv;J_pixel_xi(0, 5) = -fx * Y * Z_inv;J_pixel_xi(1, 0) = 0;J_pixel_xi(1, 1) = fy * Z_inv;J_pixel_xi(1, 2) = -fy * Y * Z2_inv;J_pixel_xi(1, 3) = -fy - fy * Y * Y * Z2_inv;J_pixel_xi(1, 4) = fy * X * Y * Z2_inv;J_pixel_xi(1, 5) = fy * X * Z_inv;//图像梯度J_img_pixel = Eigen::Vector2d(0.5 * (GetPixelValue(img2, u + 1 + x, v + y) - GetPixelValue(img2, u - 1 + x, v + y)),0.5 * (GetPixelValue(img2, u + x, v + 1 + y) - GetPixelValue(img2, u + x, v - 1 + y)));// 计算误差对于李代数的雅可比矩阵Vector6d J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose();hessian += J * J.transpose();bias += -error * J;cost_tmp += error * error;}}if (cnt_good) {// set hessian, bias and costunique_lock lck(hessian_mutex);   //多线程互斥量,自动释放H += hessian;b += bias;cost += cost_tmp / cnt_good;}
}

2.多层稀疏直接法：

类似于光流，把单层直接法扩展到金字塔式的多层直接法上，用Coarse-to-fine的过程计算相对运动。

void DirectPoseEstimationMultiLayer(const cv::Mat &img1,const cv::Mat &img2,const VecVector2d &px_ref,const vector depth_ref,Sophus::SE3d &T21) {// parametersint pyramids = 4;double pyramid_scale = 0.5;double scales[] = {1.0, 0.5, 0.25, 0.125};// create pyramidsvector pyr1, pyr2; // image pyramidsfor (int i = 0; i < pyramids; i++) {if (i == 0) {pyr1.push_back(img1);pyr2.push_back(img2);} else {cv::Mat img1_pyr, img2_pyr;cv::resize(pyr1[i - 1], img1_pyr,cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));cv::resize(pyr2[i - 1], img2_pyr,cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));pyr1.push_back(img1_pyr);pyr2.push_back(img2_pyr);}}double fxG = fx, fyG = fy, cxG = cx, cyG = cy;  // backup the old valuesfor (int level = pyramids - 1; level >= 0; level--) {VecVector2d px_ref_pyr; // set the keypoints in this pyramid levelfor (auto &px: px_ref) {px_ref_pyr.push_back(scales[level] * px);}// scale fx, fy, cx, cy in different pyramid levelsfx = fxG * scales[level];fy = fyG * scales[level];cx = cxG * scales[level];cy = cyG * scales[level];DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);}}

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！