#include #include #include #include #include #include #include "sophus/se3.h"
using namespace std;
using namespace eigen;
using namespace cv;
typedef vector> vecvector3d;
typedef vector> vecvector2d;
typedef matrixvector6d;
double fx = 520.9, fy = 521.0, cx = 325.1, cy = 249.7;
templateeigen::matrixrpy2mat(t roll, t pitch, t yaw)
void getpt3d(vecvector3d& p3d, int size)
}void outputpt3d(const vecvector3d & p3ds)
}void getuv(vecvector2d & p2ds,
vecvector3d& p3d,
sophus::se3 & se3_rt);
int main()
; //vectorposes = ;
// 2 生成3d 座標點
const int point_size = 20;
vecvector3d p3d;
getpt3d(p3d, point_size);
// 3 生成畫素座標點
vectorpt2ds(poses.size());
for(int i = 0; i < poses.size(); i++)
//outputpt3d(p3d);
//對座標點和誤差加 雜訊！
for(int i = 0; i < point_size; i++)
//eigen::matrix3d r4 = eigen::angleaxisd(m_pi/4, eigen::vector3d(0.01, 0.0, 0.81)).torotationmatrix();
//eigen::matrix3d r4 = rpy2mat(0.15, 0.10, 0.0);
//eigen::vector3d t4(1, 0, 0);
//sophus::se3 se3_rt4(r4, t4);
//poses[0] = se3_rt4;
//outputpt3d(p3d);
//高斯牛頓迭代
int iterations = 10;
// jij 的維度為 2 * (cols)
// h 的維度為 (cols) * (cols)
// b 的維度為 cols
int cols = poses.size() * 6 + p3d.size() * 3;
std::cout << "cols is " << cols << std::endl;
double lastcost = 0;
double cost = 0;
const int pose_size = poses.size();
for (int iter = 0; iter < iterations; iter++)
;vector2d e = p2 - p2_; //誤差error = 觀測值 - 估計值
cost += (e[0]*e[0]+e[1]*e[1]);
matrixxd j = eigen::matrixxd::zero(2, cols);
// 和位姿相關的塊
// 第乙個位姿 要固定
if(i != 0)
// 和 3d 座標相關的塊
eigen::matrixk;
k << -(fx / z), 0, (fx *x / (z*z)), 0, -(fy / z), fy * y / (z*z);
matrix3d r = poses[i].rotation_matrix();
j.block(0, 6 * pose_size + 3 * j, 2,3) = k * r;
h += j.transpose() * j;
b += -j.transpose() * e;
}
}// solve dx
vectorxd dx(cols);
dx = h.ldlt().solve(b);
if (std::isnan(dx[0]))
// 更新 解向量
// 先更新位姿
//std::cout << " dx " << dx << std::endl;
for(int i = 1; i < poses.size(); i++)
int startidx = poses.size() * 6;
// 再更新 座標點
for(int j = 0; j < p3d.size(); j++)
lastcost = cost;
cout << "iteration " << iter << " cost=" << cost << endl;
} // todo: 輸出結果
return 0;
}// p2ds 是觀察到的畫素點, 得儲存對應的3d點的資訊才行
// 記錄每個 p3d 的所有觀測資料
// p3d.size()== p2ds.size()
void getuv(vecvector2d & p2ds,
vecvector3d& p3d,
sophus::se3 & se3_rt)
;
p2ds.push_back(p2_);
}return ;
}
Bundle Adjustment光束平差法概述
給出從不同視角拍攝的，描述同乙個場景的一系列，bundle adjustment可以根據所有點在影象中的投影作為標準，同時提煉出描述場景結構的3d點座標 相對運動引數和相機的光學引數。通常在每個基於feature的3d場景重建演算法中都要用到bundle adjustment，作為每個基於特徵的多視...
bundle adjustment 光束法平差
推薦文獻 bundle adjustment method using sparse bfgs solution 這篇 是一篇介紹了光束法平差的原理的sci，今年 2018年 剛發表的，來自北大的 文章分析了ba的原理，並介紹了高斯牛頓和lm的 和不足之處，最後這篇 提出了一種基於擬牛頓法 bfgs...
基於opencv2的相片拼接（光束法平差）
主要是供無人機的相片使用，原理這裡就不詳述了。直接貼 qt 4 5作為資料讀入支援 include include include include include using namespace std using namespace cv int main int argc,char argv a...