深蓝学院视觉SLAM第六次习题（下）
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=(float)((X_cur*fx)/Z_cur+cx);
v=(float)((Y_cur*fy)/Z_cur+cy);
if(u>=half_patch_size&&v>=half_patch_size&&u<=(img2.cols-half_patch_size)&&v<=(img2.rows-half_patch_size))
{
nGood++;
goodProjection.push_back(Vector2d(u, v));
// and compute error and jacobian
for (int x = -half_patch_size; x < half_patch_size; x++)
for (int y = -half_patch_size; y < half_patch_size; y++) {
double error =0;
error=GetPixelValue(img1,px_ref[i][0]+x,px_ref[i][1]+y)-GetPixelValue(img2,u+x,v+y);
Matrix26d J_pixel_xi; // pixel to \xi in Lie algebra
Eigen::Vector2d J_img_pixel; // image gradients
J_img_pixel[0]=(GetPixelValue(img2,u+x+1,v+y)-GetPixelValue(img2,u+x-1,v+y))/2;
J_img_pixel[1]=(GetPixelValue(img2,u+x,v+y+1)-GetPixelValue(img2,u+x,v+y-1))/2;
J_pixel_xi(0,0)=fx/Z_cur;
J_pixel_xi(0,1)=0;
J_pixel_xi(0,2)=-(fx*X_cur)/(Z_cur*Z_cur);
J_pixel_xi(0,3)=-(fx*X_cur*Y_cur)/(Z_cur*Z_cur);
J_pixel_xi(0,4)=fx+(fx*X_cur*X_cur)/(Z_cur*Z_cur);
J_pixel_xi(0,5)=-(fx*Y_cur)/(Z_cur);
J_pixel_xi(1,0)=0;
J_pixel_xi(1,1)=fy/Z_cur;
J_pixel_xi(1,2)=-(fy*Y_cur)/(Z_cur*Z_cur);
J_pixel_xi(1,3)=-fy-(fy*Y_cur*Y_cur)/(Z_cur*Z_cur);
J_pixel_xi(1,4)=(fy*X_cur*Y_cur)/(Z_cur*Z_cur);
J_pixel_xi(1,5)=(fy*X_cur)/(Z_cur);
// total jacobian
Vector6d J=-1*(J_img_pixel.transpose()*J_pixel_xi).transpose();
H += J * J.transpose();
b += -error * J;
cost += error * error;
}
}
// END YOUR CODE HERE
}
// solve update and put it into estimation
// TODO START YOUR CODE HERE
Vector6d update;
update=H.ldlt().solve(b);
T21 = Sophus::SE3::exp(update) * T21;
// END YOUR CODE HERE
cost /= nGood;
if (isnan(update[0])) {
// sometimes occurred when we have a black or white patch and H is irreversible
cout << "update is nan" << endl;
break;
}
if (iter > 0 && cost > lastCost) {
cout << "cost increased: " << cost << ", " << lastCost << endl;
break;
}
lastCost = cost;
cout << "cost = " << cost << ", good = " << nGood << endl;
}
cout << "good projection: " << nGood << endl;
cout << "T21 = \n" << T21.matrix() << endl;
// in order to help you debug, we plot the projected pixels here
cv::Mat img1_show, img2_show;
cv::cvtColor(img1, img1_show, CV_GRAY2BGR);
cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
for (auto &px: px_ref) {
cv::rectangle(img1_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
cv::Scalar(0, 250, 0));
}
for (auto &px: goodProjection) {
cv::rectangle(img2_show, cv::Point2f(px[0] - 2, px[1] - 2), cv::Point2f(px[0] + 2, px[1] + 2),
cv::Scalar(0, 250, 0));
}
cv::imshow("reference", img1_show);
cv::imshow("current", img2_show);
cv::waitKey();
多层直接法主要是建立图像金字塔， 在缩放图像时，图像内参也需要跟着变化。
void DirectPoseEstimationMultiLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector<double> depth_ref,
Sophus::SE3 &T21
) {
// parameters
int pyramids = 4;
double pyramid_scale = 0.5;
double scales[] = {1.0, 0.5, 0.25, 0.125};
// create pyramids
vector<cv::Mat> pyr1, pyr2; // image pyramids
// TODO START YOUR CODE HERE
for (int i = 0; i < pyramids; i++) {
if(i==0)
{
pyr1.push_back(img1);
pyr2.push_back(img2);
}
else
{
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);
}
}
// END YOUR CODE HERE
double fxG = fx, fyG = fy, cxG = cx, cyG = cy; // backup the old values
for (int level = pyramids - 1; level >= 0; level--) {
VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
for (auto &px: px_ref) {
px_ref_pyr.push_back(scales[level] * px);
}
// TODO START YOUR CODE HERE
// scale fx, fy, cx, cy in different pyramid levels
fx=fxG*scales[level];
fy=fyG*scales[level];
cx=cxG*scales[level];
cy=cyG*scales[level];
cout<<"这是金字塔第"<<level<<"层"<<endl;
// END YOUR CODE HERE
DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
}
}
结果：
第四层
第三层
第二层
第一层
五幅图像的光流
输出光流图像时需要将上面的代码稍作修改，修改goodProjection，以用来对应
** 延伸讨论**
- 直接法是否可以类似光流，提出 inverse, compositional 的概念？它们有意义吗？
直接法通过求解相机运动来计算第二帧图像对应关键点的位置，如果提出 inverse, compositional 的概念，第二帧图像的信息将会失效，没有意义。
- 请思考上面算法哪些地方可以缓存或加速？
加窗口部分，合适大小的窗口，会使计算加速
- 在上述过程中，我们实际假设了哪两个 patch 不变？
灰度不变假设，同窗口内深度不变假设
- 为何可以随机取点？而不用取角点或线上的点？那些不是角点的地方，投影算对了吗？
直接法对灰度值优化，不是角点也没有关系
- 请总结直接法相对于特征点法的异同与优缺点。
看书吧书上有
3.使用光流计算视差
给定图像 left.png, right.png，计算 left.png 中的 GFTT 点在 right.png 中的（水平）视差，然后与 disparity.png
进行比较
计算误差：
int count=0;
for(int i=0;i<kp2_multi.size();i++)
{
if(success_multi[i]&&status[i])
{
count+=1;
int disparity1=disparity_img.at<uchar>(kp1[i].pt.y,kp1[i].pt.x);
int disparity2=kp1[i].pt.x-kp2_multi[i].pt.x;
//int disparity3=pt1[i].x-pt2[i].x;
int error1=disparity1-disparity2;
//int error2=disparity1-disparity3;
cout<<"第"<<count<<"个点 "<<" 实际视差： "<<disparity1<<" 光流视差： "<<disparity2<<" 误差： "<<error1<<endl;
}
}
我的结果不是很理想啊，不知道是不是我代码写错了