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