那麼如何用ceres求解？其實很簡單，殘差我們還是按照老樣子定義，只不過再初始化引數時，要初始化所以變數，及所以路標和位姿，那麼對於每乙個誤差項，我們通過指標的索引來制定要優化的位姿和路標點在放入addresidualblocks裡面。
廢話不多說，來看**吧，提示，由於我在做後端優化時使用的別人的資料集，所以我沒有寫乙個完整的多執行緒的slam，其實是我根本不會多執行緒。。。。我把localba的資料拿出來在放入globalba裡面的，看上去有些笨拙，但是對於我這樣的入門來說來說可以接受的。
#include#include#include#include#include#include#include#include#include#include#include#include#include#include #include #include #include #include #include#include#include using namespace std;
using namespace cv;
struct cost_function_define
templatebool operator()(const t* const cere_r,t* residual)const
point3d _p1;
point2d _p2;
};int main()
ifstream fin_1("./data.txt");
vectorposes;
for(int i=0;i<32;i++)
; fin_1>>data[0]>>data[1]>>data[2]>>data[3]>>data[4]>>data[5]>>data[6]>>data[7]>>data[8]>>data[9]>>data[10]>>data[11];
mat currentpose=(mat_(3,4)
vectorlocalnum;
vectorlocalqueue;
for(int i=0;i<31;i++)
for(int j=i+1;j<32;j++)
localnum.push_back(pixel2);
localqueue.push_back(queue[j]);
point3d camera_3d=point3d(pixel_cam1.x*double(d)/1000.0,pixel_cam1.y*double(d)/1000.0,double(d)/1000.0);
mat worldpoint=(mat_(3,3)
poses[i].at(1,0),poses[i].at(1,1),poses[i].at(1,2),
poses[i].at(2,0),poses[i].at(2,1),poses[i].at(2,2)
)*(mat_(3,1)
point3d world_3d=point3d(worldpoint.at(0,0),worldpoint.at(1,0),worldpoint.at(2,0));
loaclopt.push_back(world_3d);}}
cout<
for(int i=1;i<32;i++)
for(int j=0;j<3;j++)
} double dataest[186]=;
for(int i=0;i<186;i++)
double* camera=dataest;
//定義求解問題
ceres::problem problem;
for(int i=0;i(new cost_function_define(loaclopt[i],localnum[i]));
ceres::lossfunction* lossfunction=new ceres::huberloss(1.0);
//新增每一次觀測的殘差
double* cere_r=camera+6*(localqueue[i]-1);
problem.addresidualblock(costfunction,lossfunction,cere_r);
} //配置求解器
ceres::solver::options option;
//選擇迭代方式
option.linear_solver_type=ceres::sparse_schur;
//輸出迭代資訊到螢幕
option.minimizer_progress_to_stdout=true;
//顯示優化資訊
ceres::solver::summary summary;
//開始求解
ceres::solve(option,&problem,&summary);
//顯示優化資訊
cout
writefile(worldaxis,"./local.txt");
for(int i=0;i<31;i++)
//寫資料到點雲
ifstream fin_2("./local.txt");
vectorpose;
for(int i=0;i<32;i++)
; fin_2>>data[0]>>data[1]>>data[2]>>data[3]>>data[4]>>data[5]>>data[6]>>data[7]>>data[8]>>data[9]>>data[10]>>data[11];
mat currentpose=(mat_(3,4)<::ptr pointcloud pcl::pointcloud>
for(int i=0;i<32;i++)
//還原三維點
point3d camera_point;
camera_point.z=double(d)/1000.0;
camera_point.x=(u-k.at(0,2))/k.at(0,0)*camera_point.z;
camera_point.y=(v-k.at(1,2))/k.at(1,1)*camera_point.z;
mat worldpoint=(mat_(3,3)
pose[i].at(1,0),pose[i].at(1,1),pose[i].at(1,2),
pose[i].at(2,0),pose[i].at(2,1),pose[i].at(2,2)
)*(mat_(3,1)
pcl::pointxyzrgb p_3d;
p_3d.x=worldpoint.at(0,0);
p_3d.y=worldpoint.at(1,0);
p_3d.z=worldpoint.at(2,0);
p_3d.b=keyframe.data[v*keyframe.step+u*keyframe.channels()];
p_3d.g=keyframe.data[v*keyframe.step+u*keyframe.channels()+1];
p_3d.r=keyframe.data[v*keyframe.step+u*keyframe.channels()+2];
pointcloud->points.push_back(p_3d);
}//統計濾波
// pcl::pointcloud::ptr tem(new pcl::pointcloud);
// pcl::statisticaloutlierremovalstatistical_filter;
//設定聚類，閥值
// statistical_filter.setmeank(50);
// statistical_filter.setstddevmulthresh(1.0);
// statistical_filter.setinputcloud(currentpoint);
// statistical_filter.filter(*tem);
// (*pointcloud)+=*tem;
} cout<
cout
//設定最小方格
voxel_filter.setleafsize(0.01,0.01,0.01);
//定義乙個點雲指標用於儲存濾波後的點
pcl::pointcloud::ptr tempoint(new pcl::pointcloud);
voxel_filter.setinputcloud(pointcloud);
//濾波
voxel_filter.filter(*tempoint);
tempoint->swap(*pointcloud);
cout
(p.y-k.at(1,2))/k.at(1,1)
);} //將位置寫入txt便於讀取
void writefile(const mat& mat,const char* filename)
fout來看一下結果把，誤差還是縮小了一些的
SLAM中的後端優化
本節介紹slam中的後端優化過程 同時對三維點位置和相機引數進行非線性優化 原理 是一種 信賴域 的方法，當收斂速度較快時，增大信賴域使演算法趨向於高斯牛頓法 當收斂速度較慢時，減小信賴域使演算法趨向於最速下降法。優勢 速度快 可以在距離初始值較遠處得到最優解。slam優化演算法對比 演算法缺點 優...
如何用ceres進行兩幀之間的BA優化
學習高博的書已有很長一段時間了，一直看理論，看 而沒有自己親自上手，最近在做ba優化，大部分slam是用g2o進行的，而對於ceres用的很少，由於博主根本看不懂g2o的 風格，個人覺得很無語 其實是博主zz 那咋辦？於是就想乾脆用ceres實現ba優化吧。而關於ceres，其實主要還是殘差的定義了...
理解slam中的非線性優化問題
主要參考了高翔老師的14講中的知識，以及結合imu預積分 on manifold preintegration for real time visual inertial odometry 結合自己的理解，做了一下整理。整體思路是從狀態估計 最大後驗估計 最小二乘 非線性最小二乘 狀態估計 最大後驗...