视觉SLAM十四讲CH8课后习题3、4
3. 直接法能否和光流一样，提出反向法的概念？即，使用原始图像的梯度代替目标图像的梯度。
转载于：视觉SLAM十四讲（第二版）第8讲习题解答 - 知乎
代码链接：https://github.com/Philipcjh/HW-of-SLAMBOOK2/blob/main/hw8/p3.cpp
3.cpp
#include
#include
#include
#include
using namespace std;
typedef vector> VecVector2d;
// Camera intrinsics 相机内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// baseline 双目相机基线
double baseline = 0.573;
// paths 图像路径
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmt_others("../%06d.png"); // other files
// useful typedefs
typedef Eigen::Matrix Matrix6d;
typedef Eigen::Matrix Matrix26d;
typedef Eigen::Matrix Vector6d;
/// class for accumulator jacobians in parallel 用于并行计算雅可比矩阵的类
class JacobianAccumulator {
public:
//类的构造函数，使用列表进行初始化
JacobianAccumulator(
const cv::Mat &img1_,
const cv::Mat &img2_,
const VecVector2d &px_ref_,//角点坐标
const vector depth_ref_,//路标点的Z坐标值
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 range 在range范围内加速计算雅可比矩阵
void accumulate_jacobian(const cv::Range &range);
/// get hessian matrix 获取海塞矩阵
Matrix6d hessian() const { return H; }
/// get bias 获取矩阵b
Vector6d bias() const { return b; }
/// get total cost 获取总共的代价
double cost_func() const { return cost; }
/// get projected points 获取图像2中的角点坐标
VecVector2d projected_points() const { return projection; }
/// reset h, b, cost to zero 将海塞矩阵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 depth_ref;//图像1中路标点的Z坐标值
Sophus::SE3d &T21;
VecVector2d projection; // projected points
std::mutex hessian_mutex;
Matrix6d H = Matrix6d::Zero();
Vector6d b = Vector6d::Zero();
double cost = 0;
};
/**
* pose estimation using direct method
* @param img1
* @param img2
* @param px_ref
* @param depth_ref
* @param T21
*/
void DirectPoseEstimationMultiLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector depth_ref,
Sophus::SE3d &T21
);
//定义DirectPoseEstimationMultiLayer函数 多层直接法
/**
* pose estimation using direct method
* @param img1
* @param img2
* @param px_ref
* @param depth_ref
* @param T21
*/
void DirectPoseEstimationSingleLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector depth_ref,
Sophus::SE3d &T21
);
//定义DirectPoseEstimationSingleLayer函数 单层直接法
// bilinear interpolation 双线性插值求灰度值
inline float GetPixelValue(const cv::Mat &img, float x, float y) //inline表示内联函数，它是为了解决一些频繁调用的小函数大量消耗栈空间的问题而引入的
{
// boundary check
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;
//...|I1 I2|...
//...| |...
//...| |...
//...|I3 I4|...
uchar *data = &img.data[int(y) * img.step + int(x)];//x和y是整数
//data[0] -> I1 data[1] -> I2 data[img.step] -> I3 data[img.step + 1] -> I4
float xx = x - floor(x);//xx算出的是x的小数部分
float yy = y - floor(y);//yy算出的是y的小数部分
return float//最终的像素灰度值
(
(1 - xx) * (1 - yy) * data[0] +
xx * (1 - yy) * data[1] +
(1 - xx) * yy * data[img.step] +
xx * yy * data[img.step + 1]
);
}
int main(int argc, char **argv) {
cv::Mat left_img = cv::imread(left_file, 0);//0表示返回灰度图，left.png表示000000.png
cv::Mat disparity_img = cv::imread(disparity_file, 0);//0表示返回灰度图，disparity.png是left.png的视差图
// let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
//在图像1中随机选择一些像素点，然后恢复深度，得到一些路标点
cv::RNG rng;
int nPoints = 2000;
int boarder = 20;
VecVector2d pixels_ref;
vector depth_ref;
// generate pixels in ref and load depth data
for (int i = 0; i < nPoints; i++) {
int x = rng.uniform(boarder, left_img.cols - boarder); // don't pick pixels close to boarder 不要拾取靠近边界的像素
int y = rng.uniform(boarder, left_img.rows - boarder); // don't pick pixels close to boarder 不要拾取靠近边界的像素
int disparity = disparity_img.at(y, x);
double depth = fx * baseline / disparity; // you know this is disparity to depth
depth_ref.push_back(depth);
pixels_ref.push_back(Eigen::Vector2d(x, y));
}
// estimates 01~05.png's pose using this information
Sophus::SE3d T_cur_ref;
for (int i = 1; i < 6; i++)// 1~5 i从1到5，共5张图
{
cv::Mat img = cv::imread((fmt_others % i).str(), 0);//读取图片，0表示返回一张灰度图
// try single layer by uncomment this line
// DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
//利用单层直接法计算图像img相对于left_img的位姿T_cur_ref，以图片left.png为基准
DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);//调用DirectPoseEstimationMultiLayer 多层直接法
}
return 0;
}
void DirectPoseEstimationSingleLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,//第1张图中的角点坐标
const vector depth_ref,//第1张图中路标点的Z坐标值 就是深度信息
Sophus::SE3d &T21) {
const int iterations = 10;//设置迭代次数为10
double cost = 0, lastCost = 0;//将代价和最终代价初始化为0
auto t1 = chrono::steady_clock::now();//开始计时
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, std::placeholders::_1));
Matrix6d H = jaco_accu.hessian();//计算海塞矩阵
Vector6d b = jaco_accu.bias();//计算b矩阵
// solve update and put it into estimation
//求解增量方程更新优化变量T21
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 irreversible
cout << "update is nan" << endl;
break;
}
if (iter > 0 && cost > lastCost) //代价不再减小，退出迭代
{
cout << "cost increased: " << cost << ", " << lastCost << endl;
break;
}
if (update.norm() < 1e-3) //更新量的模小于1e-3，退出迭代
{
// converge
break;
}
lastCost = cost;
cout << "iteration: " << iter << ", cost: " << cost << endl;
}//GN(高斯牛顿法)迭代结束
cout << "T21 = \n" << T21.matrix() << endl;//输出T21矩阵
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 here
cv::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) {
// parameters
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++) {
// compute the projection in the second image //point_ref表示图像1中的路标点
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;//point_cur表示图像2中的路标点
if (point_cur[2] < 0) // depth invalid
continue;
//u,v表示图像2中对应的角点坐标
float u = fx * point_cur[0] / point_cur[2] + cx, v = fy * point_cur[1] / point_cur[2] + cy;//视觉slam十四讲p99式5.5
// u = fx * X / Z + cx v = fy * Y / Z + cy X -> point_cur[0] Y -> point_cur[1] Z -> point_cur[2]
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);//projection表示图像2中相应的角点坐标值
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;// Z2_inv = (1 / (Z * Z))
cnt_good++;
// and compute error and jacobian 计算海塞矩阵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++) {
//ei = I1(p1,i) - I(p2,i)其中p1，p2空间点P在两个时刻的像素位置坐标
double error = GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) -
GetPixelValue(img2, u + x, v + y);//视觉slam十四讲p219式8.13
Matrix26d J_pixel_xi;
Eigen::Vector2d J_img_pixel;
//视觉slam十四讲p220式8.18
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;
// -fx * inv_z 相当于-fx / Z
//0
// -fx * X * Z2_inv相当于fx * X / ( Z * Z )
//--fx * X * Y * Z2_inv相当于fx * X * Y / ( Z * Z) +fx
//fx + fx * X * X * Z2_inv相当于fx * X * X / (Z * Z)
//-fx * Y * Z_inv相当于 fx * Y / Z
//0
//fy * Z_inv相当于-fy / Z
//-fy * Y * Z2_inv相当于fy * Y / (Z * Z)
//-fy - fy * Y * Y * Z2_inv相当于fy + fy * Y * Y / (Z * Z)
//fy * X * Y * Z2_inv相当于fy * X * Y / ( Z * Z)
//fy * X * Z_inv相当于-fy * X / Z
//使用img1的梯度作为第二个图像的梯度
J_img_pixel = Eigen::Vector2d(
0.5 * (GetPixelValue(img1, px_ref[i][0] + 1 + x, px_ref[i][1] + y) - GetPixelValue(img1, px_ref[i][0] - 1 + x, px_ref[i][1] + y)),
0.5 * (GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + 1 + y) - GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] - 1 + y))
);
// total jacobian
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 cost
unique_lock lck(hessian_mutex);
H += hessian;//H = Jij Jij(T)(累加和)
b += bias;//b = -Jij * eij(累加和)
cost += cost_tmp / cnt_good;//cost = || eij ||2 2范数
}
}
void DirectPoseEstimationMultiLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector depth_ref,
Sophus::SE3d &T21) {
// parameters
int pyramids = 4;//金字塔层数为4
double pyramid_scale = 0.5;//每层之间的缩放因子设为0.5
double scales[] = {1.0, 0.5, 0.25, 0.125};
// create pyramids 创建图像金字塔
vector pyr1, pyr2; // image pyramids pyr1 -> 图像1的金字塔 pyr2 -> 图像2的金字塔
for (int i = 0; i < pyramids; i++) {
if (i == 0) {
pyr1.push_back(img1);
pyr2.push_back(img2);
} else {
cv::Mat img1_pyr, img2_pyr;
//将图像pyr1[i-1]的宽和高各缩放0.5倍得到图像img1_pyr
cv::resize(pyr1[i - 1], img1_pyr,
cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
//将图像pyr2[i-1]的宽和高各缩放0.5倍得到图像img2_pyr
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 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);
}
// scale fx, fy, cx, cy in different pyramid levels 在不同的金字塔级别缩放 fx, fy, cx, cy
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);
}
}
修改了304-308行
CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(ch8)
set(CMAKE_BUILD_TYPE "Release")
add_definitions("-DENABLE_SSE")
set(CMAKE_CXX_FLAGS "-std=c++14 ${SSE_FLAGS} -g -O3 -march=native")
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
find_package(OpenCV 3 REQUIRED)
find_package(G2O REQUIRED)
find_package(Sophus REQUIRED)
find_package(Pangolin REQUIRED)
find_package(Ceres REQUIRED)
include_directories(
${OpenCV_INCLUDE_DIRS}
${G2O_INCLUDE_DIRS}
${Sophus_INCLUDE_DIRS}
"/usr/include/eigen3/"
${Pangolin_INCLUDE_DIRS}
)
add_executable(optical_flow optical_flow.cpp)
target_link_libraries(optical_flow ${OpenCV_LIBS} fmt)
add_executable(direct_method direct_method.cpp)
target_link_libraries(direct_method ${OpenCV_LIBS} ${Pangolin_LIBRARIES} fmt)
add_executable(3 3.cpp)
target_link_libraries(3 ${OpenCV_LIBS} ${Pangolin_LIBRARIES} fmt)
执行结果：
./3
iteration: 0, cost: 2.86149e+06
iteration: 1, cost: 2.08712e+06
iteration: 2, cost: 1.37955e+06
iteration: 3, cost: 1.03774e+06
iteration: 4, cost: 597119
iteration: 5, cost: 327648
cost increased: 341816, 327648
T21 =
0.999991 0.0023364 0.00354983 -0.002555
-0.00234482 0.999994 0.00236884 0.00649079
-0.00354428 -0.00237714 0.999991 -0.727878
0 0 0 1
direct method for single layer: 0.00489005
iteration: 0, cost: 342675
cost increased: 343565, 342675
T21 =
0.999991 0.00243631 0.00334714 0.00142947
-0.00244436 0.999994 0.00240357 0.00325864
-0.00334127 -0.00241173 0.999992 -0.727735
0 0 0 1
direct method for single layer: 0.00160022
iteration: 0, cost: 451922
iteration: 1, cost: 441333
T21 =
0.999991 0.00236629 0.00338819 -0.0021416
-0.00237398 0.999995 0.00226632 0.00364657
-0.00338281 -0.00227434 0.999992 -0.733249
0 0 0 1
direct method for single layer: 0.00184883
iteration: 0, cost: 660625
cost increased: 668423, 660625
T21 =
0.999991 0.00250435 0.00336064 -0.0041072
-0.0025113 0.999995 0.0020639 0.00558456
-0.00335546 -0.00207232 0.999992 -0.733365
0 0 0 1
direct method for single layer: 0.00127398
iteration: 0, cost: 2.52008e+06
iteration: 1, cost: 1.49944e+06
iteration: 2, cost: 777227
iteration: 3, cost: 478373
cost increased: 478961, 478373
T21 =
0.99997 0.00129416 0.00764082 0.00628962
-0.00132497 0.999991 0.00402877 0.0141763
-0.00763554 -0.00403877 0.999963 -1.4557
0 0 0 1
direct method for single layer: 0.00175881
iteration: 0, cost: 686177
iteration: 1, cost: 685957
T21 =
0.999972 0.00118453 0.00741478 0.0107649
-0.00121283 0.999992 0.00381399 0.0093214
-0.0074102 -0.00382287 0.999965 -1.46144
0 0 0 1
direct method for single layer: 0.00202454
iteration: 0, cost: 850468
iteration: 1, cost: 796197
cost increased: 796891, 796197
T21 =
0.999971 0.00095928 0.00756889 0.00200262
-0.000988383 0.999992 0.00384216 0.00349609
-0.00756515 -0.00384952 0.999964 -1.47126
0 0 0 1
direct method for single layer: 0.00201398
iteration: 0, cost: 949270
iteration: 1, cost: 915048
T21 =
0.999971 0.00112343 0.00753328 -0.000895596
-0.00114966 0.999993 0.00347804 0.00785306
-0.00752932 -0.0034866 0.999966 -1.47225
0 0 0 1
direct method for single layer: 0.00147911
iteration: 0, cost: 2.47905e+06
iteration: 1, cost: 1.77353e+06
iteration: 2, cost: 1.40927e+06
iteration: 3, cost: 984942
iteration: 4, cost: 739113
cost increased: 744432, 739113
T21 =
0.999937 0.00246451 0.0109758 0.0336468
-0.00251487 0.999986 0.00457645 0.0344471
-0.0109644 -0.00460376 0.999929 -2.18706
0 0 0 1
direct method for single layer: 0.00177077
iteration: 0, cost: 943157
iteration: 1, cost: 854687
iteration: 2, cost: 848529
iteration: 3, cost: 835947
T21 =
0.999931 0.00202284 0.0115525 0.0128386
-0.00208159 0.999985 0.00507563 0.00902602
-0.011542 -0.00509933 0.99992 -2.21939
0 0 0 1
direct method for single layer: 0.00225068
iteration: 0, cost: 1.11097e+06
iteration: 1, cost: 1.11012e+06
iteration: 2, cost: 1.07022e+06
cost increased: 1.07269e+06, 1.07022e+06
T21 =
0.999935 0.00125359 0.0113476 0.0103366
-0.00131455 0.999985 0.00536675 -0.00138986
-0.0113407 -0.00538131 0.999921 -2.2338
0 0 0 1
direct method for single layer: 0.00237182
iteration: 0, cost: 1.50139e+06
iteration: 1, cost: 1.48247e+06
iteration: 2, cost: 1.47339e+06
T21 =
0.999934 0.000959383 0.0114124 0.00246279
-0.00101733 0.999987 0.00507315 0.00150249
-0.0114074 -0.00508443 0.999922 -2.23186
0 0 0 1
direct method for single layer: 0.0023685
iteration: 0, cost: 2.67719e+06
iteration: 1, cost: 2.29275e+06
iteration: 2, cost: 1.87409e+06
iteration: 3, cost: 1.65528e+06
iteration: 4, cost: 1.29541e+06
iteration: 5, cost: 1.01256e+06
iteration: 6, cost: 951194
iteration: 7, cost: 932621
cost increased: 934587, 932621
T21 =
0.999875 0.00157317 0.015725 0.0432911
-0.00165172 0.999986 0.00498354 0.0425205
-0.015717 -0.00500889 0.999864 -2.97182
0 0 0 1
direct method for single layer: 0.00303314
iteration: 0, cost: 1.13893e+06
iteration: 1, cost: 1.07046e+06
iteration: 2, cost: 1.0661e+06
cost increased: 1.16521e+06, 1.0661e+06
T21 =
0.999865 0.00110888 0.0163991 0.0147089
-0.00120959 0.99998 0.00613222 0.0021747
-0.016392 -0.00615122 0.999847 -3.00355
0 0 0 1
direct method for single layer: 0.00189114
iteration: 0, cost: 1.55327e+06
iteration: 1, cost: 1.44202e+06
cost increased: 1.47772e+06, 1.44202e+06
T21 =
0.99987 0.000134074 0.0161495 0.0137016
-0.00023695 0.99998 0.00636851 -0.00917049
-0.0161483 -0.0063715 0.999849 -3.0156
0 0 0 1
direct method for single layer: 0.00130465
T21 =
0.999871 -1.62381e-05 0.0160666 0.0136775
-8.15908e-05 0.999981 0.00608828 -0.00758035
-0.0160664 -0.0060888 0.999852 -3.01571
0 0 0 1
direct method for single layer: 0.000790047
iteration: 0, cost: 2.92045e+06
iteration: 1, cost: 2.45634e+06
iteration: 2, cost: 2.01145e+06
iteration: 3, cost: 1.69543e+06
iteration: 4, cost: 1.41094e+06
iteration: 5, cost: 1.24263e+06
iteration: 6, cost: 1.2027e+06
cost increased: 1.25957e+06, 1.2027e+06
T21 =
0.999787 0.00277 0.0204755 0.0329885
-0.00289656 0.999977 0.0061539 0.0238932
-0.020458 -0.0062119 0.999771 -3.77353
0 0 0 1
direct method for single layer: 0.00266324
iteration: 0, cost: 1.68149e+06
cost increased: 1.71809e+06, 1.68149e+06
T21 =
0.999782 0.00266666 0.0207137 0.020217
-0.00280174 0.999975 0.00649522 0.0024795
-0.0206958 -0.00655184 0.999764 -3.80522
0 0 0 1
direct method for single layer: 0.000890887
iteration: 0, cost: 2.31523e+06
cost increased: 2.35139e+06, 2.31523e+06
T21 =
0.999787 0.00142755 0.0206015 0.0161849
-0.0015643 0.999977 0.00662294 -0.00592936
-0.0205916 -0.00665375 0.999766 -3.82221
0 0 0 1
direct method for single layer: 0.00136992
iteration: 0, cost: 3.01526e+06
cost increased: 3.08278e+06, 3.01526e+06
T21 =
0.999796 0.000785643 0.020185 0.0200326
-0.000914921 0.999979 0.00639622 -0.00707919
-0.0201796 -0.00641339 0.999776 -3.82512
0 0 0 1
direct method for single layer: 0.00114946
./4_Ceres_direct_method
4. *使用Ceres 实现RGB-D 上的稀疏直接法和半稠密直接法。
代码链接：HW-of-SLAMBOOK2/p4_Ceres_direct_method.cpp at main · Philipcjh/HW-of-SLAMBOOK2 · GitHub
4_Ceres_direct_method.cpp
CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(ch8)
set(CMAKE_BUILD_TYPE "Release")
add_definitions("-DENABLE_SSE")
set(CMAKE_CXX_FLAGS "-std=c++14 ${SSE_FLAGS} -g -O3 -march=native")
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
find_package(OpenCV 3 REQUIRED)
find_package(G2O REQUIRED)
find_package(Sophus REQUIRED)
find_package(Pangolin REQUIRED)
find_package(Ceres REQUIRED)
include_directories(
${OpenCV_INCLUDE_DIRS}
${G2O_INCLUDE_DIRS}
${Sophus_INCLUDE_DIRS}
"/usr/include/eigen3/"
${Pangolin_INCLUDE_DIRS}
)
add_executable(optical_flow optical_flow.cpp)
target_link_libraries(optical_flow ${OpenCV_LIBS} fmt)
add_executable(direct_method direct_method.cpp)
target_link_libraries(direct_method ${OpenCV_LIBS} ${Pangolin_LIBRARIES} fmt
add_executable(3 3.cpp)
target_link_libraries(3 ${OpenCV_LIBS} ${Pangolin_LIBRARIES} fmt)
add_executable(4_Ceres_direct_method 4_Ceres_direct_method.cpp)
target_link_libraries(4_Ceres_direct_method ${OpenCV_LIBS} ${Pangolin_LIBRARIES} fmt)
add_executable(4_g2o_direct_method 4_g2o_direct_method.cpp)
target_link_libraries(4_g2o_direct_method ${OpenCV_LIBS} ${Pangolin_LIBRARIES} ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} fmt)
执行结果：
./4_Ceres_direct_method
现在是第1次
pyramid4 estimated pose:
0.999382 0.00911183 0.0339491 5.59768
-0.00926196 0.999948 0.0042674 0.137403
-0.0339084 -0.0045792 0.999414 -0.200338
0 0 0 1
solve pnp in ceres cost time: 0.025189 seconds.
pyramid3 estimated pose:
0.999286 0.00739127 0.0370391 5.54364
-0.00765113 0.999947 0.00687917 0.108043
-0.0369863 -0.00715766 0.99929 -0.247813
0 0 0 1
solve pnp in ceres cost time: 0.0104756 seconds.
pyramid2 estimated pose:
0.999255 -0.000342525 0.0386028 5.51109
-0.000313791 0.999856 0.0169944 -0.020398
-0.0386031 -0.0169939 0.99911 -0.290581
0 0 0 1
solve pnp in ceres cost time: 0.0302933 seconds.
pyramid1 estimated pose:
0.999222 -0.00219948 0.0393859 5.50712
0.00147464 0.999829 0.0184231 -0.0306487
-0.0394197 -0.0183507 0.999054 -0.302461
0 0 0 1
solve pnp in ceres cost time: 0.0124609 seconds.
现在是第2次
pyramid4 estimated pose:
0.998943 0.0057762 0.0456099 5.34197
-0.00668645 0.999781 0.0198299 -0.141361
-0.0454854 -0.0201139 0.998762 -0.502782
0 0 0 1
solve pnp in ceres cost time: 0.0191581 seconds.
pyramid3 estimated pose:
0.999183 0.00207822 0.0403556 5.4384
-0.00295171 0.999762 0.0215973 -0.154193
-0.0403011 -0.0216988 0.998952 -0.414756
0 0 0 1
solve pnp in ceres cost time: 0.012968 seconds.
pyramid2 estimated pose:
0.999206 0.00184718 0.0397897 5.4578
-0.00285976 0.999673 0.0254063 -0.216111
-0.0397298 -0.0254999 0.998885 -0.349924
0 0 0 1
solve pnp in ceres cost time: 0.0216397 seconds.
pyramid1 estimated pose:
0.999112 -0.00323179 0.0420031 5.41138
0.00207568 0.999619 0.0275388 -0.228945
-0.0420761 -0.0274272 0.998738 -0.358674
0 0 0 1
solve pnp in ceres cost time: 0.0205383 seconds.
现在是第3次
pyramid4 estimated pose:
0.998794 0.0013606 0.0490733 5.33059
-0.00316672 0.99932 0.0367457 -0.361604
-0.0489899 -0.0368568 0.998119 -0.72132
0 0 0 1
solve pnp in ceres cost time: 0.0140347 seconds.
pyramid3 estimated pose:
0.998791 0.0012295 0.0491401 5.3318
-0.00304321 0.999316 0.0368512 -0.360116
-0.0490612 -0.0369562 0.998112 -0.724425
0 0 0 1
solve pnp in ceres cost time: 0.00731537 seconds.
pyramid2 estimated pose:
0.998809 -0.00724531 0.0482591 5.33354
0.00510852 0.999007 0.0442545 -0.407529
-0.0485319 -0.0439552 0.997854 -0.75308
0 0 0 1
solve pnp in ceres cost time: 0.0177588 seconds.
pyramid1 estimated pose:
0.998753 -0.00780578 0.0493004 5.33131
0.00558935 0.998974 0.0449367 -0.411232
-0.0496006 -0.0446052 0.997773 -0.782714
0 0 0 1
solve pnp in ceres cost time: 0.0177376 seconds.
现在是第4次
pyramid4 estimated pose:
0.999865 -0.000516847 0.016415 6.09388
-0.000396244 0.998455 0.0555733 -0.629663
-0.0164184 -0.0555723 0.99832 -0.962444
0 0 0 1
solve pnp in ceres cost time: 0.0340787 seconds.
pyramid3 estimated pose:
0.999924 0.00178144 0.0122282 6.20748
-0.00249587 0.998275 0.0586606 -0.652386
-0.0121026 -0.0586866 0.998203 -0.891922
0 0 0 1
solve pnp in ceres cost time: 0.0310313 seconds.
pyramid2 estimated pose:
0.99992 0.00110374 0.0126243 6.22829
-0.00185469 0.998219 0.0596283 -0.661132
-0.012536 -0.0596469 0.998141 -0.847063
0 0 0 1
solve pnp in ceres cost time: 0.0107109 seconds.
pyramid1 estimated pose:
0.9999 0.000492337 0.0141658 6.21035
-0.00137192 0.998066 0.0621494 -0.687954
-0.0141078 -0.0621626 0.997966 -0.863831
0 0 0 1
solve pnp in ceres cost time: 0.0170076 seconds.
现在是第5次
pyramid4 estimated pose:
0.999175 -0.0293797 -0.0280545 6.7862
0.0312999 0.99701 0.0706554 -0.621938
0.0258947 -0.0714752 0.997106 0.175179
0 0 0 1
solve pnp in ceres cost time: 0.0298381 seconds.
pyramid3 estimated pose:
0.999266 -0.0276807 -0.0264845 6.75746
0.0295016 0.997038 0.071032 -0.627612
0.0244398 -0.0717612 0.997122 0.139694
0 0 0 1
solve pnp in ceres cost time: 0.00851057 seconds.
pyramid2 estimated pose:
0.999443 -0.0227171 -0.0244572 6.75201
0.024353 0.997332 0.0688121 -0.595792
0.0228287 -0.0693693 0.99733 0.125618
0 0 0 1
solve pnp in ceres cost time: 0.0231142 seconds.
pyramid1 estimated pose:
0.999401 -0.0220068 -0.0267121 6.81053
0.023914 0.997022 0.073315 -0.673161
0.0250191 -0.0739098 0.996951 0.145889
0 0 0 1
solve pnp in ceres cost time: 0.0242198 seconds.
代码链接：HW-of-SLAMBOOK2/p3.cpp at main · Philipcjh/HW-of-SLAMBOOK2 · GitHub
4_g2o_direct_method.cpp
#include
#include
#include //Eigen核心模块
#include //g2o顶点（Vertex）头文件 视觉slam十四讲p141用顶点表示优化变量，用边表示误差项
#include //g2o边（edge）头文件
#include //求解器头文件
#include //列文伯格——马尔夸特算法头文件
#include //高斯牛顿算法头文件
#include //dogleg算法头文件
#include //稠密矩阵求解
#include
#include
#include
#include
#include
using namespace std;
using namespace cv;
// Camera intrinsics 相机内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// baseline 双目相机基线
double baseline = 0.573;
// paths 图像路径
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmt_others("../%06d.png"); // other files
// g2o
typedef vector> VecVector2d;
typedef Eigen::Matrix Matrix26d;
/**
* pose estimation using direct method
* @param img1
* @param img2
* @param px_ref
* @param depth_ref
* @param T
*/
void DirectPoseEstimationMultiLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector depth_ref,
Sophus::SE3d &T
);//定义DirectPoseEstimationMultiLayer函数 多层直接法
/**
* pose estimation using direct method
* @param img1
* @param img2
* @param px_ref
* @param depth_ref
* @param T
*/
void DirectPoseEstimationSingleLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector depth_ref,
Sophus::SE3d &T
);//定义DirectPoseEstimationSingleLayer函数 单层直接法
// bilinear interpolation 双线性插值求灰度值
inline double GetPixelValue(const cv::Mat &img, float x, float y) {
// boundary check
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;
//...|I1 I2|...
//...| |...
//...| |...
//...|I3 I4|...
uchar *data = &img.data[int(y) * img.step + int(x)];//x和y是整数
//data[0] -> I1 data[1] -> I2 data[img.step] -> I3 data[img.step + 1] -> I4
float xx = x - floor(x);//xx算出的是x的小数部分
float yy = y - floor(y);//yy算出的是y的小数部分
return double//最终的像素灰度值
(
(1 - xx) * (1 - yy) * data[0] +
xx * (1 - yy) * data[1] +
(1 - xx) * yy * data[img.step] +
xx * yy * data[img.step + 1]
);
}
/// vertex and edges used in g2o ba
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d>
//:表示继承，public表示公有继承；CurveFittingVertex是派生类，BaseVertex<3, Eigen::Vector4d>是基类
{
public://以下定义的成员变量和成员函数都是公有的
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
// 重置
virtual void setToOriginImpl() override //virtual表示该函数为虚函数，override保留字表示当前函数重写了基类的虚函数
{
_estimate = Sophus::SE3d();
}
/// left multiplication on SE3
virtual void oplusImpl(const double *update) override {
Eigen::Matrix update_eigen;
update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
_estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
}
// 存盘和读盘：留空
virtual bool read(istream &in) {}//istream类是c++标准输入流的一个基类
//可参照C++ Primer Plus第六版的6.8节
virtual bool write(ostream &out) const {}//ostream类是c++标准输出流的一个基类
//可参照C++ Primer Plus第六版的6.8节
};
//1元边，测量值维度是1,对应测量值类型为灰度差，顶点对应的数据类型都是VertexPose
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class EdgeProjection : public g2o::BaseUnaryEdge<1, double, VertexPose> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//解决Eigen库数据结构内存对齐问题
EdgeProjection(const Eigen::Vector2d &px_ref, const double depth_ref, const Mat img1, const Mat img2)
: _px_ref(px_ref), _depth_ref(depth_ref), _img1(img1), _img2(img2) {}
virtual void computeError() override // 计算曲线模型误差
{
const VertexPose *v = static_cast (_vertices[0]);
Sophus::SE3d T = v->estimate();
Eigen::Vector3d point_ref =_depth_ref * Eigen::Vector3d((_px_ref[0] - cx) / fx, (_px_ref[1] - cy) / fy, 1);
Eigen::Vector3d point_cur = T * point_ref;
double u1 = fx * point_cur[0] / point_cur[2] + cx, v1 = fy * point_cur[1] / point_cur[2] + cy;
_error(0, 0) = _measurement - GetPixelValue(_img2, u1, v1);
}
virtual void linearizeOplus() override {
const VertexPose *v = static_cast (_vertices[0]);
Sophus::SE3d T = v->estimate();
Eigen::Vector3d point_ref =_depth_ref * Eigen::Vector3d((_px_ref[0] - cx) / fx, (_px_ref[1] - cy) / fy, 1);
Eigen::Vector3d point_cur = T * point_ref;
double u1 = fx * point_cur[0] / point_cur[2] + cx, v1 = fy * point_cur[1] / point_cur[2] + cy;
Eigen::Matrix J;
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;
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;
// -fx * inv_z 相当于-fx / Z
//0
// -fx * X * Z2_inv相当于fx * X / ( Z * Z )
//--fx * X * Y * Z2_inv相当于fx * X * Y / ( Z * Z) +fx
//fx + fx * X * X * Z2_inv相当于fx * X * X / (Z * Z)
//-fx * Y * Z_inv相当于 fx * Y / Z
//0
//fy * Z_inv相当于-fy / Z
//-fy * Y * Z2_inv相当于fy * Y / (Z * Z)
//-fy - fy * Y * Y * Z2_inv相当于fy + fy * Y * Y / (Z * Z)
//fy * X * Y * Z2_inv相当于fy * X * Y / ( Z * Z)
//fy * X * Z_inv相当于-fy * X / Z
J_img_pixel = Eigen::Vector2d(
0.5 * (GetPixelValue(_img2, u1 + 1 , v1 ) - GetPixelValue(_img2, u1 - 1 , v1 )),
0.5 * (GetPixelValue(_img2, u1 , v1 + 1 ) - GetPixelValue(_img2, u1 , v1 - 1 ))
);
// total jacobian
J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose();
_jacobianOplusXi << J[0], J[1], J[2], J[3], J[4], J[5];
}
virtual bool read(istream &in) override {}
virtual bool write(ostream &out) const override {}
private:
const Eigen::Vector2d _px_ref;
const double _depth_ref;
const Mat _img1;
const Mat _img2;
};
int main(int argc, char **argv) {
cv::Mat left_img = cv::imread(left_file, 0);
cv::Mat disparity_img = cv::imread(disparity_file, 0);
// let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
cv::RNG rng;
int nPoints = 2000;
int boarder = 20;
VecVector2d pixels_ref;
vector depth_ref;
// generate pixels in ref and load depth data
for (int i = 0; i < nPoints; i++) {
int x = rng.uniform(boarder, left_img.cols - boarder); // don't pick pixels close to boarder
int y = rng.uniform(boarder, left_img.rows - boarder); // don't pick pixels close to boarder
int disparity = disparity_img.at(y, x);
double depth = fx * baseline / disparity; // you know this is disparity to depth
depth_ref.push_back(depth);
pixels_ref.push_back(Eigen::Vector2d(x, y));
}
Sophus::SE3d T;
for (int i = 1; i < 6; i++) { // 1~10
cv::Mat img = cv::imread((fmt_others % i).str(), 0);
// try single layer by uncomment this line
// DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
cout<< "现在是第" << i <<"次"< depth_ref,
Sophus::SE3d &T
){
// 构建图优化，先设定g2o
typedef g2o::BlockSolver> BlockSolverType;
typedef g2o::LinearSolverDense LinearSolverType; // 线性求解器类型
// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique(g2o::make_unique()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose
vertex_pose->setId(0);
vertex_pose->setEstimate(T);
optimizer.addVertex(vertex_pose);
// edges
int index = 1;
//新增部分：第一个相机作为顶点连接的边
for (size_t i = 0; i < px_ref.size(); ++i) {
EdgeProjection *edge = new EdgeProjection(px_ref[i], depth_ref[i], img1, img2);
edge->setId(index);
edge->setVertex(0, vertex_pose);
edge->setMeasurement(GetPixelValue(img1, px_ref[i][0], px_ref[i][1]));
edge->setInformation(Eigen::Matrix::Identity());
optimizer.addEdge(edge);
index++;
}
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.setVerbose(false);
optimizer.initializeOptimization();
optimizer.optimize(10);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration time_used = chrono::duration_cast>(t2 - t1);
cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
cout << "pose estimated =\n" << vertex_pose->estimate().matrix() << endl;
T = vertex_pose->estimate();
}
void DirectPoseEstimationMultiLayer(
const cv::Mat &img1,
const cv::Mat &img2,
const VecVector2d &px_ref,
const vector depth_ref,
Sophus::SE3d &T
){
// parameters
int pyramids = 4;
double pyramid_scale = 0.5;
double scales[] = {1.0, 0.5, 0.25, 0.125};
// create pyramids
vector pyr1, pyr2; // image pyramids
for (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 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);
}
// 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<<"pyramid"<< level+1 <<'\t';
DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T);
}
}
CMakeLists.txt
和上面一样
执行结果：
./4_g2o_direct_method 现在是第1次
pyramid4 optimization costs time: 0.00353299 seconds.
pose estimated =
0.999982 0.00319095 0.00508459 -0.0372312
-0.00321305 0.999985 0.00434374 -0.0374348
-0.00507065 -0.00436 0.999978 -0.761971
0 0 0 1
pyramid3 optimization costs time: 0.00272612 seconds.
pose estimated =
0.999982 0.0034094 0.00486825 -0.0272534
-0.00342813 0.999987 0.00384421 -0.0291595
-0.00485508 -0.00386083 0.999981 -0.768783
0 0 0 1
pyramid2 optimization costs time: 0.00287789 seconds.
pose estimated =
0.999987 0.00268009 0.00430231 -0.0146829
-0.00269327 0.999992 0.00306066 -0.0119225
-0.00429407 -0.00307221 0.999986 -0.769611
0 0 0 1
pyramid1 optimization costs time: 0.00319735 seconds.
pose estimated =
0.99999 0.00254965 0.0035665 -0.00480804
-0.00255883 0.999993 0.00257074 -0.00450588
-0.00355992 -0.00257984 0.99999 -0.737699
0 0 0 1
现在是第2次
pyramid4 optimization costs time: 0.00273482 seconds.
pose estimated =
0.999955 0.000844769 0.00946332 -0.0417899
-0.000893159 0.999987 0.00511037 -0.00478078
-0.00945887 -0.00511859 0.999942 -1.48213
0 0 0 1
pyramid3 optimization costs time: 0.00271277 seconds.
pose estimated =
0.999965 9.85214e-05 0.00838077 -0.0188058
-0.000135255 0.99999 0.00438261 0.00719823
-0.00838026 -0.00438359 0.999955 -1.48619
0 0 0 1
pyramid2 optimization costs time: 0.00287543 seconds.
pose estimated =
0.999965 -0.000145758 0.00833601 -0.0185346
0.000107828 0.99999 0.00455041 -0.00698478
-0.00833659 -0.00454935 0.999955 -1.49695
0 0 0 1
pyramid1 optimization costs time: 0.00314978 seconds.
pose estimated =
0.99997 0.000866501 0.00769146 -0.00101027
-0.000896248 0.999992 0.0038649 -0.000850383
-0.00768805 -0.00387168 0.999963 -1.4868
0 0 0 1
现在是第3次
pyramid4 optimization costs time: 0.00273684 seconds.
pose estimated =
0.999905 -0.0035254 0.0133267 -0.0359808
0.00339051 0.999943 0.0101306 -0.0795221
-0.0133617 -0.0100845 0.99986 -2.2378
0 0 0 1
pyramid3 optimization costs time: 0.00275158 seconds.
pose estimated =
0.999929 0.000808264 0.011911 0.012925
-0.00089782 0.999971 0.00751533 -0.0442718
-0.0119046 -0.00752549 0.999901 -2.24238
0 0 0 1
pyramid2 optimization costs time: 0.0028557 seconds.
pose estimated =
0.999934 0.00200382 0.0113374 0.0192389
-0.00207184 0.99998 0.00599133 -0.0188404
-0.0113252 -0.00601443 0.999918 -2.25206
0 0 0 1
pyramid1 optimization costs time: 0.00306763 seconds.
pose estimated =
0.999937 0.00174512 0.0111324 0.0191879
-0.0018071 0.999983 0.0055593 -0.0100468
-0.0111225 -0.00557907 0.999923 -2.25681
0 0 0 1
现在是第4次
pyramid4 optimization costs time: 0.00270052 seconds.
pose estimated =
0.999778 -0.0025194 0.0209372 -0.140756
0.00220626 0.999886 0.0149658 -0.195176
-0.0209725 -0.0149163 0.999669 -2.9547
0 0 0 1
pyramid3 optimization costs time: 0.00274797 seconds.
pose estimated =
0.999782 0.00017769 0.0208877 -0.150334
-0.00045343 0.999913 0.0131971 -0.184953
-0.0208836 -0.0132036 0.999695 -2.9678
0 0 0 1
pyramid2 optimization costs time: 0.00282441 seconds.
pose estimated =
0.999793 0.00134962 0.0202893 -0.145184
-0.00156259 0.999944 0.0104842 -0.132602
-0.0202741 -0.0105138 0.999739 -2.94987
0 0 0 1
pyramid1 optimization costs time: 0.00333092 seconds.
pose estimated =
0.999835 0.00406482 0.0177078 -0.102805
-0.00419932 0.999963 0.00756504 -0.0867595
-0.0176764 -0.00763815 0.999815 -2.91587
0 0 0 1
现在是第5次
pyramid4 optimization costs time: 0.00260575 seconds.
pose estimated =
0.999626 0.00298751 0.0272015 -0.255176
-0.00316725 0.999973 0.0065672 0.0199082
-0.0271811 -0.00665089 0.999608 -3.55576
0 0 0 1
pyramid3 optimization costs time: 0.00254393 seconds.
pose estimated =
0.999722 0.00331044 0.0233564 -0.139835
-0.00347965 0.999968 0.00720792 -0.0175263
-0.0233318 -0.00728719 0.999701 -3.72899
0 0 0 1
pyramid2 optimization costs time: 0.00267551 seconds.
pose estimated =
0.999723 0.00315426 0.0233322 -0.141733
-0.00329156 0.999977 0.00584849 0.00850978
-0.0233132 -0.00592367 0.999711 -3.72919
0 0 0 1
pyramid1 optimization costs time: 0.00294475 seconds.
pose estimated =
0.999733 0.00216679 0.0230021 -0.119127
-0.00230165 0.99998 0.00583817 0.0175009
-0.022989 -0.00588955 0.999718 -3.75662
0 0 0 1