背景：orb-slam2最終保存的軌跡形成的平面是一個傾斜的，這個與相機(jī)初始化位置有關(guān)，但是有些時候，我們需要的是一個2d的軌跡的試圖，直接將軌跡向某一個平面投影的話。

得到的估計是失真的，所以我們需要對軌跡的location數(shù)據(jù)進(jìn)行處理，就是首先擬合出軌跡的平面，然后將這個平面繞著一個軸，旋轉(zhuǎn)一定的角度，將其旋轉(zhuǎn)成與某一個做表面平行，

然后再取出其中兩個維度的坐標(biāo)，那么我們就可以得到一個真是的2d平面。為實現(xiàn)上面的目的，第一步工作就是要根據(jù)輸入的坐標(biāo)數(shù)據(jù)，3d坐標(biāo)點，擬合出一個主平面，使用ceres

優(yōu)化求解平面的四個參數(shù)a,b,c,d的值。這個過程主要是熟悉使用ceres優(yōu)化特定問題的一個框架，然后根據(jù)實際的問題，去填入具體的殘差函數(shù)，等。

planeFitting.h

#include <iostream> #include <ceres/ceres.h> #include <ceres/rotation.h>struct OptimalPlanFitting {OptimalPlanFitting(double x, double y, double z):x_(x), y_(y), z_(z){}template<typename T>bool operator()(const T* const a, const T* const b, const T* const c, const T* const d,T* residual)const{T temp = a[0]*x_ + b[0]*y_ + c[0]*z_ + d[0];residual[0] = temp*temp/(a[0]*a[0] + b[0]*b[0] + c[0]*c[0]);return true;}static ceres::CostFunction* cost(double x, double y, double z){return(new ceres::AutoDiffCostFunction<OptimalPlanFitting, 1, 1, 1, 1, 1>(new OptimalPlanFitting(x, y, z)));} private:const double x_;const double y_;const double z_;};

planeFitting.cpp

#include <iostream> #include <ceres/ceres.h> #include <Eigen/Core> #include <Eigen/Dense> #include <Eigen/Geometry> #include <string> #include <fstream> #include "planeFitting.h" using namespace std; using namespace ceres;int split(std::string str, std::string pattern, std::vector<std::string> &words) {std::string::size_type pos;std::string word;int num = 0;str += pattern;std::string::size_type size = str.size();for (auto i = 0; i < size; i++){pos = str.find(pattern, i);if (pos == i){continue;//if first string is pattern}if (pos < size){word = str.substr(i, pos - i);words.push_back(word);i = pos + pattern.size() - 1;num++;}}return num; } void readFile(std::string filePath, std::vector<Eigen::Vector3d>& locations) {ifstream infile;infile.open(filePath);if(!infile){std::cout<<"filed to load trajectory data"<<std::endl;return;}string str;std::vector<string> words;while(!infile.eof()){words.clear();std::getline(infile, str);split(str, " ", words);Eigen::Vector3d position(atof(const_cast<const char *>(words[1].c_str())),atof(const_cast<const char *>(words[2].c_str())),atof(const_cast<const char *>(words[3].c_str())));locations.push_back(position); } } //這邊傳參數(shù)，要使用double 類型 //double *params或者是這樣 int planFittingOptimal(std::vector<Eigen::Vector3d> locations, double params[4]) {//double params[4]={1.0,0.1,0.1,0.2};ceres::Problem problem;for(int i = 0; i<locations.size(); i++){Eigen::Vector3d loc = locations[i];//std::cout<<loc<<std::endl;ceres::CostFunction *cost_func = OptimalPlanFitting::cost(loc.x(), loc.y(), loc.z());problem.AddResidualBlock(cost_func, new ceres::HuberLoss(1.0), &params[0], &params[1], &params[2], &params[3]);}ceres::Solver::Options options;options.linear_solver_type = ceres::SPARSE_SCHUR;options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;options.max_num_iterations = 50;options.minimizer_progress_to_stdout = true;ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);std::cout<<summary.BriefReport()<<"\n";std::cout<<"a: "<<params[0]<<" \nb:"<<params[1]<<" \nc:"<<params[2]<<"\nd:"<<params[3]<<std::endl;} int main() {double params[4]={1.0,0.1,0.2,0.1};std::string filePath = "/home/yunlei/COOL/ceres-study/data/CameraTrajectory_common.txt";std::vector<Eigen::Vector3d> locations;readFile(filePath, locations);//std::cout<<"locations.size(): "<<locations.size()<<std::endl;planFittingOptimal(locations, params);std::cout<<params[0]<<" "<<params[1]<<" "<<params[2]<<" "<<params[3]<<std::endl;return 0; }

?

總結(jié)

以上是生活随笔為你收集整理的ceres学习之平面拟合的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。