Skip to content

Commit

Permalink
拖尾滤波器增加角度参数
Browse files Browse the repository at this point in the history
  • Loading branch information
@zhanyiaini
zhanyiaini committed Jan 18, 2024
1 parent 370e1fc commit 3eb329a
Show file tree
Hide file tree
Showing 4 changed files with 113 additions and 62 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@ project(ydlidar_sdk C CXX)
# version
set(YDLIDAR_SDK_VERSION_MAJOR 1)
set(YDLIDAR_SDK_VERSION_MINOR 1)
set(YDLIDAR_SDK_VERSION_PATCH 22)
set(YDLIDAR_SDK_VERSION_PATCH 23)
set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})

##########################################################
Expand Down
8 changes: 7 additions & 1 deletion examples/tea_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -221,8 +221,14 @@ int main(int argc, char *argv[])
scan.config.scan_time,
scan.config.time_increment);
fflush(stdout);
for (size_t i = 0; i < scan.points.size(); ++i)
{
const LaserPoint &p = scan.points.at(i);
printf("%d d %.0f a %.02f i %.0f\n", i, p.range, p.angle * 180.0 / M_PI, p.intensity);
}

// 使用强光滤波器
filter.filter(scan, 0, 0, outScan);
// filter.filter(scan, 0, 0, outScan);
}
else
{
Expand Down
150 changes: 90 additions & 60 deletions src/filters/StrongLightFilter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,9 +4,8 @@
#include "StrongLightFilter.h"

#define MIN_VALUE 1e-8
#define IS_ZERO(d) (abs(d) < MIN_VALUE)
#define IS_EQUAL(d1, d2) IS_ZERO(d1 - d2)

#define IS_ZERO(v) (abs(v) < MIN_VALUE)
#define IS_EQUAL(v1, v2) IS_ZERO(v1 - v2)

StrongLightFilter::StrongLightFilter()
{
Expand All @@ -17,81 +16,81 @@ StrongLightFilter::~StrongLightFilter()
}

void StrongLightFilter::filter(
const LaserScan &in,
const LaserScan &in,
int lidarType,
int version,
LaserScan &out)
{
//转
int size = in.points.size(); //点数
//按角度排序
int size = in.points.size(); // 点数
// 按角度排序
std::map<float, int> ais;
for (int i=0; i<size; ++i)
for (int i = 0; i < size; ++i)
{
const LaserPoint& p = in.points.at(i);
//过滤无效点
if (p.range < in.config.min_range &&
p.range > in.config.max_range)
continue;
const LaserPoint &p = in.points.at(i);
if (IS_ZERO(p.range))
continue;
ais[p.angle] = i;
}

// printf("按角度排序从[%d]点到[%d]点\n", size, ais.size());

size = ais.size(); //更新点数（过滤无效点后的点数）
size = ais.size(); // 更新点数（过滤无效点后的点数）
out = in;
out.points.resize(size); //更新点数
out.points.resize(size); // 更新点数
std::map<float, int>::iterator it;
int i = 0;
for (it = ais.begin(); it != ais.end(); ++it)
{
out.points[i++] = in.points.at(it->second);
}
//初始化变量
std::vector<bool> noises(size, false); //是否为噪点的标记
//将遍历范围扩大到原数组的104%以便处理首尾部分的点
// 初始化变量
std::vector<bool> noises(size, false); // 是否为噪点的标记
// 将遍历范围扩大到原数组的104%以便处理首尾部分的点
int sizeEx = int(size * 1.04);
int startI = -1; //标记拖尾起始点下标位置
LaserPoint lastP; //上一个点信息
int startI = -1; // 标记拖尾起始点下标位置
LaserPoint lastP; // 上一个点信息

//主循环函数
// 主循环函数
for (int i = 0; i < sizeEx; ++i)
{
const LaserPoint& p = out.points.at(i % size);
const LaserPoint &p = out.points.at(i % size);
if (i != 0)
{
//计算两点连线到原点的距离（直角坐标系）

Point p1 = Point::polar2Angular(Point(p.angle, p.range));
Point p2 = Point::polar2Angular(Point(lastP.angle, lastP.range));
float d = Point::calcDist(Point(0, 0), p1, p2);
Point p3 = Point(0, 0); //原点
Point p4 = Point((p1.x + p2.x) / 2.0f, (p1.y + p2.y) / 2.0f); //两点中点
// 计算两点连线到原点的距离（直角坐标系）
float d = Point::calcDist(p3, p1, p2);
//计算两线段组成直线的夹角（直角坐标系）
float a = Point::calcAngle(p1, p2, p3, p4);

// printf("点[%d]距离[%.03f]\n", i % size, d);
//如果当前距离小于标准则认为是拖尾点
if (d < maxDist)
// 如果当前距离小于标准，且角度小于标准，则认为是拖尾点
if (d < maxDist && a < maxAngle)
{
//如果起始点无效则标记
// 如果起始点无效则标记
if (-1 == startI)
startI = i;
}
//如果点的距离是在增加的，则当前距离小于2倍标准也认为是拖尾点
// 如果点的距离是在增加的，且当前距离小于2倍标准，且角度小于标准，则认为是拖尾点
else if (-1 != startI &&
p.range > lastP.range &&
d < maxDist * 2)
d < maxDist * 2 &&
a < maxAngle)
{
//无处理
// 无处理
}
else
{
//判断统计位置是否有效，如果有效需要标记
// 判断统计位置是否有效，如果有效需要标记
if (-1 != startI &&
i - startI >= minNoise)
{
for (int j=startI; j<=i; ++j)
for (int j = startI; j <= i; ++j)
{
noises[j % size] = true;
const LaserPoint& pp = out.points.at(j % size);
const LaserPoint &pp = out.points.at(j % size);
// printf("噪点[%d] a[%.02f] r[%.02f]\n",
// j % size, ydlidar::core::math::to_degrees(pp.angle), pp.range);
}
Expand All @@ -103,14 +102,14 @@ void StrongLightFilter::filter(
lastP = p;
}

//处理被标记的点
// 处理被标记的点
int noiseCount = 0;
for (int i = 0; i < size; ++i)
{
if (noises[i])
{
out.points[i].range = 0.0f;
noiseCount ++;
noiseCount++;
}
}

Expand All @@ -124,16 +123,16 @@ StrongLightFilter::Point::Point(float x, float y)
}

StrongLightFilter::Point StrongLightFilter::Point::angular2Polar(
const StrongLightFilter::Point &p)
const StrongLightFilter::Point &p)
{
//1.极坐标系中的两个坐标 r 和 θ 可以由下面的公式转换为直角坐标系下的坐标值x = r*cos（θ），y = r*sin（θ）。
//2.由上述二公式，可得到从直角坐标系中x和y两坐标如何计算出极坐标下的坐标，r = sqrt(x^2 + y^2),θ = arctan(y/x)
// float r = qSqrt(x * x + y * y);
// float theta = qAtan(y / x);
// 1.极坐标系中的两个坐标 r 和 θ 可以由下面的公式转换为直角坐标系下的坐标值x = r*cos（θ），y = r*sin（θ）。
// 2.由上述二公式，可得到从直角坐标系中x和y两坐标如何计算出极坐标下的坐标，r = sqrt(x^2 + y^2),θ = arctan(y/x)
// float r = qSqrt(x * x + y * y);
// float theta = qAtan(y / x);

//将弧度值从[-M_PI/2,M_PI/2]转成[0, 2*M_PI]
// 将弧度值从[-M_PI/2,M_PI/2]转成[0, 2*M_PI]
float theta = .0;
if (!IS_ZERO(p.x)) //x不为0时
if (!IS_ZERO(p.x)) // x不为0时
{
theta = atan(p.y / p.x);
if (p.x > 0.0)
Expand All @@ -150,35 +149,66 @@ StrongLightFilter::Point StrongLightFilter::Point::angular2Polar(
}

StrongLightFilter::Point StrongLightFilter::Point::polar2Angular(
const StrongLightFilter::Point &p)
const StrongLightFilter::Point &p)
{
return Point(p.y * cos(p.x), p.y * sin(p.x));
}

float StrongLightFilter::Point::calcDist(
const StrongLightFilter::Point &p,
const StrongLightFilter::Point &p1,
const StrongLightFilter::Point &p2)
const StrongLightFilter::Point &p,
const StrongLightFilter::Point &p1,
const StrongLightFilter::Point &p2)
{
//计算点到直线的最近距离
//输入点P(x0,y0)和直线AB（x1,y1,x2,y2）,输出点到直线的最近距离
//# 如果两点相同，则输出一个点的坐标为垂足
//if x1 == x2 and y1 == y2:
// return math.sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0))
// 计算点到直线的最近距离
// 输入点P(x0,y0)和直线AB（x1,y1,x2,y2）,输出点到直线的最近距离
// # 如果两点相同，则输出一个点的坐标为垂足
// if x1 == x2 and y1 == y2:
// return math.sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0))
if (IS_EQUAL(p1.x, p2.x) &&
IS_EQUAL(p1.y, p2.y))
return sqrt((p1.x - p.x) * (p1.x - p.x) +
(p1.y - p.y) * (p1.y - p.y));
(p1.y - p.y) * (p1.y - p.y));

//# 根据向量外积计算面积
//s = (x0 - x1) * (y2 - y1) - (y0 - y1) * (x2 - x1)
// # 根据向量外积计算面积
// s = (x0 - x1) * (y2 - y1) - (y0 - y1) * (x2 - x1)
float s = (p.x - p1.x) * (p2.y - p1.y) -
(p.y - p1.y) * (p2.x - p1.x);
//# 计算直线上两点之间的距离
//d = math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
(p.y - p1.y) * (p2.x - p1.x);
// # 计算直线上两点之间的距离
// d = math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
float d = sqrt((p2.x - p1.x) * (p2.x - p1.x) +
(p2.y - p1.y) * (p2.y - p1.y));
(p2.y - p1.y) * (p2.y - p1.y));

//return math.fabs(s / d)
// return math.fabs(s / d)
return fabs(s / d);
}

float StrongLightFilter::Point::calcLen(
const StrongLightFilter::Point &v)
{
return sqrt(v.x * v.x + v.y * v.y);
}

float StrongLightFilter::Point::calcDot(
const StrongLightFilter::Point &v1,
const StrongLightFilter::Point &v2)
{
return v1.x * v2.x + v1.y * v2.y;
}

float StrongLightFilter::Point::calcAngle(
const StrongLightFilter::Point &p1,
const StrongLightFilter::Point &p2,
const StrongLightFilter::Point &p3,
const StrongLightFilter::Point &p4)
{
Point v1(p2.x - p1.x, p2.y - p1.y); //向量1
Point v2(p4.x - p3.x, p4.y - p3.y); //向量2
//计算两向量夹角（锐角）
float theta = calcDot(v1, v2) / (calcLen(v1) * calcLen(v2));
float a = acos(theta) * 180.0f / M_PI;
if (a < .0f)
a = -a;
if (a > 90.0f)
a = 180.0f - a;
return a;
}
15 changes: 15 additions & 0 deletions src/filters/StrongLightFilter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@ class YDLIDAR_API StrongLightFilter : public FilterInterface
int version,
LaserScan &out);
void setMaxDist(float dist) {maxDist = dist;}
void setMaxAngle(float angle) {maxAngle = angle;}
void setMinNoise(int noise) {minNoise = noise;}

protected:
Expand All @@ -32,9 +33,23 @@ class YDLIDAR_API StrongLightFilter : public FilterInterface
const Point &p,
const Point &p1,
const Point &p2);
//计算向量的长度
static float calcLen(
const Point &v);
//计算向量的乘积
static float calcDot(
const Point &v1,
const Point &v2);
//计算直角坐标系中两线段组成直线的夹角
static float calcAngle(
const Point &p1,
const Point &p2,
const Point &p3,
const Point &p4);
};

float maxDist = 0.05; //最大距离阈值，单位米（此值可根据需要自己修改）
float maxAngle = 12.0; //最大角度阈值，单位°（此值可根据需要自己修改）
int minNoise = 2; //最小连续噪点数（此值可根据需要自己修改）
};

Expand Down

0 comments on commit 3eb329a

Please sign in to comment.