1： 粒子滤波
粒子滤波的结构实际上就是加一层重要性采样思想在里面的蒙特卡罗方法（Monte Carlo method,即以某时间出现的频率来指代该事件的概率）。该方法的基本思想是用一组样本（或称粒子）来近似表示系统的后验概率分布，然后使用这一近似的表示来估计非线性系统的状态。采用此思想，在滤波过程中粒子滤波可以处理任意形式的概率，而不像Kalman老伯伯只能处理线性高斯分布的概率问题。粒子滤波的一大优势也在于此。
粒子滤波步骤
(1)初始状态：用大量粒子模拟运动状态，使粒子在空间内均匀分布；
(2)预测阶段：根据状态转移方程，将每一个粒子带入，得到一个预测粒子；
(3)校正阶段：对预测粒子进行评价（计算权重），越接近于真实状态的粒子，其权重越大；
(4)重采样：根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；
(5)滤波：将重采样后的粒子带入状态转移方程得到新的预测粒子，即步骤(2)。
粒子滤波是以贝叶斯推理（点击打开链接）和重要性采样为基本框架的。因此，想要掌握粒子滤波，对于上述两个基本内容必须有一个初步的了解。重要性采样呢，其实就是根据对粒子的信任程度添加不同的权重，添加权重的规则就是：对于我们信任度高的粒子，给它们添加的权重就相对大一些；否则，就加的权重小一些。根据权重的分布形式，实际上就是它与目标的相似程度。
一、粒子滤波步骤：
1、初始状态：用大量粒子模拟X（t），粒子在空间内均匀分布；
2、预测阶段：根据状态转移方程，每一个粒子得到一个预测粒子；
3、校正阶段：对预测粒子进行评价，越接近于真实状态的粒子，其权重越大；
4、重采样：根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；
5、滤波：将重采样后的粒子带入状态转移方程得到新的预测粒子，即步骤2。
二、上述过程，每一步的具体做法
首先，看看如下任意状态下的状态方程;
x(t)=f(x(t-1),u(t),w(t))
y(t)=h(x(t),e(t))
其中，x（t）为t时刻状态，u（t）为控制量，w(t)和e(t)分别为状态噪音和观测噪音。前一个方程描述是状态转移，后一个是观测方程。对于这么一个问题粒子滤波怎么来从观测y(t)和x(t-1),u(t)滤出真实状态x（t）呢？
初始状态：由于开始对x(0)一无所知，所有我们认为x（0）在全状态空间内平均分布。然后将所有采样输入状态转移方程，得到预测粒子。
预测阶段：粒子滤波首先根据x(t-1)的概率分布生成大量的采样，这些采样就称之为粒子。那么这些采样在状态空间中的分布实际是x(t-1)的概率分布了。接下来依据状态转移方程加上控制量可以对每一粒子得到一个预测粒子。
校正阶段：观测值y到达后，利用观测方程即条件概率P(y|xi)，对所有的粒子进行评价。直白的说，这个条件概率代表了假设真实状态x(t)取第i个粒子xi时获得观测y的概率。令这个条件概率为第i个粒子的权重。如此这样，继续对所有的粒子都进行这么个评价，那么越有可能获得观测y的粒子，当然获得的权重越高。
重采样：去除低权值的粒子，复制高权值的粒子，得到我们需要的真实状态x(t)。而这些重采样后的粒子，就代表了真实状态的概率分布。下一轮滤波，再将重采样后的粒子集输入到状态转移方程中，直接就能够获得预测粒子了。
三、实例
1、初始阶段：该阶段需要人工指定跟踪目标，程序计算跟踪目标的特征。比如可以采用目标的颜色特征。具体到Rob Hess的代码，开始时需要人工用鼠标拖出一个跟踪区域，然后程序自动计算该区域色调（Hue）的直方图，即为目标的特征。直方图可以用一个向量来表示，所以目标特征就是一个N*1的向量V。
2、搜索阶段：此时，需要去搜索目标对象，也就是粒子滤波了。首先，需要撒粒子。撒粒子的方法有很多种，最常用的两种方法，即：1）均匀的放（在整个图像平面均匀的撒上粒子）；2）在上一帧得到的目标附近按照高斯分布来放，可以理解成，靠近目标的地方多放，远离目标的少放。Rob Hess的代码用的是后一种方法。然后，按照初始化阶段得到的目标特征（色调直方图，向量V）对目标进行搜索。利用每个粒子所处的位置处图像的颜色特征，得到一个色调直方图，向量Vi，计算该直方图与目标直方图的相似性。相似性有多种度量，最简单的一种就是计算sum(abs(Vi-V)。每个粒子算出相似度后再做一次归一化，使得到的相似度加起来等于1.
3、决策阶段:根据上面计算出来的每个粒子与目标的相似度，我们做次加权平均。设N号粒子的图像像素坐标是（Xn,Yn），它报告的相似度是Wn，于是目标最可能的像素坐标X=sum(Xn*Wn)，Y=sum(Yn*Wn)。
4、重采样阶段：根据每个粒子处，相似度的大小，重新分布粒子。相似度高的地方，多放粒子；相似度低的地方，少放粒子。
2->3->4->2，如此反复，即完成了目标的动态跟踪。
————————————————
图像应用：
#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <ctime>
#include <fstream>
#include <iostream>
#include <assert.h>
using namespace cv;
using namespace std;
//****************************定义粒子数目**********************************//
#define PARTICLE_NUMBER 300
#define HIST_SIZE 16
//*******************************定义粒子结构体类型************************//
typedef struct particle//关于typedef struct和struct见下文补充
{
int orix, oriy; //原始粒子坐标
int x, y; //当前粒子的坐标
double scale; //当前粒子窗口的尺寸
int prex, prey; //上一帧粒子的坐标
double prescale; //上一帧粒子窗口的尺寸
Rect rect; //当前粒子矩形窗口
double weight; //当前粒子权值
}PARTICLE;
bool leftButtonDownFlag=false; //左键单击后的标志位
bool leftButtonUpFlag=false; //左键单击后松开的标志位
Point Point_s; //矩形框起点
Point Point_e; //矩形框鼠标左键弹起来的终点
Point processPoint; //矩形框移动的终点
bool tracking = false;
//****有关粒子窗口变化用到的相关变量****// 目标运行预测值
int A1 = 2;
int A2 = -1;
int B0 = 1;
double sigmax = 1.0;
double sigmay = 0.5;
double sigmas = 0.001;
double *m_hist, *hist;
int p_num = 0;
vector<PARTICLE> newParticle;//定义一个新的粒子数组
/****粒子权重值的降序排列****/
// 排序法则，其中：< 升序 >降序
bool comparison(PARTICLE p1,PARTICLE p2)
{
return p1.weight > p2.weight ;
}
void onMouse( int event, int x, int y, int flags, void *param )
{
if(event==CV_EVENT_LBUTTONDOWN)
{
tracking = false;
leftButtonDownFlag = true; //标志位
leftButtonUpFlag = false;
processPoint=Point(x,y); //设置左键按下点的矩形起点
Point_s=processPoint;
}
else if(event == CV_EVENT_MOUSEMOVE && leftButtonDownFlag)
{
processPoint=Point(x,y);
}
else if(event==CV_EVENT_LBUTTONUP && leftButtonDownFlag)
{
leftButtonDownFlag=false;
processPoint=Point(x,y);
Point_e=processPoint;
leftButtonUpFlag = true;
tracking = true;
}
}
void init_target(Mat mould, vector<PARTICLE> &particles, Rect &rect)
{
int q_r, q_g, q_b, q_temp;
particles.clear();
//初始化权值矩阵和目标直方图
for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
{
m_hist[i] = 0.0;
}
//计算目标权值直方
for (int i = 0;i < mould.rows; i++)
{
for (int j = 0;j < mould.cols; j++)
{
//rgb颜色空间量化为16*16*16 bins
q_r = mould.at<Vec3b>(i, j)[2]/255/HIST_SIZE;
q_g = mould.at<Vec3b>(i, j)[1]/255/HIST_SIZE;
q_b = mould.at<Vec3b>(i, j)[0]/255/HIST_SIZE;
q_temp = q_r * HIST_SIZE * HIST_SIZE + q_g * HIST_SIZE + q_b;
m_hist[q_temp]++; // 颜色权重直方图
}
}
// 归一化
double m_hist_sum = 0.0;
for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
{
m_hist_sum += pow(m_hist[i], 2);
}
m_hist_sum = sqrt(m_hist_sum);
for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
{
m_hist[i] = m_hist[i]/m_hist_sum;
}
// 目标粒子初始化
PARTICLE pParticle;
for (int k=0; k<PARTICLE_NUMBER; k++) //对于每个粒子
{
pParticle.x = cvRound(rect.x + 0.5*rect.width);//当前粒子的x坐标
pParticle.y = cvRound(rect.y + 0.5*rect.height);//当前粒子的y坐标
//粒子的原始坐标为选定矩形框(即目标)的中心
pParticle.orix = pParticle.x;
pParticle.oriy = pParticle.y;
//当前粒子窗口的尺寸
pParticle.scale = 1;//初始化为1，然后后面粒子到搜索的时候才通过计算更新
//更新上一帧粒子的坐标
pParticle.prex = pParticle.x;
pParticle.prey = pParticle.y;
//上一帧粒子窗口的尺寸
pParticle.prescale = 1;
//当前粒子矩形窗口
pParticle.rect = rect;
//当前粒子权值
pParticle.weight = 0;//权重初始为0
particles.push_back(pParticle);
}
}
void My_Tracking(Mat img, vector<PARTICLE> &particles)
{
int xpre, ypre;
double prescale, scale;
int x, y;
double sum = 0.0;
RNG rng; //随机数产生器
int q_r, q_g, q_b, q_temp;
/*计算粒子区域的直方图*/
//初始化目标直方图
for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
{
hist[i] = 0.0;
}
for (int k=0; k<PARTICLE_NUMBER; k++)
{
//当前粒子的坐标
xpre = particles.at(k).x;
ypre = particles.at(k).y;
//当前粒子窗口的尺寸
prescale = particles.at(k).scale;
/*更新跟踪矩形框中心，即粒子中心*///使用二阶动态回归来自动更新粒子状态
x = cvRound(A1*(particles.at(k).x - particles.at(k).orix) + A2*(particles.at(k).prex - particles.at(k).orix) +
B0*rng.gaussian(sigmax) + particles.at(k).orix);
particles.at(k).x = max(0, min(x, img.cols - 1));
y = cvRound(A1*(particles.at(k).y - particles.at(k).oriy) + A2*(particles.at(k).prey - particles.at(k).oriy) +
B0*rng.gaussian(sigmay) + particles.at(k).oriy);
particles.at(k).y = max(0, min(y, img.rows - 1));
scale = A1*(particles.at(k).scale - 1) + A2*(particles.at(k).prescale - 1) + B0*(rng.gaussian(sigmas)) + 1.0;
particles.at(k).scale = max(1.0, min(scale, 3.0));//此处参数设置存疑
particles.at(k).prex = xpre;
particles.at(k).prey = ypre;
particles.at(k).prescale = prescale;
/*计算更新得到矩形框数据*/
particles.at(k).rect.x = max(0, min(cvRound(particles.at(k).x - 0.5*particles.at(k).scale*particles.at(k).rect.width), img.cols));
particles.at(k).rect.y = max(0, min(cvRound(particles.at(k).y - 0.5*particles.at(k).scale*particles.at(k).rect.height), img.rows));
particles.at(k).rect.width = min(cvRound(particles.at(k).rect.width), img.cols - particles.at(k).rect.x);
particles.at(k).rect.height = min(cvRound(particles.at(k).rect.height), img.rows - particles.at(k).rect.y);
//计算粒子权值直方图
for (int i = particles.at(k).rect.y; i < particles.at(k).rect.y + particles.at(k).rect.height; i++)
{
for (int j = particles.at(k).rect.x; j < particles.at(k).rect.x + particles.at(k).rect.width; j++)
{
//rgb颜色空间量化为16*16*16 bins
q_r = img.at<Vec3b>(i, j)[2]/255/HIST_SIZE;
q_g = img.at<Vec3b>(i, j)[1]/255/HIST_SIZE;
q_b = img.at<Vec3b>(i, j)[0]/255/HIST_SIZE;
q_temp = q_r * HIST_SIZE * HIST_SIZE + q_g * HIST_SIZE + q_b;
hist[q_temp]++; // 颜色权重直方图
}
}
// 直方图归一化
double hist_sum = 0.0;
double sim_sum = 0.0;
for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
{
hist_sum += pow(hist[i], 2);
}
hist_sum = sqrt(hist_sum);
for (int i=0;i<HIST_SIZE * HIST_SIZE * HIST_SIZE;i++)
{
hist[i] = hist[i]/hist_sum;
if(m_hist[i] > 0.0 && hist[i] > 0.0)
{
sim_sum += sqrt(m_hist[i]*hist[i]); // 计算巴氏距离
}
}
particles.at(k).weight = sim_sum;
/*粒子权重累加*/
sum += particles.at(k).weight;
}
// 赋值每个粒子权重
for (int k=0; k<PARTICLE_NUMBER; k++)
{
particles.at(k).weight /= sum;
}
sort(particles.begin(), particles.end(), comparison); // 权重排序
//*********************重采样，根据粒子权重重采样********************//
int T = 0; // 阈值，只要T个粒子
bool flag = false; // 是否对每个粒子重新赋值跳出标志位
newParticle.clear();
for (int k = 0;k < PARTICLE_NUMBER;k++)
{
if(flag) // 完成粒子赋值，跳出循环
{
break;
}
T = cvRound(particles.at(k).weight*PARTICLE_NUMBER); //将权重较弱的粒子淘汰掉，保留权重在阈值以上的
if(T > 0)
{
for (int i = 0;i < T;i++) // 权重越大，该点赋值数越多
{
newParticle.push_back(particles.at(k));
if (newParticle.size() >= PARTICLE_NUMBER)
{
flag = true;
break;
}
}
}
else // 没有可以满足条件的权值，跳出循环
{
break;
}
}
if(!flag) // 点未全部赋值，将剩下的用最大值进行赋值
{
while (newParticle.size() < PARTICLE_NUMBER)
{
newParticle.push_back(particles.at(0));//复制大的权值的样本填满空间
}
}
// 将粒子点替换为更新后的粒子点
for (int k = 0;k < PARTICLE_NUMBER;k++)
{
particles.at(k) = newParticle.at(k);
}
//***********计算最大权重目标的期望位置，采用权值最大的1/4个粒子数作为跟踪结果************//
Rect rectTracking; //初始化一个Rect作为跟踪的临时
double weight_temp = 0.0;
for (int k = 0; k<PARTICLE_NUMBER / 4; k++)
{
weight_temp += particles.at(k).weight;
}
for (int k = 0; k<PARTICLE_NUMBER / 4; k++)
{
particles.at(k).weight /= weight_temp;
}
// 更新检测框
for (int k = 0; k<PARTICLE_NUMBER / 4; k++)
{
rectTracking.x += particles.at(k).rect.x*particles.at(k).weight;
rectTracking.y += particles.at(k).rect.y*particles.at(k).weight;
rectTracking.width += particles.at(k).rect.width*particles.at(k).weight;
rectTracking.height += particles.at(k).rect.height*particles.at(k).weight;
}
rectangle(img, rectTracking, Scalar(0, 255, 0), 3, 8, 0);//显示跟踪结果，框出
}
int main()
{
Mat img_mould, frame, mould;
Rect rect;
vector<PARTICLE> particles; // 粒子参数
//打开摄像头或者特定视频
VideoCapture cap;
cap.open(0);//或cap.open("文件名")
//读入视频是否为空
if (!cap.isOpened())
{
return -1;
}
namedWindow("输出视频", 1);
setMouseCallback("输出视频", onMouse, 0);//鼠标回调函数，响应鼠标以选择跟踪区域
m_hist = (double *)malloc(sizeof(double)*HIST_SIZE * HIST_SIZE * HIST_SIZE); // 目标直方图
hist = (double *)malloc(sizeof(double)*HIST_SIZE * HIST_SIZE * HIST_SIZE); // 目标直方图
while(1)
{
cap >> frame;
if (frame.empty())
{
return -1;
}
blur(frame, frame, Size(2, 2));//先对原图进行均值滤波处理
if(tracking && leftButtonUpFlag)
{
leftButtonUpFlag = false;
rect.x = Point_s.x;
rect.y = Point_s.y;
rect.width = Point_e.x - Point_s.x;
rect.height = Point_e.y - Point_s.y;
img_mould = frame.clone();
mould = Mat(img_mould, rect); //目标图像
init_target(mould, particles, rect);
p_num = 0;
}
if(leftButtonDownFlag) // 绘制截取目标窗口
{
rect.x = Point_s.x;
rect.y = Point_s.y;
rect.width = processPoint.x - Point_s.x;
rect.height = processPoint.y - Point_s.y;
rectangle(frame, rect, Scalar(0, 255, 0), 3, 8, 0);
}
if(tracking)
{
My_Tracking(frame, particles);
cout << p_num++ << endl; // 输出检测帧数
}
imshow("输出视频", frame);
waitKey(1);
}
return 0;
}
2： kalman滤波
粗浅理解看来，
粒子滤波的核心思想是随机采样+重要性重采样。既然不知道目标在哪里，那就随机的撒粒子吧。撒完粒子后，根据特征相似度计算每个粒子的重要性，然后在重要的地方撒更多的粒子，不重要的地方少撒粒子。所以说粒子滤波较之蒙特卡洛滤波，计算量较小。这个思想和RANSAC算法真是不谋而合。RANSAC的思想也是（比如用在最简单的直线拟合上），既然不知道直线方程是什么，那就随机的取两个点先算个直线出来，然后再看有多少点符合这条直线。哪条直线能获得最多的点的支持，哪条直线就是目标直线。想法非常简单，但效果很好。
3： 单目测距及标定
https://zhuanlan.zhihu.com/p/94244568
对于不同的图片，内参矩阵[公式] 为定值；对于同一张图片，内参矩阵[公式]，外参矩阵 [公式] 为定值；对于同一张图片上的单点，内参矩阵[公式]，外参矩阵 [公式]，尺度因子Z 为定值。
如何lidar和camera的外参标定好的情况下：：
图像坐标可用反投影到世界坐标，但是前提是假设 Z为目标的底部，因为尺度因子可以安装公式抵消掉；
就是下面公式相反的求解；
利用相机内参不变，先求解出内参：
对于同一个相机，相机的内参矩阵取决于相机的内部参数，无论标定板和相机的位置关系是怎么样的，相机的内参矩阵不变。这也正是在第2部分“求解内参矩阵”中，我们可以利用不同的图片（标定板和相机位置关系不同）获取的矩阵 [公式] ，共同求解相机内参矩阵 [公式] 的原因。
标定版和相机位置是不断改变：
因此，可以求解出外参矩阵
但是，外参矩阵反映的是标定板和相机的位置关系。
对于不同的图片，标定板和相机的位置关系已经改变，此时每一张图片对应的外参矩阵都是不同的。
4： 图遍历算法，激光点云聚类
BFS广度优先遍历
DFS深度优先遍历
广度优先搜索适用于结构化点云
void labelComponents(int row, int col){
float d1, d2, alpha, angle;
int fromIndX, fromIndY, thisIndX, thisIndY;
bool lineCountFlag[N_SCAN] = {false};
queueIndX[0] = row;
queueIndY[0] = col;
int queueSize = 1;
int queueStartInd = 0;
int queueEndInd = 1;
allPushedIndX[0] = row;
allPushedIndY[0] = col;
int allPushedIndSize = 1;
// 标准的BFS，作用是以(row，col)为中心向外面扩散，判断(row,col)是否是这个平面中一点
while(queueSize > 0){
fromIndX = queueIndX[queueStartInd];
fromIndY = queueIndY[queueStartInd];
--queueSize;
++queueStartInd;
// labelCount的初始值为1，后面会递增
labelMat.at<int>(fromIndX, fromIndY) = labelCount;
// neighbor=[[-1,0];[0,1];[0,-1];[1,0]]
// 遍历点[fromIndX,fromIndY]边上的四个邻点
for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
thisIndX = fromIndX + (*iter).first;
thisIndY = fromIndY + (*iter).second;
if (thisIndX < 0 || thisIndX >= N_SCAN)
continue;
// 由于雷达水平角度360度相连的特性是个环状的图片，左右连通
if (thisIndY < 0)
thisIndY = Horizon_SCAN - 1;
if (thisIndY >= Horizon_SCAN)
thisIndY = 0;
// 如果点[thisIndX,thisIndY]已经标记过
// labelMat中，-1代表无效点，0代表未进行标记过，其余为其他的标记
// 如果当前的邻点已经标记过，则跳过该点。
// 如果labelMat已经标记为正整数，则已经聚类完成，不需要再次对该点聚类
if (labelMat.at<int>(thisIndX, thisIndY) != 0)
continue;
d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY),
rangeMat.at<float>(thisIndX, thisIndY));
d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY),
rangeMat.at<float>(thisIndX, thisIndY));
// alpha代表角度分辨率，X方向上角度分辨率是segmentAlphaX(rad)，Y方向上角度分辨率是segmentAlphaY(rad)
if ((*iter).first == 0)
alpha = segmentAlphaX;
else
alpha = segmentAlphaY;
// 通过下面的公式计算这两点之间是否有平面特征，是否是同一类
// atan2(y,x)的值越大，d1，d2之间的差距越小,越接近，越平坦
angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
if (angle > segmentTheta){
// segmentTheta=1.0472<==>60度
// 如果算出角度大于60度，则假设这是个平面
queueIndX[queueEndInd] = thisIndX;
queueIndY[queueEndInd] = thisIndY;
++queueSize;
++queueEndInd;
labelMat.at<int>(thisIndX, thisIndY) = labelCount;
lineCountFlag[thisIndX] = true;
allPushedIndX[allPushedIndSize] = thisIndX;
allPushedIndY[allPushedIndSize] = thisIndY;
++allPushedIndSize;
}
}
}
bool feasibleSegment = false;
// 如果聚类超过30个点，直接标记为一个可用聚类，labelCount需要递增
if (allPushedIndSize >= 30)
feasibleSegment = true;
else if (allPushedIndSize >= segmentValidPointNum){
// 如果聚类点数小于30大于等于5，统计竖直方向上的聚类点数，这种情况典型例子是电线杆
int lineCount = 0;
for (size_t i = 0; i < N_SCAN; ++i)
if (lineCountFlag[i] == true)
++lineCount;
// 竖直方向上超过3个也将它标记为有效聚类
if (lineCount >= segmentValidLineNum)
feasibleSegment = true;
}
if (feasibleSegment == true){
++labelCount;
}else{
for (size_t i = 0; i < allPushedIndSize; ++i){
// 标记为999999的是需要舍弃的聚类的点，因为他们的数量小于30个，且竖直方向少于3
labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
}
}
}
5： 非线性优化，雅可比矩阵
ceres例子：
http://ceres-solver.org/analytical_derivatives.html
#include "ceres/ceres.h"
#include <opencv2/core/core.hpp>
#include <chrono>
using namespace std;
using namespace ceres;
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
struct CostFunctor
{
CostFunctor( double x, double y ) : _x ( x ), _y ( y ) {}
template <typename T> bool operator()(const T* const bb, //模型参数，有4维
T* residual) const //残差
{
residual[0] = bb[0]/pow(1.0+exp(bb[1]-bb[2]*T(_x)),1.0/bb[3])-T(_y);
return true;
}
const double _x, _y; // x,y数据
};
int main(int argc, char** argv)
{
double b1=1,b2=2,b3=3,b4=4; // 真实参数值
int N=100; // 数据点
double w_sigma=0.001; // 噪声Sigma值
cv::RNG rng; // OpenCV随机数产生器
double bEst[4] = {1,1,1,1}; // b参数的估计值
vector<double> x_data, y_data; // 数据
cout<<"generating data: "<<endl;
for ( int i=0; i<N; i++ )
{
double x = i/100.0;
x_data.push_back ( x );
y_data.push_back (b1/pow(1.0+exp(b2-b3*x),1.0/b4)+ rng.gaussian ( w_sigma ));
cout<<x_data[i]<<" "<<y_data[i]<<endl;
}
// Build the problem.
Problem problem;
for ( int i=0; i<N; i++ )
{
CostFunction* cost_function =new AutoDiffCostFunction<CostFunctor, 1, 4>(new CostFunctor( x_data[i], y_data[i] ));
problem.AddResidualBlock(cost_function, NULL, bEst);
}
// Run the solver!
Solver::Options options;
options.minimizer_progress_to_stdout = true;
Solver::Summary summary;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
ceres::Solve ( options, &problem, &summary );
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
std::cout << summary.BriefReport() << "\n";
std::cout << "output b1: " << bEst[0] << "\n";
std::cout << "output b2: " << bEst[1] << "\n";
std::cout << "output b3: " << bEst[2] << "\n";
std::cout << "output b4: " << bEst[3] << "\n";
return 0;
}
例如：：
lego-loam中，求解位姿关系：
https://zhuanlan.zhihu.com/p/245603082
式中的f函数即为计算新一帧Corner Point点到与其匹配到的前一帧两个Corner Point点连线的距离公式，
此函数为非线性故进行优化解算的话需首先求取其一阶导组成的***雅克比矩阵***，之后直接使用优化算法即可完成tx，tz，ry的求解。
bool calculateTransformationSurf(int iterCount) {
int pointSelNum = laserCloudOri->points.size();
cv::Mat matA(pointSelNum, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matAt(3, pointSelNum, CV_32F, cv::Scalar::all(0));
cv::Mat matAtA(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matB(pointSelNum, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matAtB(3, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matX(3, 1, CV_32F, cv::Scalar::all(0));
float srx = sin(transformCur[0]);
float crx = cos(transformCur[0]);
float sry = sin(transformCur[1]);
float cry = cos(transformCur[1]);
float srz = sin(transformCur[2]);
float crz = cos(transformCur[2]);
float tx = transformCur[3];
float ty = transformCur[4];
float tz = transformCur[5];
float a1 = crx * sry * srz;
float a2 = crx * crz * sry;
float a3 = srx * sry;
float a4 = tx * a1 - ty * a2 - tz * a3;
float a5 = srx * srz;
float a6 = crz * srx;
float a7 = ty * a6 - tz * crx - tx * a5;
float a8 = crx * cry * srz;
float a9 = crx * cry * crz;
float a10 = cry * srx;
float a11 = tz * a10 + ty * a9 - tx * a8;
float b1 = -crz * sry - cry * srx * srz;
float b2 = cry * crz * srx - sry * srz;
float b5 = cry * crz - srx * sry * srz;
float b6 = cry * srz + crz * srx * sry;
float c1 = -b6;
float c2 = b5;
float c3 = tx * b6 - ty * b5;
float c4 = -crx * crz;
float c5 = crx * srz;
float c6 = ty * c5 + tx * -c4;
float c7 = b2;
float c8 = -b1;
float c9 = tx * -b2 - ty * -b1;
// 构建雅可比矩阵，求解
for (int i = 0; i < pointSelNum; i++) {
pointOri = laserCloudOri->points[i];
coeff = coeffSel->points[i];
float arx = (-a1 * pointOri.x + a2 * pointOri.y + a3 * pointOri.z + a4) *
coeff.x +
(a5 * pointOri.x - a6 * pointOri.y + crx * pointOri.z + a7) *
coeff.y +
(a8 * pointOri.x - a9 * pointOri.y - a10 * pointOri.z + a11) *
coeff.z;
float arz = (c1 * pointOri.x + c2 * pointOri.y + c3) * coeff.x +
(c4 * pointOri.x - c5 * pointOri.y + c6) * coeff.y +
(c7 * pointOri.x + c8 * pointOri.y + c9) * coeff.z;
float aty = -b6 * coeff.x + c4 * coeff.y + b2 * coeff.z;
float d2 = coeff.intensity;
matA.at<float>(i, 0) = arx;
matA.at<float>(i, 1) = arz;
matA.at<float>(i, 2) = aty;
matB.at<float>(i, 0) = -0.05 * d2;
}
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR); // 用来解线性方程 A*X=B
if (iterCount == 0) {
cv::Mat matE(1, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV2(3, 3, CV_32F, cv::Scalar::all(0));
cv::eigen(matAtA, matE, matV);
matV.copyTo(matV2);
isDegenerate = false;
float eignThre[3] = {10, 10, 10};
for (int i = 2; i >= 0; i--) {
if (matE.at<float>(0, i) < eignThre[i]) {
for (int j = 0; j < 3; j++) {
matV2.at<float>(i, j) = 0;
}
isDegenerate = true;
} else {
break;
}
}
matP = matV.inv() * matV2;
}
if (isDegenerate) {
cv::Mat matX2(3, 1, CV_32F, cv::Scalar::all(0));
matX.copyTo(matX2);
matX = matP * matX2;
}
transformCur[0] += matX.at<float>(0, 0);
transformCur[2] += matX.at<float>(1, 0);
transformCur[4] += matX.at<float>(2, 0);
for (int i = 0; i < 6; i++) {
if (isnan(transformCur[i]))
transformCur[i] = 0;
}
float deltaR = sqrt(pow(rad2deg(matX.at<float>(0, 0)), 2) +
pow(rad2deg(matX.at<float>(1, 0)), 2));
float deltaT = sqrt(pow(matX.at<float>(2, 0) * 100, 2));
if (deltaR < 0.1 && deltaT < 0.1) {
return false;
}
return true;
}
https://blog.csdn.net/weixin_44684139/article/details/118581383
6：Ransac
7：利用SVD求解平面参数
结论：
通常函数库计算中：即V的第三列为平面法向量
原因：
// 协方差矩阵的SVD变换中，最小奇异值对应的奇异向量就是平面的方向
void FitPlaneSVD::compute()
{
// 1、计算质心
Eigen::RowVector3d centroid = m_cloud.colwise().mean();
// 2、去质心
Eigen::MatrixXd demean = m_cloud;
demean.rowwise() -= centroid;
// 3、构建协方差矩阵
Eigen::MatrixXd covMat = demean.transpose()* demean;
// 4、SVD分解求解协方差矩阵的特征值特征向量
Eigen::JacobiSVD<Eigen::MatrixXd> svd(covMat, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::Matrix3d V = svd.matrixV();
Eigen::MatrixXd U = svd.matrixU();
Eigen::Matrix3d S = U.inverse() * covMat * V.transpose().inverse();
// 5、平面的法向量a,b,c
Eigen::RowVector3d normal;
normal << V(0,2), V(1,2), V(2,2);
// 6、原点到平面的距离d
double d = -normal * centroid.transpose();
// 7、获取拟合平面的参数a,b,c,d和质心x,y,z。
m_planeparameters << normal, d, centroid;
}
// 综上：对矩阵A做奇异值分解，最小奇异值对应的特征向量就是拟合平面的系数向量。
#include <vector>
#include <Eigen/SVD>
Eigen::Vector4d PlaneFitting(const std::vector<Eigen::Vector3d> & plane_pts) {
Eigen::Vector3d center = Eigen::Vector3d::Zero();
for (const auto & pt : plane_pts) center += pt;
center /= plane_pts.size();
Eigen::MatrixXd A(plane_pts.size(), 3);
for (int i = 0; i < plane_pts.size(); i++) {
A(i, 0) = plane_pts[i][0] - center[0];
A(i, 1) = plane_pts[i][1] - center[1];
A(i, 2) = plane_pts[i][2] - center[2];
}
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinV);
const float a = svd.matrixV()(0, 2);
const float b = svd.matrixV()(1, 2);
const float c = svd.matrixV()(2, 2);
const float d = -(a * center[0] + b * center[1] + c * center[2]);
return Eigen::Vector4d(a, b, c, d);
}
扫一扫
684
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
