2.直接法
误差为
雅可比矩阵为
首先要计算移动过的点的坐标,之后求解雅可比矩阵
void DirectPoseEstimationSingleLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector<double> depth_ref,
Sophus::SE3 &T21
) {
// parameters
int half_patch_size = 4;
int iterations = 100;
double cost = 0, lastCost = 0;
int nGood = 0; // good projections
VecVector2d goodProjection;
for (int iter = 0; iter < iterations; iter++) {
nGood = 0;
goodProjection.clear();
// Define Hessian and bias
Matrix6d H = Matrix6d::Zero(); // 6x6 Hessian
Vector6d b = Vector6d::Zero(); // 6x1 bias
for (size_t i = 0; i < px_ref.size(); i++) {
// compute the projection in the second image
// TODO START YOUR CODE HERE
double X_ref=depth_ref[i]*(px_ref[i][0]-cx)/fx;
double Y_ref=depth_ref[i]*(px_ref[i][1]-cy)/fy;
double Z_ref=depth_ref[i];
Matrix4d T_21=T21.matrix();
double X_cur=T_21(0,0)*X_ref+T_21(0,1)*Y_ref+T_21(0,2)*Z_ref+T_21(0,3);
double Y_cur=T_21(1,0)*X_ref+T_21(1,1)*Y_ref+T_21(1,2)*Z_ref+T_21(1,3);
double Z_cur=T_21(2,0)*X_ref+T_21(2,1)*Y_ref+T_21(2,2)*Z_ref+T_21(2,3);
float u =0, v = 0;
u=(