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;
}

ceres学习之平面拟合相关推荐

三维空间点进行空间平面拟合原理及MATLAB和C++代码实现
平面拟合原理参考网页:https://blog.csdn.net/duiwangxiaomi/article/details/89246715 MATLAB实现参考网页:https://blog.cs ...
python 最小二乘法三维坐标拟合平面_matlab三维曲面进行平面拟合，利用最小二乘法...
展开全部可以直接2113使用matlab的曲面拟5261合工具箱,但是平面4102拟合的效果一1653般 1.在命令窗口内输入待拟合容的数据>> x=[11.4,11.4,11.4,11 ...
点云法向量与点云平面拟合的关系（PCA）
点云法向量估计的主要思路是对K-近邻的N个点进行平面拟合(平面过N点重心),平面法向量即为所求:所以求法向量就是变相的求拟合平面. 下面我们用最小二乘法求k近邻点云的拟合平面: 当 ||x||＝1时, ...
PCL最小二乘法进行平面拟合原理
最小二乘法进行平面拟合原理 1 最小二乘原理 2 最小二乘拟合平面 1 最小二乘原理最小二乘法(又称最小平方法)是一种数学优化技术.它通过最小化误差的平方和寻找数据的最佳函数匹配.利用最小二乘法可以 ...
利用PCL做点云的平面拟合
前提: 需要验证一组点云的误差,基本上都是墙面上的点,直观地看点云的话可以看出一个平面,但远近不一致,因此想尝试一下做一下平面几何,计算一下内点数量,然后算一下精度: 可以自己用RANSAC迭代,用三 ...
基于学习的平面抓取检测方法分类及讨论
平面抓取检测的任务是,输入感知数据,输出抓取配置.到目前,基于学习的平面抓取检测方法主要分为两类: (1)一阶段的端到端学习方法. (2)两阶段的学习方法. 1.一阶段学习法在这类方法中,直接学习从 ...
RANSAC点云多平面拟合分割
RANSAC点云多平面拟合分割三维平面拟合(最小二乘法) 假设 P P P 为一个点集, p i p_i pi 是点集中的任意一点, p i ∈ P p_i\in P pi∈P, p c p_c ...
基于最小二乘法的点云空间平面拟合（C++实现）
文章目录 1 空间平面的点法式方程和一般式方程 2 最小二乘法进行空间平面拟合 3 代码实现 4 实验对比 5 相关链接 1 空间平面的点法式方程和一般式方程已知平面 Π Π Π过点 P 0 ( x ...
Matlab基于主成分分析(PCA)的平面拟合—点云处理及可视化第2期
目录 1 概述 2 代码实现 3 可视化验证完整代码: PCA平面拟合结果: 特别提示:<Matlab点云处理及可视化>系列文章整理自作者博士期间的部分成果,旨在为初入点云处理领域的朋友 ...

ceres学习之平面拟合

ceres学习之平面拟合相关推荐

最新文章

热门文章