310 lines
11 KiB
C
310 lines
11 KiB
C
/******************** (C) COPYRIGHT 2020 VIVO************************************
|
||
* File Name : simulator_location_pdr.c
|
||
* Department : Sensor Algorithm Team
|
||
* Current Version : V1.2
|
||
* Author : ZhangJingrui@vivo.com
|
||
* Date of Issued : 2020.7.4
|
||
* Comments : PDR 输出轨迹平滑的功能函数
|
||
********************************************************************************/
|
||
/* Header File Including -----------------------------------------------------*/
|
||
#include <stdlib.h>
|
||
#include <math.h>
|
||
#include <float.h>
|
||
#include <stddef.h>
|
||
#include <string.h>
|
||
#include "pdr_trackSmooth.h"
|
||
#include "pdr_linearFit.h"
|
||
#include "pdr_util.h"
|
||
|
||
/* Global Variable Definition ------------------------------------------------*/
|
||
|
||
/* 轨迹平滑历史输入点存储缓冲区 */
|
||
static LatLng *SmoothBuffer = NULL; //最新的点在[0]索引处
|
||
static size_t SmoothBufferSize = 0;
|
||
static size_t elemTrail = 0;
|
||
/* 输出点缓存 */
|
||
// #define OUTPUT_BUFFER_SIZE 5
|
||
static LatLng *OutputBuffer = NULL;
|
||
static size_t outputTrail = 0;
|
||
static size_t outputBufferSize = 0;
|
||
|
||
/* 拐点存储 */
|
||
static LatLng InflectionPoint = {0,0};
|
||
/* 直线拟合缓存区 */
|
||
|
||
static LatLng fittingDock[FITTING_DOCK_SIZE] = {{0,0}};
|
||
static size_t dockTrail = 0;
|
||
/* 条件阈值 */
|
||
const double thresholdDis = 5; // 拐点判断阈值
|
||
const double thresholdFittingDockSparsity = 1; // 直线拟合样本输入点间的稀疏度(即间隙)
|
||
const double thresholdFittingDis = 1; // 拟合直线更新阈值
|
||
const double half = 0.5;
|
||
|
||
/* Function Declaration ------------------------------------------------------*/
|
||
/**----------------------------------------------------------------------
|
||
* Function : pushBuffer
|
||
* Description : 将输入的位置坐标推入缓存buffer
|
||
* Date : 2020/7/4 yuanlin_rjyb@vivo.com
|
||
*---------------------------------------------------------------------**/
|
||
static void pushBuffer(LatLng inLoc){
|
||
size_t i;
|
||
for (i = outputBufferSize - 1; i > 0; --i){
|
||
OutputBuffer[i] = OutputBuffer[i-1];
|
||
}
|
||
OutputBuffer[0] = inLoc;
|
||
if(outputTrail < outputBufferSize)
|
||
++outputTrail;
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : trackSmoothingSlideWindowSet
|
||
* Description : 使用平滑模块时首先调用的函数，设定轨迹平滑窗口大小。也可以通过此函
|
||
* 数重新设定滑动窗口大小。
|
||
* Input : slideWindowSize: 滑动窗口的大小
|
||
* Output : size : 当前滑动窗口的大小，一般大于0,当返回大小为0时说明设置滑动
|
||
* 窗口为0或是内存申请失败。
|
||
* Date : 2020/7/4 yuanlin_rjyb@vivo.com
|
||
*---------------------------------------------------------------------**/
|
||
int setTrackSmoothWindow(size_t slideWindowSize)
|
||
{
|
||
LatLng *ptr = NULL;
|
||
size_t smoothBufferMallocRes = 0;
|
||
size_t OutputBufferMallocRes = 0;
|
||
|
||
ptr = (LatLng *)malloc( sizeof(LatLng) * slideWindowSize);
|
||
if (ptr != NULL)
|
||
{
|
||
smoothBufferMallocRes = slideWindowSize;
|
||
if (SmoothBuffer != NULL)
|
||
{
|
||
free(SmoothBuffer);
|
||
SmoothBuffer = NULL;
|
||
SmoothBufferSize = 0;
|
||
}
|
||
SmoothBuffer = ptr;
|
||
SmoothBufferSize = slideWindowSize;
|
||
memset(SmoothBuffer, 0, sizeof(LatLng)*SmoothBufferSize);
|
||
elemTrail = 0;
|
||
}
|
||
else
|
||
{
|
||
smoothBufferMallocRes = 0;
|
||
}
|
||
ptr = NULL;
|
||
ptr = (LatLng *)malloc( sizeof(LatLng) * slideWindowSize);
|
||
if( ptr != NULL)
|
||
{
|
||
OutputBufferMallocRes = slideWindowSize;
|
||
if(OutputBuffer != NULL)
|
||
{
|
||
free(OutputBuffer);
|
||
OutputBuffer = NULL;
|
||
outputBufferSize = 0;
|
||
}
|
||
OutputBuffer = ptr;
|
||
outputBufferSize = slideWindowSize;
|
||
memset(OutputBuffer, 0, sizeof(LatLng)*outputBufferSize);
|
||
outputTrail = 0;
|
||
}
|
||
else
|
||
{
|
||
OutputBufferMallocRes = 0;
|
||
}
|
||
/* 不论是否重新调整SmoothBuffer和SmoothBufferSize，都会重置fittingDock和dockTrail*/
|
||
memset(fittingDock, 0, sizeof(LatLng)*FITTING_DOCK_SIZE);
|
||
dockTrail = 0;
|
||
|
||
return (int)((smoothBufferMallocRes && OutputBufferMallocRes)? slideWindowSize: 0);
|
||
}
|
||
/**----------------------------------------------------------------------
|
||
* Function : trackSmoothing
|
||
* Description : 轨迹平滑模块关闭后需要使用此函数释放资源
|
||
* Input : lat: 输入纬度
|
||
* lon: 输入经度
|
||
* Output : outLat: 输出纬度存储变量指针
|
||
* outLon: 输出经度存储变量指针
|
||
* return: 返回处理状态
|
||
* Date : 2020/7/4 yuanlin_rjyb@vivo.com
|
||
*---------------------------------------------------------------------**/
|
||
int pdr_trackSmooth(double lat, double lon, double *outLat, double *outLon)
|
||
{
|
||
size_t i;
|
||
double distance;
|
||
LatLng outLatLng;
|
||
LatLng projPointLatLng;
|
||
double fittingDockLat[FITTING_DOCK_SIZE] = { 0 };
|
||
double fittingDockLon[FITTING_DOCK_SIZE] = { 0 };
|
||
LatLng tmp_lla = { 0 };
|
||
double a = 0; double b = 0; /* y = a*x + b */
|
||
// double sumLat = 0; double sumLon = 0;
|
||
int ret;
|
||
LatLng fittingProjPoint;
|
||
double distanceFitting;
|
||
if( SmoothBuffer == NULL || SmoothBufferSize <= 1)
|
||
{
|
||
*outLat = lat;
|
||
*outLon = lon;
|
||
return 1;
|
||
}
|
||
/* 配置各个缓冲区、列表的相应内容和参数*/
|
||
/* 将SmoothBuffer右移以插入新数据 */
|
||
for (i = SmoothBufferSize -1 ; i > 0 ; --i){
|
||
SmoothBuffer[i] = SmoothBuffer[i-1];
|
||
}
|
||
SmoothBuffer[0].lat = lat;
|
||
SmoothBuffer[0].lon = lon;
|
||
if (elemTrail == 0) { /* 当第一个点输入时，将其设置为第一个拐点 */
|
||
InflectionPoint.lat = lat;
|
||
InflectionPoint.lon = lon;
|
||
}
|
||
/* 及时更新elemTrail */
|
||
if (elemTrail < SmoothBufferSize ) {
|
||
++elemTrail;
|
||
}
|
||
/* 初始化用于集合的存储点数组fittingDock[FITTING_DOCK_SIZE]*/
|
||
if (dockTrail < FITTING_DOCK_SIZE) {
|
||
if (dockTrail > 0)
|
||
{ /*当不是fittingDock的首个元素时，要注意fittingDock输入样本的稀疏度*/
|
||
tmp_lla.lat = lat;
|
||
tmp_lla.lon = lon;
|
||
distance = calDistance(tmp_lla, fittingDock[dockTrail - 1]);
|
||
if (distance >= thresholdFittingDockSparsity) {
|
||
fittingDock[dockTrail].lat = lat;
|
||
fittingDock[dockTrail].lon = lon;
|
||
if (dockTrail < outputBufferSize)
|
||
++dockTrail;
|
||
}
|
||
}
|
||
else
|
||
{ /*当本次输入的坐标点是fittingDock的首个元素，直接将此坐标点作为拟合直线的起点*/
|
||
fittingDock[dockTrail].lat = lat;
|
||
fittingDock[dockTrail].lon = lon;
|
||
if (dockTrail < outputBufferSize)
|
||
++dockTrail;
|
||
}
|
||
}
|
||
/* 平滑处理逻辑开始 */
|
||
if(elemTrail < 3 ) //计算拐点的前提是输入点至少有3个
|
||
{
|
||
*outLat = lat;
|
||
*outLon = lon;
|
||
outLatLng.lat = lat;
|
||
outLatLng.lon = lon;
|
||
pushBuffer(outLatLng);
|
||
|
||
}
|
||
else
|
||
{
|
||
projPointLatLng = ProjPointOfLatLng(SmoothBuffer[1], InflectionPoint, SmoothBuffer[0]);
|
||
distance = calDistance(SmoothBuffer[1], projPointLatLng);
|
||
/*判断SmoothBuffer[1]到预测直线的距离distance，根据距离的大小执行不同的逻辑*/
|
||
if( distance > thresholdDis)
|
||
{
|
||
/*输入坐标点被判断为拐点后，要更新拐点，清空拟合点集fittingDock*/
|
||
/* 输出点(lat, lon) */
|
||
outLatLng.lat = SmoothBuffer[1].lat;
|
||
outLatLng.lon = SmoothBuffer[1].lon;
|
||
pushBuffer(outLatLng);
|
||
/* 将(lat, lon)作为新的拐点更新 InflectionPoint */
|
||
InflectionPoint.lat = lat;
|
||
InflectionPoint.lon = lon;
|
||
/* 拐点更新后，重新设置fittingDock中的数值内容 */
|
||
memset(fittingDock, 0, sizeof(LatLng) * FITTING_DOCK_SIZE);
|
||
fittingDock[0].lat = lat;
|
||
fittingDock[0].lon = lon;
|
||
dockTrail = 1;
|
||
}
|
||
else
|
||
{
|
||
/*当输入点被判断为非拐点后，根据拟合点集fittingDock的大小执行不同的逻辑*/
|
||
if( dockTrail >= FITTING_DOCK_SIZE)
|
||
{
|
||
|
||
for ( i = 0 ; i < FITTING_DOCK_SIZE ; ++i)
|
||
{
|
||
fittingDockLat[i] = fittingDock[i].lat;
|
||
fittingDockLon[i] = fittingDock[i].lon;
|
||
}
|
||
ret = linearLeastSquaresFit(fittingDockLat, fittingDockLon, FITTING_DOCK_SIZE, &a, &b);
|
||
fittingProjPoint = SmoothBuffer[1];
|
||
if (ret == 0) /* ret==0表示直线拟合成功 */
|
||
{
|
||
fittingProjPoint.lat = (a*(SmoothBuffer[1].lon - b) + SmoothBuffer[1].lat) / (vivopow(a, 2) + 1);
|
||
fittingProjPoint.lon = a*fittingProjPoint.lat + b;
|
||
}
|
||
distanceFitting = calDistance(SmoothBuffer[1], fittingProjPoint);
|
||
if( distanceFitting > thresholdFittingDis)
|
||
{
|
||
// //FIXME: 射线拟合可能有逻辑漏洞，修复。
|
||
// dockTrail = 2;
|
||
// fittingDock[1] = fittingProjPoint;
|
||
// //fittingDock[1] = SmoothBuffer[1]; //TEST
|
||
// LatLng outLatLng = {lat, lon}; //有问题，漏更一个点
|
||
// pushBuffer(outLatLng);
|
||
//TODO: 测试新的添点策略
|
||
dockTrail = 2; /* remove the elements whose the index >= 3 */
|
||
fittingDock[1] = SmoothBuffer[1];
|
||
outLatLng.lat = SmoothBuffer[1].lat;
|
||
outLatLng.lon = SmoothBuffer[1].lon;
|
||
pushBuffer(outLatLng);
|
||
}
|
||
else
|
||
{
|
||
outLatLng.lat = fittingProjPoint.lat;
|
||
outLatLng.lon = fittingProjPoint.lon;
|
||
pushBuffer(outLatLng);
|
||
}
|
||
}else{
|
||
/*当fittingDock未被装满，则说明当前输入点不符合fittingDock的输入条件，直接将其输入以保证实时性*/
|
||
outLatLng.lat = lat;
|
||
outLatLng.lon = lon;
|
||
pushBuffer(outLatLng);
|
||
}
|
||
}
|
||
|
||
/* 2019-05-22 平滑改动：由均值滤波变成加权求和*/
|
||
double factor = 0;
|
||
if (outputTrail > 1)
|
||
{
|
||
factor = half / (outputTrail - 1);
|
||
}
|
||
double sumLat = 0; double sumLon = 0;
|
||
for (i = 0; i < outputTrail; ++i)
|
||
{
|
||
sumLat += OutputBuffer[i].lat * factor;
|
||
sumLon += OutputBuffer[i].lon * factor;
|
||
}
|
||
*outLat = sumLat + OutputBuffer[0].lat * (half - factor);
|
||
*outLon = sumLon + OutputBuffer[0].lon * (half - factor);
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : trackSmoothSlideWindowFree
|
||
* Description : 轨迹平滑模块关闭后需要使用此函数释放资源
|
||
* Date : 2020/7/4 yuanlin_rjyb@vivo.com
|
||
*---------------------------------------------------------------------**/
|
||
int freeTrackSmooth(void)
|
||
{
|
||
if ( SmoothBuffer != NULL)
|
||
{
|
||
free(SmoothBuffer);
|
||
SmoothBuffer = NULL;
|
||
SmoothBufferSize = 0;
|
||
elemTrail = 0;
|
||
}
|
||
if ( OutputBuffer != NULL)
|
||
{
|
||
free(OutputBuffer);
|
||
OutputBuffer = NULL;
|
||
outputBufferSize = 0;
|
||
outputTrail = 0;
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
|