3 直接法
以图像left.png为参考图片，已知其视差真值disparity.png，求图像000001.png-000005.png相对于参考图片的位姿。
单个残差项
e
i
,
x
,
y
(
T
)
e_{i,x,y}(T)
ei,x,y(T)为，
e
i
,
x
,
y
(
T
)
=
I
1
(
u
i
+
x
,
v
i
+
y
)
−
I
2
(
u
i
′
+
x
,
v
i
′
+
y
)
e_{i,x,y}(T)=I_1(u_i+x,v_i+y)-I_2(u_i'+x,v_i'+y)
ei,x,y(T)=I1(ui+x,vi+y)−I2(ui′+x,vi′+y)
那么，优化问题可用数学语言表述为，
T
21
∗
=
argmin
T
21
∑
i
=
1
N
∑
x
=
−
1
1
∑
y
=
−
1
1
e
i
,
x
,
y
T_{21}^*=\underset{T_{21}}{\operatorname{argmin}} \sum_{i=1}^N \sum_{x=-1}^1 \sum_{y=-1}^1 e_{i,x,y}
T21∗=T21argmini=1∑Nx=−1∑1y=−1∑1ei,x,y
单个误差项的雅克比矩阵
J
i
,
x
,
y
(
T
)
J_{i,x,y}(T)
Ji,x,y(T)可求得为，
J
i
,
x
,
y
(
T
)
=
∂
e
i
,
x
,
y
∂
T
=
−
1
⋅
∂
I
2
(
u
i
′
+
x
,
v
i
′
+
y
)
∂
(
u
i
′
+
x
,
v
i
′
+
y
)
T
⋅
∂
(
u
i
′
+
x
,
v
i
′
+
y
)
∂
(
X
′
,
Y
′
,
Z
′
)
T
⋅
∂
(
X
′
,
Y
′
,
Z
′
)
∂
T
⋅
δ
T
J_{i,x,y}(T)=\frac{\partial e_{i,x,y}}{\partial T}= -1 \cdot \frac{\partial I_2(u_i'+x,v_i'+y)}{\partial(u_i'+x,v_i'+y)^T} \cdot \frac{\partial (u_i'+x,v_i'+y)}{\partial (X',Y',Z')^T} \cdot \frac{\partial (X',Y',Z')}{\partial T} \cdot \delta T
Ji,x,y(T)=∂T∂ei,x,y=−1⋅∂(ui′+x,vi′+y)T∂I2(ui′+x,vi′+y)⋅∂(X′,Y′,Z′)T∂(ui′+x,vi′+y)⋅∂T∂(X′,Y′,Z′)⋅δT
其中第1项偏导数为梯度，第2项为相机投影模型求偏导，第3项为李代数求偏导。规范书写一般将
T
T
T写成
δ
ξ
\delta \xi
δξ。
3.1 单层直接法
cpp文件内容为，
#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <boost/format.hpp>
#include <pangolin/pangolin.h>
#include <Eigen/Core> //Eigen核心模块
#include <Eigen/Dense> //Eigen稠密矩阵运算模块
using namespace std;
using namespace cv;
using namespace Sophus;
using namespace pangolin;
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
//相机内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
//双目相机基线
double baseline = 0.573;
//表示哪张图的光流
int INDEX = 1;
//图片路径
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmtOthers("../%06d.png"); //其它图像路径
typedef Matrix<double, 6, 6> Matrix6d;
typedef Matrix<double, 2, 6> Matrix26d;
typedef Matrix<double, 6, 1> Vector6d;
//用于并行计算雅可比矩阵的类
class JacobianAccumulator{
public:
//类的构造函数，使用列表进行初始化
JacobianAccumulator(
const Mat& img1_,
const Mat& img2_,
const VecVector2d& px_ref_,
const vector<double> depth_ref_,
SE3d& T21_
) : img1(img1_), img2(img2_), px_ref(px_ref_), depth_ref(depth_ref_), T21(T21_)
{
projection = VecVector2d(px_ref.size(), Vector2d(0, 0));
}
//在range范围内加速计算雅可比矩阵
void accumulate_jacobian(const cv::Range& range);
//获取海塞矩阵
Matrix6d hessian() const {return H;}
//获取矩阵b
Vector6d bias() const {return b;}
//获取总共的代价
double cost_func() const {return cost;}
//获取图像2中的角点坐标
VecVector2d project_points() const {return projection;}
//将海塞矩阵H，矩阵b和代价cost置为0
void reset()
{
H = Matrix6d::Zero();
b = Vector6d::Zero();
cost = 0;
}
private:
const cv::Mat& img1;
const cv::Mat& img2;
const VecVector2d& px_ref; //这是图像1中角点坐标
const vector<double> depth_ref; //这是图像1中路标点的Z坐标值
SE3d& T21;
VecVector2d projection;
std::mutex hessian_mutex;
Matrix6d H = Matrix6d::Zero();
Vector6d b = Vector6d::Zero();
double cost = 0;
};
void DirectPoseEstimationSingleLayer(
const Mat& img1,
const Mat& img2,
const VecVector2d& px_ref,
const vector<double> depth_ref,
SE3d& T21
);
//双线性插值求灰度值
inline float GetPixelValue(const Mat& img, float x, float y) //inline表示内联函数，它是为了解决一些频繁调用的小函数大量消耗栈空间的问题而引入的
{
//边界检验
if(x < 0) x = 0;
if(y < 0) y = 0;
if(x >= img.cols) x = img.cols - 1;
if(y >= img.rows) y = img.rows - 1;
//单精度浮点数x和y的整数部分和小数部分
int xInteger = (int)x;
int yInteger = (int)y;
float xFraction = x - xInteger;
float yFraction = y - yInteger;
//...I1 I2...
// I
//
//...I3 I4...
//I1，I2，I3和I4计算如下
uchar I1 = img.at<uchar>(yInteger, xInteger);
uchar I2 = img.at<uchar>(yInteger, xInteger+1);
uchar I3 = img.at<uchar>(yInteger+1, xInteger);
uchar I4 = img.at<uchar>(yInteger+1, xInteger+1);
//I1，I2，I3和I4的权重计算如下
float w1 = (1 - xFraction) * (1 - yFraction);
float w2 = xFraction * (1 - yFraction);
float w3 = (1 - xFraction) * yFraction;
float w4 = xFraction * yFraction;
//计算最终的像素灰度值
float I = w1 * I1 + w2 * I2 + w3 * I3 + w4 * I4;
return I;
}
int main()
{
Mat left_img = imread(left_file, 0); //0表示返回灰度图，left.png表示000000.png
Mat disparity_img = imread(disparity_file, 0); //0表示返回灰度图，disparity.png是left.png的视差图
//在图像1中随机选择一些像素点，然后恢复深度，得到一些路标点
RNG rng;
int nPoints = 2000;
int boarder = 20;
VecVector2d pixels_ref;
vector<double> depth_ref;
for(int i = 0; i < nPoints; i++)
{
int x = rng.uniform(boarder, left_img.cols - boarder);
int y = rng.uniform(boarder, left_img.rows - boarder);
int disparity = disparity_img.at<uchar>(y, x);
double depth = fx * baseline / disparity;
depth_ref.push_back(depth); //depth_ref存储图像1中路标点的Z坐标值
pixels_ref.push_back(Vector2d(x, y)); //pixels_ref存储图像1中角点坐标值
}
SE3d T_cur_ref;
for(int i = 1; i < 6; i++) //i从1到5，总共5张图
{
Mat img = imread((fmtOthers % i).str(), 0); //读图，0表示返回一张灰度图
//利用单层直接法计算图像img相对于left_img的位姿T_cur_ref，以图片left.png为基准
DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
cout << "图像" << (fmtOthers % i).str() << "相对于图像left.png的变换矩阵T21为：\n" << T_cur_ref.matrix() << endl;
}
return 0;
}
void DirectPoseEstimationSingleLayer(
const Mat& img1,
const Mat& img2,
const VecVector2d& px_ref, //第1张图中的角点坐标
const vector<double> depth_ref, //第1张图中路标点的Z坐标值
SE3d& T21
)
{
const int iterations = 10;
double cost = 0, lastCost = 0;
JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);
for(int iter = 0; iter < iterations; iter++)
{
jaco_accu.reset();
//下面这条语句完成并行计算海塞矩阵H，矩阵b和代价cost
cv::parallel_for_(cv::Range(0, px_ref.size()), std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, placeholders::_1));
Matrix6d H = jaco_accu.hessian();
Vector6d b = jaco_accu.bias();
//求解增量方程更新优化变量T21
Vector6d update = H.ldlt().solve(b);
T21 = SE3d::exp(update) * T21;
cost = jaco_accu.cost_func();
if(isnan(update[0]))
{
//cout << "解出来的更新量不是一个数字，退出迭代！" << endl;
break;
}
if(iter > 0 && cost > lastCost) //去掉等于号，有些影响
{
//cout << "代价不再减小，退出迭代！" << endl;
break;
}
if(update.norm() < 1e-3)
{
//cout << "更新量的模小于1e-3，退出迭代！" << endl;
break;
}
lastCost = cost;
}//GN迭代结束
//画出光流图
char str1[10];
sprintf(str1, "%06d", INDEX++);
char str2[] = ".png";
strcat(str1, str2); //将str1拼成000001.png的形式
Mat img2_show;
cvtColor(img2, img2_show, CV_GRAY2BGR);
VecVector2d projection = jaco_accu.project_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) //p_cur[0]还得小于img2_show.cols，p_cur[1]还得小于img2_show.rows呢？
{
circle(img2_show, Point2f(p_cur[0], p_cur[1]), 2, Scalar(0, 255, 0), 2);
line(img2_show, Point2f(p_ref[0], p_ref[1]), Point2f(p_cur[0], p_cur[1]), Scalar(0, 255, 0));
}
}
imshow(str1, img2_show);
waitKey(0);
memset(str1, 0, sizeof str1);
}
void JacobianAccumulator::accumulate_jacobian(const cv::Range &range)
{
const 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++)
{
//point_ref表示图像1中的路标点
Vector3d point_ref = depth_ref[i] * Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
//point_cur表示图像2中的路标点
Vector3d point_cur = T21 * point_ref;
if(point_cur[2] < 0)
continue; //此时深度无效
//u,v表示图像2中对应的角点坐标
float u = fx * point_cur[0] / point_cur[2] + cx;
float v = fy * point_cur[1] / point_cur[2] + cy;
if(u < 1 || u > img2.cols - 1 || v < 1 || v > img2.rows - 1)
continue;
projection[i] = Vector2d(u, v); //projection表示图像2中相应的角点坐标值
double X = point_cur[0], Y = point_cur[1], Z = point_cur[2];
double Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;
cnt_good++;
//计算海塞矩阵H，矩阵b和代价cost
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;
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[0] = 0.5 * (GetPixelValue(img2, u + x + 1, v + y) - GetPixelValue(img2, u + x - 1, v + y));
J_img_pixel[1] = 0.5 * (GetPixelValue(img2, u + x, v + y + 1) - GetPixelValue(img2, u + x, v + y - 1));
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)
{
unique_lock<mutex> lck(hessian_mutex); //线程锁
H += hessian; //在range外加海塞矩阵是什么意思？奇怪
b += bias;
cost += cost_tmp / cnt_good; //每次迭代成功的角点数目可能不一样，所以计算一个平均的代价
}
}
CMakeLists.txt文件内容为，
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})
include_directories("/usr/include/eigen3")
find_package(Sophus REQUIRED)
include_directories(${Sophus_DIRECTORIES})
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_DIRECTORIES})
add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES} ${Pangolin_LIBRARIES})
程序运行结果为，
图像../000001.png相对于图像left.png的变换矩阵T21为：
0.999999 -9.92972e-05 0.00132181 -0.021536
9.49612e-05 0.999995 0.00327994 -0.0148981
-0.00132213 -0.00327981 0.999994 -0.127968
0 0 0 1
图像../000002.png相对于图像left.png的变换矩阵T21为：
0.999999 -0.000726889 0.00140624 -0.0267175
0.00072109 0.999991 0.00411998 0.00496084
-0.00140922 -0.00411896 0.999991 -0.121664
0 0 0 1
图像../000003.png相对于图像left.png的变换矩阵T21为：
0.999968 -0.00694494 0.00396381 -0.0966541
0.00692305 0.999961 0.00551135 0.0118712
-0.00400193 -0.00548373 0.999977 -0.20365
0 0 0 1
图像../000004.png相对于图像left.png的变换矩阵T21为：
0.999969 -0.00768232 0.00172409 -0.0786847
0.00766556 0.999925 0.00952419 -0.0530805
-0.00179713 -0.00951068 0.999953 -0.328418
0 0 0 1
图像../000005.png相对于图像left.png的变换矩阵T21为：
0.999943 -0.0106705 0.000627648 -0.0921105
0.0106616 0.999861 0.0128082 -0.0830512
-0.000764231 -0.0128008 0.999918 -0.374587
0 0 0 1
3.2 多层直接法
cpp文件内容为，
#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <boost/format.hpp>
#include <pangolin/pangolin.h>
#include <Eigen/Core> //Eigen核心模块
#include <Eigen/Dense> //Eigen稠密矩阵运算模块
using namespace std;
using namespace cv;
using namespace Sophus;
using namespace pangolin;
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
//相机内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
//双目相机基线
double baseline = 0.573;
//表示哪张图的光流
int INDEX = 1;
//图片路径
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmtOthers("../%06d.png"); //其它图像路径
typedef Matrix<double, 6, 6> Matrix6d;
typedef Matrix<double, 2, 6> Matrix26d;
typedef Matrix<double, 6, 1> Vector6d;
//用于并行计算雅可比矩阵的类
class JacobianAccumulator{
public:
//类的构造函数，使用列表进行初始化
JacobianAccumulator(
const Mat& img1_,
const Mat& img2_,
const VecVector2d& px_ref_,
const vector<double> depth_ref_,
SE3d& T21_
) : img1(img1_), img2(img2_), px_ref(px_ref_), depth_ref(depth_ref_), T21(T21_)
{
projection = VecVector2d(px_ref.size(), Vector2d(0, 0));
}
//在range范围内加速计算雅可比矩阵
void accumulate_jacobian(const cv::Range& range);
//获取海塞矩阵
Matrix6d hessian() const {return H;}
//获取矩阵b
Vector6d bias() const {return b;}
//获取总共的代价
double cost_func() const {return cost;}
//获取图像2中的角点坐标
VecVector2d project_points() const {return projection;}
//将海塞矩阵H，矩阵b和代价cost置为0
void reset()
{
H = Matrix6d::Zero();
b = Vector6d::Zero();
cost = 0;
}
private:
const cv::Mat& img1;
const cv::Mat& img2;
const VecVector2d& px_ref; //这是图像1中角点坐标
const vector<double> depth_ref; //这是图像1中路标点的Z坐标值
SE3d& T21;
VecVector2d projection;
std::mutex hessian_mutex;
Matrix6d H = Matrix6d::Zero();
Vector6d b = Vector6d::Zero();
double cost = 0;
};
//单层直接法
void DirectPoseEstimationSingleLayer(
const Mat& img1,
const Mat& img2,
const VecVector2d& px_ref,
const vector<double> depth_ref,
SE3d& T21
);
//多层直接法
void DirectPoseEstimationMultiLayer(
const Mat& img1,
const Mat& img2,
const VecVector2d& px_ref,
const vector<double> depth_ref,
SE3d& T21
);
//双线性插值求灰度值
inline float GetPixelValue(const Mat& img, float x, float y) //inline表示内联函数，它是为了解决一些频繁调用的小函数大量消耗栈空间的问题而引入的
{
//边界检验
if(x < 0) x = 0;
if(y < 0) y = 0;
if(x >= img.cols) x = img.cols - 1;
if(y >= img.rows) y = img.rows - 1;
//单精度浮点数x和y的整数部分和小数部分
int xInteger = (int)x;
int yInteger = (int)y;
float xFraction = x - xInteger;
float yFraction = y - yInteger;
//...I1 I2...
// I
//
//...I3 I4...
//I1，I2，I3和I4计算如下
uchar I1 = img.at<uchar>(yInteger, xInteger);
uchar I2 = img.at<uchar>(yInteger, xInteger+1);
uchar I3 = img.at<uchar>(yInteger+1, xInteger);
uchar I4 = img.at<uchar>(yInteger+1, xInteger+1);
//I1，I2，I3和I4的权重计算如下
float w1 = (1 - xFraction) * (1 - yFraction);
float w2 = xFraction * (1 - yFraction);
float w3 = (1 - xFraction) * yFraction;
float w4 = xFraction * yFraction;
//计算最终的像素灰度值
float I = w1 * I1 + w2 * I2 + w3 * I3 + w4 * I4;
return I;
}
int main()
{
Mat left_img = imread(left_file, 0); //0表示返回灰度图，left.png表示000000.png
Mat disparity_img = imread(disparity_file, 0); //0表示返回灰度图，disparity.png是left.png的视差图
//在图像1中随机选择一些像素点，然后恢复深度，得到一些路标点
RNG rng;
int nPoints = 2000;
int boarder = 20;
VecVector2d pixels_ref;
vector<double> depth_ref;
for(int i = 0; i < nPoints; i++)
{
int x = rng.uniform(boarder, left_img.cols - boarder);
int y = rng.uniform(boarder, left_img.rows - boarder);
int disparity = disparity_img.at<uchar>(y, x);
double depth = fx * baseline / disparity;
depth_ref.push_back(depth); //depth_ref存储图像1中路标点的Z坐标值
pixels_ref.push_back(Vector2d(x, y)); //pixels_ref存储图像1中角点坐标值
}
SE3d T_cur_ref;
for(int i = 1; i < 6; i++) //i从1到5，总共5张图
{
Mat img = imread((fmtOthers % i).str(), 0); //读图，0表示返回一张灰度图
//利用多层直接法计算图像img相对于left_img的位姿T_cur_ref，以图片left.png为基准
DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
INDEX++;
cout << "图像" << (fmtOthers % i).str() << "相对于图像left.png的变换矩阵T21为：\n" << T_cur_ref.matrix() << endl;
}
return 0;
}
void DirectPoseEstimationSingleLayer(
const Mat& img1,
const Mat& img2,
const VecVector2d& px_ref, //第1张图中的角点坐标
const vector<double> depth_ref, //第1张图中路标点的Z坐标值
SE3d& T21
)
{
const int iterations = 10;
double cost = 0, lastCost = 0;
JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);
for(int iter = 0; iter < iterations; iter++)
{
jaco_accu.reset();
//下面这条语句完成并行计算海塞矩阵H，矩阵b和代价cost
cv::parallel_for_(cv::Range(0, px_ref.size()), std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, placeholders::_1));
Matrix6d H = jaco_accu.hessian();
Vector6d b = jaco_accu.bias();
//求解增量方程更新优化变量T21
Vector6d update = H.ldlt().solve(b);
T21 = SE3d::exp(update) * T21;
cost = jaco_accu.cost_func();
if(isnan(update[0]))
{
//cout << "解出来的更新量不是一个数字，退出迭代！" << endl;
break;
}
if(iter > 0 && cost > lastCost) //去掉等于号，有些影响
{
//cout << "代价不再减小，退出迭代！" << endl;
break;
}
if(update.norm() < 1e-3)
{
//cout << "更新量的模小于1e-3，退出迭代！" << endl;
break;
}
lastCost = cost;
}//GN迭代结束
//画出光流图
char str1[10];
sprintf(str1, "%06d", INDEX);
char str2[] = ".png";
strcat(str1, str2); //将str1拼成000001.png的形式
Mat img2_show;
cvtColor(img2, img2_show, CV_GRAY2BGR);
VecVector2d projection = jaco_accu.project_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) //p_cur[0]还得小于img2_show.cols，p_cur[1]还得小于img2_show.rows呢？
{
circle(img2_show, Point2f(p_cur[0], p_cur[1]), 2, Scalar(0, 255, 0), 2);
line(img2_show, Point2f(p_ref[0], p_ref[1]), Point2f(p_cur[0], p_cur[1]), Scalar(0, 255, 0));
}
}
imshow(str1, img2_show);
waitKey(0);
memset(str1, 0, sizeof str1);
}
void JacobianAccumulator::accumulate_jacobian(const cv::Range &range)
{
const 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++)
{
//point_ref表示图像1中的路标点
Vector3d point_ref = depth_ref[i] * Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
//point_cur表示图像2中的路标点
Vector3d point_cur = T21 * point_ref;
if(point_cur[2] < 0)
continue; //此时深度无效
//u,v表示图像2中对应的角点坐标
float u = fx * point_cur[0] / point_cur[2] + cx;
float v = fy * point_cur[1] / point_cur[2] + cy;
if(u < 1 || u > img2.cols - 1 || v < 1 || v > img2.rows - 1)
continue;
projection[i] = Vector2d(u, v); //projection表示图像2中相应的角点坐标值
double X = point_cur[0], Y = point_cur[1], Z = point_cur[2];
double Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;
cnt_good++;
//计算海塞矩阵H，矩阵b和代价cost
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;
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[0] = 0.5 * (GetPixelValue(img2, u + x + 1, v + y) - GetPixelValue(img2, u + x - 1, v + y));
J_img_pixel[1] = 0.5 * (GetPixelValue(img2, u + x, v + y + 1) - GetPixelValue(img2, u + x, v + y - 1));
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)
{
unique_lock<mutex> lck(hessian_mutex); //线程锁
H += hessian; //在range外加海塞矩阵是什么意思？奇怪
b += bias;
cost += cost_tmp / cnt_good; //每次迭代成功的角点数目可能不一样，所以计算一个平均的代价
}
}
void DirectPoseEstimationMultiLayer(
const Mat& img1,
const Mat& img2,
const VecVector2d& px_ref,
const vector<double> depth_ref,
SE3d& T21
)
{
int pyramids = 4;
double pyramid_scale = 0.5;
double scales[] = {1.0, 0.5, 0.25, 0.125};
vector<Mat> pyr1, pyr2;
for(int i = 0; i < pyramids; i++)
{
if(i == 0)
{
pyr1.push_back(img1);
pyr2.push_back(img2);
}
else
{
Mat img1_pyr, img2_pyr;
resize(pyr1[i-1], img1_pyr, Size(pyr1[i-1].cols * pyramid_scale, pyr1[i-1].rows * pyramid_scale));
resize(pyr2[i-1], img2_pyr, 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;
for(int level = pyramids - 1; level >= 0; level--)
{
VecVector2d px_ref_pyr;
for(auto& px : px_ref) //这个引用不必要的吧
px_ref_pyr.push_back(scales[level] * px);
fx = 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); //depth_ref通过下标找对应关系的，不需要改变
}
}
CMakeLists.txt文件内容为，
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})
include_directories("/usr/include/eigen3")
find_package(Sophus REQUIRED)
include_directories(${Sophus_DIRECTORIES})
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_DIRECTORIES})
add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES} ${Pangolin_LIBRARIES})
程序运行结果为，
图像../000001.png相对于图像left.png的变换矩阵T21为：
0.999991 0.00248062 0.00343341 -0.003729
-0.00248817 0.999994 0.0021947 0.00304231
-0.00342795 -0.00220322 0.999992 -0.732333
0 0 0 1
图像../000002.png相对于图像left.png的变换矩阵T21为：
0.999971 0.000699416 0.00758987 -0.00249627
-0.000727685 0.999993 0.0037224 0.00401794
-0.00758721 -0.00372781 0.999964 -1.48137
0 0 0 1
图像../000003.png相对于图像left.png的变换矩阵T21为：
0.999934 0.00125148 0.011408 0.00258488
-0.00131166 0.999985 0.00526954 -0.000584668
-0.0114012 -0.00528415 0.999921 -2.24077
0 0 0 1
图像../000004.png相对于图像left.png的变换矩阵T21为：
0.999864 0.00026841 0.0165195 -0.012233
-0.000365341 0.999983 0.00586493 0.000225231
-0.0165177 -0.00587017 0.999846 -3.02747
0 0 0 1
图像../000005.png相对于图像left.png的变换矩阵T21为：
0.999785 0.00146542 0.0206771 -0.00334858
-0.00159125 0.99998 0.0060703 0.00734338
-0.0206678 -0.0061019 0.999768 -3.86175
0 0 0 1
注：T21表示的是图像left.png到当前图像的变换矩阵，即
T
C
W
T_{CW}
TCW。那么世界坐标系原点在000005.png相机坐标系中的坐标为
(
−
0.00334858
,
0.00734338
,
−
3.86175
)
T
(-0.00334858, 0.00734338, -3.86175)^T
(−0.00334858,0.00734338,−3.86175)T，因为相机在向前运动，该坐标值
Z
Z
Z值自然是负的。
扫一扫
1355
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
