ceres学习之平面拟合

背景：orb-slam2最终保存的轨迹形成的平面是一个倾斜的，这个与相机初始化位置有关，但是有些时候，我们需要的是一个2d的轨迹的试图，直接将轨迹向某一个平面投影的话。

得到的估计是失真的，所以我们需要对轨迹的location数据进行处理，就是首先拟合出轨迹的平面，然后将这个平面绕着一个轴，旋转一定的角度，将其旋转成与某一个做表面平行，

然后再取出其中两个维度的坐标，那么我们就可以得到一个真是的2d平面。为实现上面的目的，第一步工作就是要根据输入的坐标数据，3d坐标点，拟合出一个主平面，使用ceres

优化求解平面的四个参数a,b,c,d的值。这个过程主要是熟悉使用ceres优化特定问题的一个框架，然后根据实际的问题，去填入具体的残差函数，等。

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);    
    }
}
//这边传参数，要使用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;
}