# 激光雷达学习笔记（四）定位

一、移动机器人 SLAM 技术

SLAM中，系统的状态由机器人的位姿和地图信息(特征的位置信息)组成。假设机器人在t时刻观测到

//SLAM示意图

SLAM方法有很多种，主要包括基于扩展卡尔曼滤波的SLAM技术，基于传统粒子滤波的SLAM技术，

1.1基于扫描匹配的 SLAM 技术

1.2 范例 ：

[Xw] [cos(alpha)-sin(alpha)]XrTx

=   *

[Yw]    [sin(alpha) cos(alpha)]YrTy

由两条直线解出格子坐标系下的交点，代入上式可以解出Tx Ty，上式中都是矩阵计算，由于没有word那么

//中值滤波 只能对初始的连续数据滤波
//滤波基本不丢弃数据，两端会各自扔掉几个数据
vector<int>rho;
vector<double>theta;
int halfWindowSize = 2;
int *neighbor = new int[2*halfWindowSize+1];
int temp;
for (int i = halfWindowSize; i< (int)RadarRho.size() - halfWindowSize;i++)
{
for (int j = -halfWindowSize;j <= halfWindowSize;j++)
{
neighbor[j + halfWindowSize] = RadarRho.at(i + j);
}
//排序
for (int m = 0; m < 2*halfWindowSize +1;m++)
{
for (int n = m + 1;n < 2*halfWindowSize + 1;n++)
{
if (neighbor[m]> neighbor[n])
{
temp = neighbor[m];
neighbor[m] = neighbor[n];
neighbor[n] = temp;
}
}
}
rho.push_back(neighbor[halfWindowSize]);
}

for (int i = 0; i < (int)(rho.size());i++)
{
}
}

//已知四条直线如何计算变换参数
void Coordinate::CalCoorTransPara(CoorTransPara &transPara,
LinePara W1,
LinePara W2,
LinePara R1,
LinePara R2)
{
double theta = ( W1.Rho - R1.Rho + W2.Rho - R2.Rho )/2;
//double theta = ( W1.Rho - R1.Rho);
//求解出Xw Yw Xr Yr
double Xw = (double)(W1.b - W2.b)/(W2.a - W1.a);
double Yw = W1.a*Xw + W1.b;

double Xr = (double)(R1.b - R2.b)/(R2.a - R1.a);
double Yr = R1.a*Xr + R1.b;

int Tx = (int)(Xw - cos(theta)*Xr + sin(theta)*Yr);
int Ty = (int)(Yw - sin(theta)*Xr - cos(theta)*Yr);
//交点判定，场地上的几条直线都是有角点的
iPoint crossPoint;//交点
iPoint vectorW1,vectorR1;//向量
//iPoint vectorR2,vectorW2;
if (W1.startPoint.x == W2.startPoint.x && W1.startPoint.y == W2.startPoint.y)
{
crossPoint = ipoint(W1.startPoint.x,W1.startPoint.y);
vectorW1 = ipoint(W1.endPoint.x - W1.startPoint.x, W1.endPoint.y - W1.startPoint.y);
//vectorW2 = ipoint(W2.endPoint.x - W2.startPoint.x, W2.endPoint.y - W2.startPoint.y);
}else if (W1.endPoint.x == W2.startPoint.x && W1.endPoint.y == W2.startPoint.y)
{
crossPoint = ipoint(W1.endPoint.x,W1.endPoint.y);
vectorW1 = ipoint(W1.startPoint.x - W1.endPoint.x, W1.startPoint.y - W1.endPoint.y);
//vectorW2 = ipoint(W2.endPoint.x - W2.startPoint.x, W2.endPoint.y - W2.startPoint.y);
}else if (W1.startPoint.x == W2.endPoint.x && W1.startPoint.y == W2.endPoint.y)
{
crossPoint = ipoint(W1.startPoint.x,W1.startPoint.y);
vectorW1 = ipoint(W1.endPoint.x - W1.startPoint.x, W1.endPoint.y - W1.startPoint.y);
//vectorW2 = ipoint(W2.startPoint.x - W2.endPoint.x, W2.startPoint.y - W2.endPoint.y);
}else if (W1.endPoint.x == W2.endPoint.x && W1.endPoint.y == W2.endPoint.y)
{
crossPoint = ipoint(W1.endPoint.x,W1.endPoint.y);
vectorW1 = ipoint(W1.startPoint.x - W1.endPoint.x, W1.startPoint.y - W1.endPoint.y);
//vectorW2 = ipoint(W2.startPoint.x - W2.endPoint.x, W2.startPoint.y - W2.endPoint.y);
}
//将激光雷达下的两个点旋转到W系下
transPara.theta = theta;
transPara.Tx = Tx;
transPara.Ty = Ty;
iPoint R1ToW;
//iPoint R2ToW;
TransformCoord(transPara,R1.startPoint,R1ToW);
//TransformCoord(transPara,R2.startPoint,R2ToW);
vectorR1.x = R1ToW.x - crossPoint.x;
vectorR1.y = R1ToW.y - crossPoint.y;
//判断是否在同一侧？
if (vectorW1.x * vectorR1.x + vectorW1.y*vectorR1.y < 0)
{
//旋转角度差了180度，需要调转180度
transPara.theta = theta + PI;
transPara.Tx = (int)(Xw - cos(transPara.theta)*Xr + sin(transPara.theta)*Yr);
transPara.Ty = (int)(Yw - sin(transPara.theta)*Xr - cos(transPara.theta)*Yr);
}else{

}
//数据测试
/* TransformCoord(transPara,R1.startPoint,R1ToW);
cout<<"R1ToW.x "<<R1ToW.x<<"  R1ToW.y "<<R1ToW.y<<endl;
TransformCoord(transPara,R1.endPoint,R1ToW);
cout<<"R1ToW.x "<<R1ToW.x<<"  R1ToW.y "<<R1ToW.y<<endl;

TransformCoord(transPara,R2.startPoint,R2ToW);
cout<<"R2ToW.x "<<R2ToW.x<<"  R2ToW.y "<<R2ToW.y<<endl;

TransformCoord(transPara,R2.endPoint,R2ToW);
cout<<"R2ToW.x "<<R2ToW.x<<"  R2ToW.y "<<R2ToW.y<<endl;*/
//进行一次验证，看看交点进行坐标变换之后是否接近匹配的点
/* iPoint R = ipoint(Xr,Yr);
iPoint R2W;
TransformCoord(transPara,R,R2W);
cout<<"R2W.x "<<R2W.x<<"  R2W.y "<<R2W.y<<endl;*/

}

//完整的Coordinate.h

#pragma once
#include "WeightedFit.h"
#include <iostream>
using namespace std;

//场地中的关键点
static iPoint FieldPointA = ipoint(9192,0);
static iPoint FieldPointB = ipoint(0,9192);
static iPoint FieldPointC = ipoint(-9192,0);
static iPoint FieldPointD = ipoint(0,-9192);
//场地中的直线变量
static LinePara FieldLine1 = linePara(-1.0,9192.3881554,FieldPointA,FieldPointB);
static LinePara FieldLine2 = linePara(1.0,9192.3881554,FieldPointB,FieldPointC);
static LinePara FieldLine3 = linePara(-1.0,-9192.3881554,FieldPointC,FieldPointD);
static LinePara FieldLine4 = linePara(1.0,-9192.3881554,FieldPointD,FieldPointA);
static LinePara FieldLine5 = linePara(100000.0,0.0,FieldPointB,FieldPointD);

//场地中的圆
static CirclePara FieldCircle1 = circlePara(-3000,1301,400,350);
static CirclePara FieldCircle2 = circlePara(-1951,880,400,350);
static CirclePara FieldCircle3 = circlePara(-651,815,400,350);
static CirclePara FieldCircle4 = circlePara(-495,2416,400,350);
static CirclePara FieldCircle5 = circlePara(-3347,-997,400,350);
static CirclePara FieldCircle6 = circlePara(-2400,-2848,400,350);
static CirclePara FieldCircle7 = circlePara(-1499,-2499,400,350);

static CirclePara FieldCircle8 = circlePara(3000,1301,400,350);
static CirclePara FieldCircle9 = circlePara(1951,880,400,350);
static CirclePara FieldCircle10 = circlePara(651,815,400,350);
static CirclePara FieldCircle11= circlePara(495,2416,400,350);
static CirclePara FieldCircle12 = circlePara(3347,-997,400,350);
static CirclePara FieldCircle13 = circlePara(2400,-2848,400,350);
static CirclePara FieldCircle14 = circlePara(1499,-2499,400,350);

//坐标系类，进行坐标系相关的计算
typedef struct{
int Tx;
int Ty;
double theta;//旋转角
}CoorTransPara;  //坐标变换参数

class Coordinate
{
public:

Coordinate(void);
~Coordinate(void);
//已知四条直线如何计算变换参数
void CalCoorTransPara(CoorTransPara &transPara,
LinePara W1,
LinePara W2,
LinePara R1,
LinePara R2);
void CoortransTest();

void TransformCoord(CoorTransPara transPara,iPoint R,iPoint& W);

};
//完整的Coordiate.cpp

#include "Coordinate.h"

Coordinate::Coordinate(void)
{
}

Coordinate::~Coordinate(void)
{
}

//已知四条直线如何计算变换参数
void Coordinate::CalCoorTransPara(CoorTransPara &transPara,
LinePara W1,
LinePara W2,
LinePara R1,
LinePara R2)
{
double theta = ( W1.Rho - R1.Rho + W2.Rho - R2.Rho )/2;
//double theta = ( W1.Rho - R1.Rho);
//求解出Xw Yw Xr Yr
double Xw = (double)(W1.b - W2.b)/(W2.a - W1.a);
double Yw = W1.a*Xw + W1.b;

double Xr = (double)(R1.b - R2.b)/(R2.a - R1.a);
double Yr = R1.a*Xr + R1.b;

int Tx = (int)(Xw - cos(theta)*Xr + sin(theta)*Yr);
int Ty = (int)(Yw - sin(theta)*Xr - cos(theta)*Yr);
//交点判定，场地上的几条直线都是有角点的
iPoint crossPoint;//交点
iPoint vectorW1,vectorR1;//向量
//iPoint vectorR2,vectorW2;
if (W1.startPoint.x == W2.startPoint.x && W1.startPoint.y == W2.startPoint.y)
{
crossPoint = ipoint(W1.startPoint.x,W1.startPoint.y);
vectorW1 = ipoint(W1.endPoint.x - W1.startPoint.x, W1.endPoint.y - W1.startPoint.y);
//vectorW2 = ipoint(W2.endPoint.x - W2.startPoint.x, W2.endPoint.y - W2.startPoint.y);
}else if (W1.endPoint.x == W2.startPoint.x && W1.endPoint.y == W2.startPoint.y)
{
crossPoint = ipoint(W1.endPoint.x,W1.endPoint.y);
vectorW1 = ipoint(W1.startPoint.x - W1.endPoint.x, W1.startPoint.y - W1.endPoint.y);
//vectorW2 = ipoint(W2.endPoint.x - W2.startPoint.x, W2.endPoint.y - W2.startPoint.y);
}else if (W1.startPoint.x == W2.endPoint.x && W1.startPoint.y == W2.endPoint.y)
{
crossPoint = ipoint(W1.startPoint.x,W1.startPoint.y);
vectorW1 = ipoint(W1.endPoint.x - W1.startPoint.x, W1.endPoint.y - W1.startPoint.y);
//vectorW2 = ipoint(W2.startPoint.x - W2.endPoint.x, W2.startPoint.y - W2.endPoint.y);
}else if (W1.endPoint.x == W2.endPoint.x && W1.endPoint.y == W2.endPoint.y)
{
crossPoint = ipoint(W1.endPoint.x,W1.endPoint.y);
vectorW1 = ipoint(W1.startPoint.x - W1.endPoint.x, W1.startPoint.y - W1.endPoint.y);
//vectorW2 = ipoint(W2.startPoint.x - W2.endPoint.x, W2.startPoint.y - W2.endPoint.y);
}
//将激光雷达下的两个点旋转到W系下
transPara.theta = theta;
transPara.Tx = Tx;
transPara.Ty = Ty;
iPoint R1ToW;
//iPoint R2ToW;
TransformCoord(transPara,R1.startPoint,R1ToW);
//TransformCoord(transPara,R2.startPoint,R2ToW);
vectorR1.x = R1ToW.x - crossPoint.x;
vectorR1.y = R1ToW.y - crossPoint.y;
//判断是否在同一侧？
if (vectorW1.x * vectorR1.x + vectorW1.y*vectorR1.y < 0)
{
//旋转角度差了180度，需要调转180度
transPara.theta = theta + PI;
transPara.Tx = (int)(Xw - cos(transPara.theta)*Xr + sin(transPara.theta)*Yr);
transPara.Ty = (int)(Yw - sin(transPara.theta)*Xr - cos(transPara.theta)*Yr);
}else{

}
//数据测试
/* TransformCoord(transPara,R1.startPoint,R1ToW);
cout<<"R1ToW.x "<<R1ToW.x<<"  R1ToW.y "<<R1ToW.y<<endl;
TransformCoord(transPara,R1.endPoint,R1ToW);
cout<<"R1ToW.x "<<R1ToW.x<<"  R1ToW.y "<<R1ToW.y<<endl;

TransformCoord(transPara,R2.startPoint,R2ToW);
cout<<"R2ToW.x "<<R2ToW.x<<"  R2ToW.y "<<R2ToW.y<<endl;

TransformCoord(transPara,R2.endPoint,R2ToW);
cout<<"R2ToW.x "<<R2ToW.x<<"  R2ToW.y "<<R2ToW.y<<endl;*/
//进行一次验证，看看交点进行坐标变换之后是否接近匹配的点
/* iPoint R = ipoint(Xr,Yr);
iPoint R2W;
TransformCoord(transPara,R,R2W);
cout<<"R2W.x "<<R2W.x<<"  R2W.y "<<R2W.y<<endl;*/

}

void Coordinate::CoortransTest(){
Coordinate coord;
CoorTransPara coordtrans;
coord.CalCoorTransPara(coordtrans,FieldLine1,FieldLine5,FieldLine2,FieldLine5);
cout<<"theta : "<<coordtrans.theta*180/PI<<" Tx: "<<coordtrans.Tx<<"  Ty: "<<coordtrans.Ty<<endl;
}

cout<<"theta : "<<coordtrans.theta*180/PI<<" Tx: "<<coordtrans.Tx<<"  Ty: "<<coordtrans.Ty<<endl;
}

void Coordinate::TransformCoord(CoorTransPara transPara,iPoint R,iPoint& W){

W.x = (int)(R.x*cos(transPara.theta) - R.y*sin(transPara.theta) );
W.y = (int)(R.x*sin(transPara.theta) + R.y*cos(transPara.theta) );
W.x = W.x + transPara.Tx ;
W.y = W.y + transPara.Ty ;
}