pull/5/head
laptoy 6 months ago
parent e7778c32b7
commit f6130b2d1f

@ -1,11 +1,4 @@
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "GPSManager.h" #include "GPSManager.h"

@ -1,11 +1,3 @@
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#pragma once #pragma once

@ -1,11 +1,4 @@
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#pragma once #pragma once

@ -1,11 +1,4 @@
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "GPSProvider.h" #include "GPSProvider.h"

@ -1,12 +1,3 @@
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#pragma once #pragma once

@ -1,11 +1,4 @@
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "RTCMMavlink.h" #include "RTCMMavlink.h"

@ -1,11 +1,4 @@
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#pragma once #pragma once

@ -1,35 +1,4 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/** /**
* @file definitions.h * @file definitions.h

@ -1,35 +1,4 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once #pragma once
#include <stdint.h> #include <stdint.h>

@ -1,37 +1,4 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/* Auto-generated by genmsg_cpp from file /home/beat/px4/src/Firmware/msg/vehicle_gps_position.msg */
#pragma once #pragma once

@ -1,43 +1,3 @@
/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
// License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2010-2013, University of Nizhny Novgorod, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
//Modified from latentsvm module's "lsvmc_featurepyramid.cpp". //Modified from latentsvm module's "lsvmc_featurepyramid.cpp".
@ -77,60 +37,92 @@
// RESULT // RESULT
// Error status // Error status
*/ */
// 从输入的图像中提取特征映射
int getFeatureMaps(const IplImage* image, const int k, CvLSVMFeatureMapCaskade **map) int getFeatureMaps(const IplImage* image, const int k, CvLSVMFeatureMapCaskade **map)
{ {
// 图像的宽度和高度
int sizeX, sizeY; int sizeX, sizeY;
int p, px, stringSize; // 每个cell中的特征数量
int p, px;
// 存储特征的字符串大小
int stringSize;
// 图像的高度、宽度和通道数
int height, width, numChannels; int height, width, numChannels;
// 循环变量
int i, j, kk, c, ii, jj, d; int i, j, kk, c, ii, jj, d;
// 梯度数据指针
float * datadx, * datady; float * datadx, * datady;
int ch; // 角度相关变量
int ch;
float magnitude, x, y, tx, ty; float magnitude, x, y, tx, ty;
// 梯度图像
IplImage * dx, * dy; IplImage * dx, * dy;
// 最近的邻域点索引
int *nearest; int *nearest;
float *w, a_x, b_x; // 权重向量
float *w;
// 线性插值系数
float a_x, b_x;
// 梯度算子
float kernel[3] = {-1.f, 0.f, 1.f}; float kernel[3] = {-1.f, 0.f, 1.f};
// OpenCV矩阵格式的梯度算子
CvMat kernel_dx = cvMat(1, 3, CV_32F, kernel); CvMat kernel_dx = cvMat(1, 3, CV_32F, kernel);
CvMat kernel_dy = cvMat(3, 1, CV_32F, kernel); CvMat kernel_dy = cvMat(3, 1, CV_32F, kernel);
// 临时指针
float * r; float * r;
// 未知用途的整型数组(可能是用于后续处理的索引或标志)
int * alfa; int * alfa;
// 边界角度值数组
float boundary_x[NUM_SECTOR + 1]; float boundary_x[NUM_SECTOR + 1];
float boundary_y[NUM_SECTOR + 1]; float boundary_y[NUM_SECTOR + 1];
// 用于查找最大值的变量
float max, dotProd; float max, dotProd;
int maxi; int maxi;
// 获取图像的高度和宽度
height = image->height; height = image->height;
width = image->width ; width = image->width;
// 获取图像的通道数
numChannels = image->nChannels; numChannels = image->nChannels;
dx = cvCreateImage(cvSize(image->width, image->height), // 创建用于存储梯度图像的容器
dx = cvCreateImage(cvSize(image->width, image->height),
IPL_DEPTH_32F, 3); IPL_DEPTH_32F, 3);
dy = cvCreateImage(cvSize(image->width, image->height), dy = cvCreateImage(cvSize(image->width, image->height),
IPL_DEPTH_32F, 3); IPL_DEPTH_32F, 3);
// 计算特征映射的宽和高基于k的大小
sizeX = width / k; sizeX = width / k;
sizeY = height / k; sizeY = height / k;
px = 3 * NUM_SECTOR; // 计算每个cell中的特征数量基于NUM_SECTOR
px = 3 * NUM_SECTOR;
p = px; p = px;
// 计算整个特征映射所需的总存储大小
stringSize = sizeX * p; stringSize = sizeX * p;
// 分配特征映射对象的内存
allocFeatureMapObject(map, sizeX, sizeY, p); allocFeatureMapObject(map, sizeX, sizeY, p);
// 使用Sobel算子计算图像的梯度
cvFilter2D(image, dx, &kernel_dx, cvPoint(-1, 0)); cvFilter2D(image, dx, &kernel_dx, cvPoint(-1, 0));
cvFilter2D(image, dy, &kernel_dy, cvPoint(0, -1)); cvFilter2D(image, dy, &kernel_dy, cvPoint(0, -1));
// 计算边界角度值
float arg_vector; float arg_vector;
for(i = 0; i <= NUM_SECTOR; i++) for(i = 0; i <= NUM_SECTOR; i++)
{ {
// 计算角度基于NUM_SECTOR将圆周等分为NUM_SECTOR个部分
arg_vector = ( (float) i ) * ( (float)(PI) / (float)(NUM_SECTOR) ); arg_vector = ( (float) i ) * ( (float)(PI) / (float)(NUM_SECTOR) );
// 计算对应的余弦和正弦值
boundary_x[i] = cosf(arg_vector); boundary_x[i] = cosf(arg_vector);
boundary_y[i] = sinf(arg_vector); boundary_y[i] = sinf(arg_vector);
}/*for(i = 0; i <= NUM_SECTOR; i++) */ }
r = (float *)malloc( sizeof(float) * (width * height)); r = (float *)malloc( sizeof(float) * (width * height));
alfa = (int *)malloc( sizeof(int ) * (width * height * 2)); alfa = (int *)malloc( sizeof(int ) * (width * height * 2));
@ -287,43 +279,63 @@ int getFeatureMaps(const IplImage* image, const int k, CvLSVMFeatureMapCaskade *
// RESULT // RESULT
// Error status // Error status
*/ */
// 对特征映射进行归一化和截断处理
int normalizeAndTruncate(CvLSVMFeatureMapCaskade *map, const float alfa) int normalizeAndTruncate(CvLSVMFeatureMapCaskade *map, const float alfa)
{ {
int i,j, ii; int i, j, ii; // 定义循环变量
int sizeX, sizeY, p, pos, pp, xp, pos1, pos2; int sizeX, sizeY, p, pos, pp, xp; // 定义变量
float * partOfNorm; // norm of C(i, j) float * partOfNorm; // 存储C(i, j)的范数的指针
float * newData; float * newData; // 存储归一化后数据的指针
float valOfNorm; float valOfNorm; // 存储范数的值
sizeX = map->sizeX; sizeX = map->sizeX; // 获取特征映射的宽度
sizeY = map->sizeY; sizeY = map->sizeY; // 获取特征映射的高度
partOfNorm = (float *)malloc (sizeof(float) * (sizeX * sizeY)); partOfNorm = (float *)malloc (sizeof(float) * (sizeX * sizeY)); // 分配存储范数的内存
p = NUM_SECTOR; p = NUM_SECTOR; // 定义一个常量,可能表示特征的分区数
xp = NUM_SECTOR * 3; xp = NUM_SECTOR * 3; // 定义一个常量,可能表示特征的某种属性
pp = NUM_SECTOR * 12; pp = NUM_SECTOR * 12; // 定义一个常量,可能表示特征的某种属性
// 计算每个部分区域的范数
for(i = 0; i < sizeX * sizeY; i++) for(i = 0; i < sizeX * sizeY; i++)
{ {
valOfNorm = 0.0f; valOfNorm = 0.0f; // 初始化范数为0
pos = i * map->numFeatures; pos = i * map->numFeatures; // 计算每个部分的起始位置
for(j = 0; j < p; j++) for(j = 0; j < p; j++) // 遍历每个部分的特征
{ {
valOfNorm += map->map[pos + j] * map->map[pos + j]; valOfNorm += map->map[pos + j] * map->map[pos + j]; // 计算每个部分的范数
}/*for(j = 0; j < p; j++)*/ }/*for(j = 0; j < p; j++)*/
partOfNorm[i] = valOfNorm; partOfNorm[i] = valOfNorm; // 将每个部分的范数存储起来
}/*for(i = 0; i < sizeX * sizeY; i++)*/ }/*for(i = 0; i < sizeX * sizeY; i++)*/
// 调整特征映射的大小
sizeX -= 2; sizeX -= 2;
sizeY -= 2; sizeY -= 2;
// 分配存储归一化后数据的内存
newData = (float *)malloc (sizeof(float) * (sizeX * sizeY * pp)); newData = (float *)malloc (sizeof(float) * (sizeX * sizeY * pp));
//normalization
// 归一化处理
for(i = 1; i <= sizeY; i++) for(i = 1; i <= sizeY; i++)
{ {
for(j = 1; j <= sizeX; j++) for(j = 1; j <= sizeX; j++)
{ {
valOfNorm = sqrtf( valOfNorm = sqrtf( // 计算当前像素的范数
partOfNorm[(i )*(sizeX + 2) + (j )] +
partOfNorm[(i )*(sizeX + 2) + (j + 1)] +
partOfNorm[(i + 1)*(sizeX + 2) + (j )] +
partOfNorm[(i + 1)*(sizeX + 2) + (j + 1)]) + FLT_EPSILON;
pos1 = (i ) * (sizeX + 2) * xp + (j ) * xp;
pos2 = (i-1) * (sizeX ) * pp + (j-1) * pp;
for(ii = 0; ii < p; ii++)
{
newData[pos2 + ii ] = map->map[pos1 + ii ] / valOfNorm; // 归一化
}/*for(ii = 0; ii < p; ii++)*/
for(ii = 0; ii < 2 * p; ii++)
{
newData[pos2 + ii + p * 4] = map->map[pos1 + ii + p] / valOfNorm; // 归一化
}/*for(ii = 0; ii < 2 * p; ii++)*/
valOfNorm = sqrtf( // 计算当前像素的范数
partOfNorm[(i )*(sizeX + 2) + (j )] + partOfNorm[(i )*(sizeX + 2) + (j )] +
partOfNorm[(i )*(sizeX + 2) + (j + 1)] + partOfNorm[(i )*(sizeX + 2) + (j + 1)] +
partOfNorm[(i + 1)*(sizeX + 2) + (j )] + partOfNorm[(i + 1)*(sizeX + 2) + (j )] +
@ -411,102 +423,159 @@ int normalizeAndTruncate(CvLSVMFeatureMapCaskade *map, const float alfa)
// RESULT // RESULT
// Error status // Error status
*/ */
// 对输入的CvLSVMFeatureMapCaskade对象进行PCA特征映射处理
int PCAFeatureMaps(CvLSVMFeatureMapCaskade *map) int PCAFeatureMaps(CvLSVMFeatureMapCaskade *map)
{ {
int i,j, ii, jj, k; int i, j, ii, jj, k;
int sizeX, sizeY, p, pp, xp, yp, pos1, pos2; int sizeX, sizeY, p, pp, xp, yp, pos1, pos2;
float * newData; float * newData;
float val; float val;
float nx, ny; float nx, ny;
// 获取特征映射的宽、高和特征数量
sizeX = map->sizeX; sizeX = map->sizeX;
sizeY = map->sizeY; sizeY = map->sizeY;
p = map->numFeatures; p = map->numFeatures;
// 定义新的特征数量pp和xp、yp等参数
pp = NUM_SECTOR * 3 + 4; pp = NUM_SECTOR * 3 + 4;
yp = 4; yp = 4;
xp = NUM_SECTOR; xp = NUM_SECTOR;
// 计算nx和ny两个归一化因子
nx = 1.0f / sqrtf((float)(xp * 2)); nx = 1.0f / sqrtf((float)(xp * 2));
ny = 1.0f / sqrtf((float)(yp )); ny = 1.0f / sqrtf((float)(yp));
// 为新的特征映射数据分配内存
newData = (float *)malloc (sizeof(float) * (sizeX * sizeY * pp)); newData = (float *)malloc (sizeof(float) * (sizeX * sizeY * pp));
if (newData == NULL) {
// 如果内存分配失败,应返回错误代码
// 这里为了简洁省略了错误处理
return LATENT_SVM_MEM_ERROR; // 假设的错误码
}
// 遍历特征映射的每一个位置
for(i = 0; i < sizeY; i++) for(i = 0; i < sizeY; i++)
{ {
for(j = 0; j < sizeX; j++) for(j = 0; j < sizeX; j++)
{ {
// 计算原始数据在当前位置的索引pos1
pos1 = ((i)*sizeX + j)*p; pos1 = ((i)*sizeX + j)*p;
// 计算新数据在当前位置的索引pos2
pos2 = ((i)*sizeX + j)*pp; pos2 = ((i)*sizeX + j)*pp;
k = 0;
k = 0; // 初始化k用于索引新数据的位置
// 计算第一个部分的新特征
for(jj = 0; jj < xp * 2; jj++) for(jj = 0; jj < xp * 2; jj++)
{ {
val = 0; val = 0; // 初始化累加值
// 遍历yp个原始特征值进行累加
for(ii = 0; ii < yp; ii++) for(ii = 0; ii < yp; ii++)
{ {
val += map->map[pos1 + yp * xp + ii * xp * 2 + jj]; val += map->map[pos1 + yp * xp + ii * xp * 2 + jj];
}/*for(ii = 0; ii < yp; ii++)*/ }
// 将累加值乘以ny并存储到新数据中
newData[pos2 + k] = val * ny; newData[pos2 + k] = val * ny;
k++; k++;
}/*for(jj = 0; jj < xp * 2; jj++)*/ }
// 计算第二个部分的新特征
for(jj = 0; jj < xp; jj++) for(jj = 0; jj < xp; jj++)
{ {
val = 0; val = 0; // 初始化累加值
// 遍历yp个原始特征值进行累加
for(ii = 0; ii < yp; ii++) for(ii = 0; ii < yp; ii++)
{ {
val += map->map[pos1 + ii * xp + jj]; val += map->map[pos1 + ii * xp + jj];
}/*for(ii = 0; ii < yp; ii++)*/ }
// 将累加值乘以ny并存储到新数据中
newData[pos2 + k] = val * ny; newData[pos2 + k] = val * ny;
k++; k++;
}/*for(jj = 0; jj < xp; jj++)*/ }
// 计算第三个部分的新特征
for(ii = 0; ii < yp; ii++) for(ii = 0; ii < yp; ii++)
{ {
val = 0; val = 0; // 初始化累加值
// 遍历2*xp个原始特征值进行累加
for(jj = 0; jj < 2 * xp; jj++) for(jj = 0; jj < 2 * xp; jj++)
{ {
val += map->map[pos1 + yp * xp + ii * xp * 2 + jj]; val += map->map[pos1 + yp * xp + ii * xp * 2 + jj];
}/*for(jj = 0; jj < xp; jj++)*/ }
// 将累加值乘以nx并存储到新数据中
newData[pos2 + k] = val * nx; newData[pos2 + k] = val * nx;
k++; k++;
} /*for(ii = 0; ii < yp; ii++)*/ }
}/*for(j = 0; j < sizeX; j++)*/ }
}/*for(i = 0; i < sizeY; i++)*/ }
//swop data
// 交换数据,这里看起来没有实际的交换操作,可能是对数据的后续处理或注释有误
// 更新特征数量
map->numFeatures = pp; map->numFeatures = pp;
// 释放原始特征映射数据占用的内存
free (map->map); free (map->map);
// 将新的特征映射数据赋值给map的map成员
map->map = newData; map->map = newData;
// 返回成功码
return LATENT_SVM_OK; return LATENT_SVM_OK;
} }
// 分配一个CvLSVMFeatureMapCaskade对象并初始化其成员
//modified from "lsvmc_routine.cpp" int allocFeatureMapObject(CvLSVMFeatureMapCaskade **obj, const int sizeX,
//两个函数分别用于分配和释放CvLSVMFeatureMapCaskade结构体的内存。
int allocFeatureMapObject(CvLSVMFeatureMapCaskade **obj, const int sizeX,
const int sizeY, const int numFeatures) const int sizeY, const int numFeatures)
{ {
int i; int i;
// 使用malloc为CvLSVMFeatureMapCaskade对象分配内存
(*obj) = (CvLSVMFeatureMapCaskade *)malloc(sizeof(CvLSVMFeatureMapCaskade)); (*obj) = (CvLSVMFeatureMapCaskade *)malloc(sizeof(CvLSVMFeatureMapCaskade));
if (!(*obj)) { // 如果分配失败,应该返回一个错误代码,但这里假设它总是成功的
// 实际应用中应该添加错误处理
}
// 初始化对象的成员变量
(*obj)->sizeX = sizeX; (*obj)->sizeX = sizeX;
(*obj)->sizeY = sizeY; (*obj)->sizeY = sizeY;
(*obj)->numFeatures = numFeatures; (*obj)->numFeatures = numFeatures;
(*obj)->map = (float *) malloc(sizeof (float) *
// 使用malloc为map成员分配内存它用于存储特征映射数据
(*obj)->map = (float *) malloc(sizeof (float) *
(sizeX * sizeY * numFeatures)); (sizeX * sizeY * numFeatures));
if (!(*obj)->map) { // 如果分配失败,应该返回一个错误代码,但这里假设它总是成功的
// 实际应用中应该添加错误处理,并释放已经为对象分配的内存
free(*obj);
*obj = NULL;
return LATENT_SVM_MEM_ERROR; // 假设这是一个内存分配错误的返回码
}
// 将map中的所有元素初始化为0.0f
for(i = 0; i < sizeX * sizeY * numFeatures; i++) for(i = 0; i < sizeX * sizeY * numFeatures; i++)
{ {
(*obj)->map[i] = 0.0f; (*obj)->map[i] = 0.0f;
} }
// 返回成功码
return LATENT_SVM_OK; return LATENT_SVM_OK;
} }
// 释放一个CvLSVMFeatureMapCaskade对象占用的内存
int freeFeatureMapObject (CvLSVMFeatureMapCaskade **obj) int freeFeatureMapObject (CvLSVMFeatureMapCaskade **obj)
{ {
// 如果obj为空返回内存为空的错误码
if(*obj == NULL) return LATENT_SVM_MEM_NULL; if(*obj == NULL) return LATENT_SVM_MEM_NULL;
// 释放map成员占用的内存
free((*obj)->map); free((*obj)->map);
// 释放对象本身占用的内存
free(*obj); free(*obj);
// 将指针设置为NULL避免野指针
(*obj) = NULL; (*obj) = NULL;
// 返回成功码
return LATENT_SVM_OK; return LATENT_SVM_OK;
} }

@ -0,0 +1,363 @@
// Track Object---advanced by Wangmingqiang -----------------------------------------
// 2024.6.01 at Hunan Changsha.
#include <math.h>
#include <string>
#include <vector>
#include <iostream>
#include <pthread.h>
#include <thread>
#include <chrono>
#include <boost/thread/mutex.hpp>
#include <boost/thread/shared_mutex.hpp>
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Pose2D.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/ml.hpp>
#include <std_srvs/SetBool.h>
#include "kcftracker.hpp"
#include "prometheus_gimbal_control/VisionDiff.h"
#include "gimbal_track/WindowPosition.h"
using namespace std;
using namespace cv;
#define MARKER_SIZE 0.18
#define F1 300
#define F2 300
#define C1 320
#define C2 240
static const std::string RGB_WINDOW = "RGB Image window";
//! Camera related parameters.
int frameWidth_;
int frameHeight_;
float get_ros_time(ros::Time begin); //获取ros当前时间
std_msgs::Header imageHeader_;
cv::Mat camImageCopy_;
boost::shared_mutex mutexImageCallback_;
bool imageStatus_ = false;
boost::shared_mutex mutexImageStatus_;
/*在接收到ROS图像话题消息时被调用。这个函数用于处理从摄像头或其他图像源接收到的图像数据并将其转换为OpenCV格式。*/
// 图像话题回调函数,用于处理接收到的图像消息
void cameraCallback(const sensor_msgs::ImageConstPtr &msg)
{
ROS_DEBUG("[EllipseDetector] USB image received."); // 打印调试信息,表示收到了图像消息
cv_bridge::CvImagePtr cam_image; // 定义一个OpenCV图像指针
// 尝试将ROS图像消息转换为OpenCV图像
try
{
cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); // 转换为BGR8格式的OpenCV图像
imageHeader_ = msg->header; // 保存图像消息的头部信息
}
catch (cv_bridge::Exception &e) // 捕获cv_bridge的异常
{
ROS_ERROR("cv_bridge exception: %s", e.what()); // 打印错误信息
return; // 结束函数调用
}
// 如果图像转换成功
if (cam_image)
{
// 使用互斥锁来保护图像数据的复制过程
{
boost::unique_lock<boost::shared_mutex> lockImageCallback(mutexImageCallback_);
camImageCopy_ = cam_image->image.clone(); // 克隆图像到全局变量
}
// 使用互斥锁来更新图像状态
{
boost::unique_lock<boost::shared_mutex> lockImageStatus(mutexImageStatus_);
imageStatus_ = true; // 设置图像状态为已接收
}
frameWidth_ = cam_image->image.size().width; // 保存图像宽度
frameHeight_ = cam_image->image.size().height; // 保存图像高度
}
return; // 结束函数调用
}
// 用此函数查看是否收到图像话题
bool getImageStatus(void)
{
boost::shared_lock<boost::shared_mutex> lock(mutexImageStatus_);
return imageStatus_;
}
//! ROS subscriber and publisher.
image_transport::Subscriber imageSubscriber_;
image_transport::Publisher image_vision_pub;
ros::Publisher pose_pub;
cv::Rect selectRect;
cv::Point origin;
cv::Rect result;
bool select_flag = false;
bool bRenewROI = false; // the flag to enable the implementation of KCF algorithm for the new chosen ROI
bool bBeginKCF = false;
int g_control_gimbal = 1;
float get_ros_time(ros::Time begin)
{
ros::Time time_now = ros::Time::now();
float currTimeSec = time_now.sec - begin.sec;
float currTimenSec = time_now.nsec / 1e9 - begin.nsec / 1e9;
return (currTimeSec + currTimenSec);
}
void bboxDrawCb(const gimbal_track::WindowPosition::ConstPtr &msg)
{
if (msg->mode != 0)
{
selectRect.x = msg->origin_x;
selectRect.y = msg->origin_y;
selectRect.width = msg->width;
selectRect.height = msg->height;
selectRect &= cv::Rect(0, 0, frameWidth_, frameHeight_);
if (selectRect.width * selectRect.height > 64)
{
bRenewROI = true;
}
g_control_gimbal = 1;
}
else
{
g_control_gimbal = 0;
}
}
/*在OpenCV中用于处理鼠标事件。这个函数在用户与图像窗口交互时被调用
ROI*/
// 鼠标事件回调函数
void onMouse(int event, int x, int y, int, void *)
{
// 如果正在选择区域
if (select_flag)
{
// 更新选择区域的坐标和大小
selectRect.x = MIN(origin.x, x); // 选择区域的左上角x坐标是最小值
selectRect.y = MIN(origin.y, y); // 选择区域的左上角y坐标是最小值
selectRect.width = abs(x - origin.x); // 选择区域的宽度是鼠标移动的水平距离
selectRect.height = abs(y - origin.y); // 选择区域的高度是鼠标移动的垂直距离
// 限制选择区域在图像范围内
selectRect &= cv::Rect(0, 0, frameWidth_, frameHeight_);
}
// 如果是鼠标左键按下事件
if (event == CV_EVENT_LBUTTONDOWN)
{
bBeginKCF = false; // 标志位表示不开始KCF跟踪
select_flag = true; // 标志位,表示开始选择区域
origin = cv::Point(x, y); // 记录鼠标按下的起始点
selectRect = cv::Rect(x, y, 0, 0); // 初始化选择区域
}
// 如果是鼠标左键释放事件
else if (event == CV_EVENT_LBUTTONUP)
{
// 如果选择区域的面积小于64可能是一个无效的选择
if (selectRect.width * selectRect.height < 64)
{
// 不执行任何操作
}
else
{
select_flag = false; // 标志位,表示结束选择区域
bRenewROI = true; // 标志位表示需要更新ROI
}
}
}
/*定义了一个ROS服务回调函数gimbalSer它用于处理一个名为/detection/gimbal_control的服务请求。
gimbal*/
// 服务回调函数,用于处理云台控制服务请求
bool gimbalSer(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &resp)
{
// 如果请求的数据为真(通常表示关闭云台控制)
if (req.data)
{
g_control_gimbal = 0; //// 设置全局变量,表示关闭云台控制
}
// 如果请求的数据为假(通常表示开启云台控制)
else if (selectRect.width * selectRect.height > 0)
{
bRenewROI = true; // 设置标志表示需要更新跟踪的区域ROI
g_control_gimbal = 1; // 设置全局变量,表示开启云台控制
}
else
{
bRenewROI = false; // 设置标志,表示不需要更新跟踪的区域
bBeginKCF = false; // 设置标志表示不需要开始KCF跟踪
}
// 设置响应的成功标志为真
resp.success = true;
// 根据请求的数据设置响应的消息
resp.message = req.data ? "Gimbal Control Close" : "Gimbal Control Open";
// 返回真,表示服务处理成功
return true;
}
bool HOG = true;
bool FIXEDWINDOW = false;
bool MULTISCALE = true;
bool SILENT = true;
bool LAB = false;
// Create KCFTracker object
KCFTracker tracker(HOG, FIXEDWINDOW, MULTISCALE, LAB);
int main(int argc, char **argv)
{
ros::init(argc, argv, "tracker_ros"); // // 初始化ROS节点
ros::NodeHandle nh("~"); // 创建节点句柄
image_transport::ImageTransport it(nh); // 图像传输对象
ros::Rate loop_rate(30); // 定义循环频率
bool auto_zoom, show_ui;
float max_size, min_size;
nh.param<bool>("auto_zoom", auto_zoom, false); // 从参数服务器获取参数
nh.param<bool>("show_ui", show_ui, true);
nh.param<float>("max_size", max_size, 0.0);
nh.param<float>("min_size", min_size, 0.0);
std::cout << "auto_zoom: " << auto_zoom << " "
<< "max_size: " << max_size << " "
<< "min_size: " << min_size << std::endl;
// 接收图像的话题
imageSubscriber_ = it.subscribe("/gimbal/image_raw", 1, cameraCallback);
// 发送绘制图像
image_vision_pub = it.advertise("/detection/image", 1);
// diff
ros::Publisher position_diff_pub = nh.advertise<prometheus_gimbal_control::VisionDiff>("/gimbal/track", 10);
// ros::Publisher auto_zoom_pub = nh.advertise<prometheus_gimbal_control::Diff>("/gimbal_server/auto_zoom", 10);
ros::Subscriber sub_bbox_draw = nh.subscribe("/detection/bbox_draw", 10, bboxDrawCb);
ros::ServiceServer server = nh.advertiseService("/detection/gimbal_control", gimbalSer);
sensor_msgs::ImagePtr msg_ellipse;
const auto wait_duration = std::chrono::milliseconds(2000);
if (show_ui)
{
cv::namedWindow(RGB_WINDOW);
cv::setMouseCallback(RGB_WINDOW, onMouse, 0);
}
float cur_time;
float last_time;
float last_error_x, last_error_y;
float dt;
prometheus_gimbal_control::VisionDiff error_pixels;
ros::Time begin_time = ros::Time::now();
while (ros::ok()) // 循环只要ROS节点正常运行就继续执行
{
// 获取当前ROS时间与开始时间的差值
cur_time = get_ros_time(begin_time);
dt = (cur_time - last_time); // 计算时间差
if (dt > 1.0 || dt < 0.0) // 如果时间差大于1秒或小于0秒则设置为0.05秒
{
dt = 0.05;
}
// 等待直到接收到图像
while (!getImageStatus())
{
printf("Waiting for image.\n"); // 打印等待图像的消息
std::this_thread::sleep_for(wait_duration); // 休眠一段时间
ros::spinOnce(); // 更新ROS事件循环
}
Mat frame; // 定义一个OpenCV图像矩阵
{
boost::unique_lock<boost::shared_mutex> lockImageCallback(mutexImageCallback_);
frame = camImageCopy_.clone(); // 克隆图像到局部变量
}
if (bRenewROI) // 如果需要更新ROI
{
tracker.init(selectRect, frame); // 初始化跟踪器
cv::rectangle(frame, selectRect, cv::Scalar(255, 0, 0), 2, 8, 0); // 在图像上绘制ROI
bRenewROI = false; // 重置更新ROI的标志
bBeginKCF = true; // 开始KCF跟踪
}
else if (bBeginKCF) // 如果已经开始KCF跟踪
{
result = tracker.update(frame); // 更新跟踪结果
error_pixels.detect = 1; // 设置检测到的标志
error_pixels.objectX = result.x; // 对象在图像中的位置
error_pixels.objectY = result.y;
error_pixels.objectWidth = result.width;
error_pixels.objectHeight = result.height;
error_pixels.frameWidth = frameWidth_; // 图像的宽度
error_pixels.frameHeight = frameHeight_; // 图像的高度
error_pixels.currSize = (float)result.width * (float)result.height / (frameHeight_ * frameWidth_); // 当前对象的大小
error_pixels.maxSize = (float)selectRect.width * (float)selectRect.height / (frameHeight_ * frameWidth_); // 最大对象大小
cv::rectangle(frame, result, cv::Scalar(255, 0, 0), 2, 8, 0); // 在图像上绘制跟踪结果
}
else // 如果未开始KCF跟踪
{
error_pixels.detect = 0; // 设置未检测到的标志
}
error_pixels.kp = 0.2; // 比例常数
error_pixels.ki = 0.0001; // 积分常数
error_pixels.kd = 0.003; // 微分常数
if (max_size != 0 && min_size != 0 && auto_zoom) // 如果自动缩放被启用
{
error_pixels.maxSize = max_size; // 设置最大对象大小
error_pixels.minSize = min_size; // 设置最小对象大小
}
error_pixels.autoZoom = auto_zoom; // 设置自动缩放标志
error_pixels.trackIgnoreError = 35; // 设置跟踪忽略误差
if (g_control_gimbal == 0) // 如果云台控制关闭
{
error_pixels.detect = 0; // 设置检测到的标志为0
}
position_diff_pub.publish(error_pixels); // 发布位置差异信息
// auto_zoom_pub.publish(error_pixels);
float left_point = frame.cols / 2 - 20;
float right_point = frame.cols / 2 + 20;
float up_point = frame.rows / 2 + 20;
float down_point = frame.rows / 2 - 20;
// draw
line(frame, Point(left_point, frame.rows / 2), Point(right_point, frame.rows / 2), Scalar(0, 255, 0), 1, 8);
line(frame, Point(frame.cols / 2, down_point), Point(frame.cols / 2, up_point), Scalar(0, 255, 0), 1, 8);
putText(frame, "x:", Point(50, 60), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 23, 0), 3, 8);
putText(frame, "y:", Point(50, 90), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 23, 0), 3, 8);
// draw
char s[20] = "";
sprintf(s, "%.2f", float(result.x + result.width / 2 - frame.cols / 2));
putText(frame, s, Point(100, 60), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 23, 0), 2, 8);
sprintf(s, "%.2f", float(result.y + result.height / 2 - frame.rows / 2));
putText(frame, s, Point(100, 90), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 23, 0), 2, 8);
if (show_ui)
{
imshow(RGB_WINDOW, frame);
waitKey(20);
}
image_vision_pub.publish(cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg());
ros::spinOnce();
loop_rate.sleep();
}
}

@ -1,84 +1,4 @@
/*
Tracker based on Kernelized Correlation Filter (KCF) [1] and Circulant Structure with Kernels (CSK) [2].
CSK is implemented by using raw gray level features, since it is a single-channel filter.
KCF is implemented by using HOG features (the default), since it extends CSK to multiple channels.
[1] J. F. Henriques, R. Caseiro, P. Martins, J. Batista,
"High-Speed Tracking with Kernelized Correlation Filters", TPAMI 2015.
[2] J. F. Henriques, R. Caseiro, P. Martins, J. Batista,
"Exploiting the Circulant Structure of Tracking-by-detection with Kernels", ECCV 2012.
Authors: Joao Faro, Christian Bailer, Joao F. Henriques
Contacts: joaopfaro@gmail.com, Christian.Bailer@dfki.de, henriques@isr.uc.pt
Institute of Systems and Robotics - University of Coimbra / Department Augmented Vision DFKI
Constructor parameters, all boolean:
hog: use HOG features (default), otherwise use raw pixels
fixed_window: fix window size (default), otherwise use ROI size (slower but more accurate)
multiscale: use multi-scale tracking (default; cannot be used with fixed_window = true)
Default values are set for all properties of the tracker depending on the above choices.
Their values can be customized further before calling init():
interp_factor: linear interpolation factor for adaptation
sigma: gaussian kernel bandwidth
lambda: regularization
cell_size: HOG cell size
padding: area surrounding the target, relative to its size
output_sigma_factor: bandwidth of gaussian target
template_size: template size in pixels, 0 to use ROI size
scale_step: scale step for multi-scale estimation, 1 to disable it
scale_weight: to downweight detection scores of other scales for added stability
For speed, the value (template_size/cell_size) should be a power of 2 or a product of small prime numbers.
Inputs to init():
image is the initial frame.
roi is a cv::Rect with the target positions in the initial frame
Inputs to update():
image is the current frame.
Outputs of update():
cv::Rect with target positions for the current frame
By downloading, copying, installing or using the software you agree to this license.
If you do not agree to this license, do not download, install,
copy or use the software.
License Agreement
For Open Source Computer Vision Library
(3-clause BSD License)
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the names of the copyright holders nor the names of the contributors
may be used to endorse or promote products derived from this software
without specific prior written permission.
This software is provided by the copyright holders and contributors "as is" and
any express or implied warranties, including, but not limited to, the implied
warranties of merchantability and fitness for a particular purpose are disclaimed.
In no event shall copyright holders or contributors be liable for any direct,
indirect, incidental, special, exemplary, or consequential damages
(including, but not limited to, procurement of substitute goods or services;
loss of use, data, or profits; or business interruption) however caused
and on any theory of liability, whether in contract, strict liability,
or tort (including negligence or otherwise) arising in any way out of
the use of this software, even if advised of the possibility of such damage.
*/
#include <iostream> #include <iostream>
#ifndef _KCFTRACKER_HEADERS #ifndef _KCFTRACKER_HEADERS
#include "kcftracker.hpp" #include "kcftracker.hpp"
@ -88,75 +8,74 @@ the use of this software, even if advised of the possibility of such damage.
#include "labdata.hpp" #include "labdata.hpp"
#endif #endif
// Constructor // KCFTracker类的构造函数用于初始化KCF跟踪器的参数
KCFTracker::KCFTracker(bool hog, bool fixed_window, bool multiscale, bool lab) KCFTracker::KCFTracker(bool hog, bool fixed_window, bool multiscale, bool lab)
{ {
// 所有情况下参数都相同
lambda = 0.0001; // 权重参数
padding = 2.5; // 边界填充参数
output_sigma_factor = 0.125; // 输出高斯函数的标准差缩放因子
// Parameters equal in all cases // 根据是否使用HOG特征来设置参数
lambda = 0.0001; if (hog) { // 如果使用HOG特征
padding = 2.5;
//output_sigma_factor = 0.1;
output_sigma_factor = 0.125;
if (hog) { // HOG
// VOT // VOT
interp_factor = 0.012; interp_factor = 0.012; // 插值因子
sigma = 0.6; sigma = 0.6; // 高斯核的标准差
// TPAMI
//interp_factor = 0.02;
//sigma = 0.5;
cell_size = 4;
_hogfeatures = true;
cell_size = 4; // 大小
_hogfeatures = true; // 设置HOG特征标志为真
// 如果使用LAB特征
if (lab) { if (lab) {
interp_factor = 0.005; interp_factor = 0.005; // 插值因子
sigma = 0.4; sigma = 0.4; // 高斯核的标准差
//output_sigma_factor = 0.025; //output_sigma_factor = 0.025;
output_sigma_factor = 0.1; output_sigma_factor = 0.1; // 输出高斯函数的标准差缩放因子
_labfeatures = true; _labfeatures = true; // 设置LAB特征标志为真
_labCentroids = cv::Mat(nClusters, 3, CV_32FC1, &data); _labCentroids = cv::Mat(nClusters, 3, CV_32FC1, &data); // 初始化LAB特征的聚类中心
cell_sizeQ = cell_size*cell_size; cell_sizeQ = cell_size * cell_size; // 计算细胞大小的平方
} }
else{ else {
_labfeatures = false; _labfeatures = false; // 设置LAB特征标志为假
} }
} }
else { // RAW else { // 如果使用原始特征
interp_factor = 0.075; interp_factor = 0.075; // 插值因子
sigma = 0.2; sigma = 0.2; // 高斯核的标准差
cell_size = 1; cell_size = 1; // 细胞大小
_hogfeatures = false; _hogfeatures = false; // 设置HOG特征标志为假
// 如果尝试使用LAB特征则打印错误信息
if (lab) { if (lab) {
printf("Lab features are only used with HOG features.\n"); printf("Lab features are only used with HOG features.\n");
_labfeatures = false; _labfeatures = false; // 设置LAB特征标志为假
} }
} }
// 根据是否使用多尺度来设置参数
if (multiscale) { // multiscale if (multiscale) { // 如果使用多尺度
template_size = 96; template_size = 96; // 模板大小
//template_size = 100; //template_size = 100;
scale_step = 1.05; scale_step = 1.05; // 缩放步长
scale_weight = 0.95; scale_weight = 0.95; // 缩放权重
if (!fixed_window) { if (!fixed_window) { // 如果不是固定窗口
//printf("Multiscale does not support non-fixed window.\n"); //printf("Multiscale does not support non-fixed window.\n");
fixed_window = true; fixed_window = true; // 设置固定窗口标志为真
} }
} }
else if (fixed_window) { // fit correction without multiscale else if (fixed_window) { // 如果使用固定窗口
template_size = 96; template_size = 96; // 模板大小
//template_size = 100; //template_size = 100;
scale_step = 1; scale_step = 1; // 缩放步长
} }
else { else { // 如果都不使用
template_size = 1; template_size = 1; // 模板大小
scale_step = 1; scale_step = 1; // 缩放步长
} }
} }
// Initialize tracker // Initialize tracker
void KCFTracker::init(const cv::Rect &roi, cv::Mat image) void KCFTracker::init(const cv::Rect &roi, cv::Mat image)
{ {
@ -172,51 +91,51 @@ void KCFTracker::init(const cv::Rect &roi, cv::Mat image)
// Update position based on the new frame // Update position based on the new frame
cv::Rect KCFTracker::update(cv::Mat image) cv::Rect KCFTracker::update(cv::Mat image)
{ {
if (_roi.x + _roi.width <= 0) _roi.x = -_roi.width + 1; if (_roi.x + _roi.width <= 0) _roi.x = -_roi.width + 1; // 如果ROI的x坐标加上宽度小于0则调整x坐标
if (_roi.y + _roi.height <= 0) _roi.y = -_roi.height + 1; if (_roi.y + _roi.height <= 0) _roi.y = -_roi.height + 1; // 如果ROI的y坐标加上高度小于0则调整y坐标
if (_roi.x >= image.cols - 1) _roi.x = image.cols - 2; if (_roi.x >= image.cols - 1) _roi.x = image.cols - 2; // 如果ROI的x坐标加上宽度大于图像宽度则调整x坐标
if (_roi.y >= image.rows - 1) _roi.y = image.rows - 2; if (_roi.y >= image.rows - 1) _roi.y = image.rows - 2; // 如果ROI的y坐标加上高度大于图像高度则调整y坐标
float cx = _roi.x + _roi.width / 2.0f;
float cy = _roi.y + _roi.height / 2.0f;
float cx = _roi.x + _roi.width / 2.0f; // 计算ROI的中心x坐标
float cy = _roi.y + _roi.height / 2.0f; // 计算ROI的中心y坐标
float peak_value; float peak_value; // 峰值值
cv::Point2f res = detect(_tmpl, getFeatures(image, 0, 1.0f), peak_value); cv::Point2f res = detect(_tmpl, getFeatures(image, 0, 1.0f), peak_value); // 使用检测函数和模板进行检测
if (scale_step != 1) { if (scale_step != 1) { // 如果缩放步长不是1则尝试在更小的缩放级别上进行检测
// Test at a smaller _scale // 在更小的缩放级别上进行测试
float new_peak_value; float new_peak_value;
cv::Point2f new_res = detect(_tmpl, getFeatures(image, 0, 1.0f / scale_step), new_peak_value); cv::Point2f new_res = detect(_tmpl, getFeatures(image, 0, 1.0f / scale_step), new_peak_value);
if (scale_weight * new_peak_value > peak_value) { if (scale_weight * new_peak_value > peak_value) { // 如果新峰值值更大,则更新结果
res = new_res; res = new_res;
peak_value = new_peak_value; peak_value = new_peak_value;
_scale /= scale_step; _scale /= scale_step; // 更新缩放级别
_roi.width /= scale_step; _roi.width /= scale_step; // 更新ROI的宽度
_roi.height /= scale_step; _roi.height /= scale_step; // 更新ROI的高度
} }
// Test at a bigger _scale // 在更大的缩放级别上进行测试
new_res = detect(_tmpl, getFeatures(image, 0, scale_step), new_peak_value); new_res = detect(_tmpl, getFeatures(image, 0, scale_step), new_peak_value);
if (scale_weight * new_peak_value > peak_value) { if (scale_weight * new_peak_value > peak_value) { // 如果新峰值值更大,则更新结果
res = new_res; res = new_res;
peak_value = new_peak_value; peak_value = new_peak_value;
_scale *= scale_step; _scale *= scale_step; // 更新缩放级别
_roi.width *= scale_step; _roi.width *= scale_step; // 更新ROI的宽度
_roi.height *= scale_step; _roi.height *= scale_step; // 更新ROI的高度
} }
} }
// Adjust by cell size and _scale // 根据细胞大小和缩放级别调整ROI
_roi.x = cx - _roi.width / 2.0f + ((float) res.x * cell_size * _scale); _roi.x = cx - _roi.width / 2.0f + ((float) res.x * cell_size * _scale); // 调整ROI的x坐标
_roi.y = cy - _roi.height / 2.0f + ((float) res.y * cell_size * _scale); _roi.y = cy - _roi.height / 2.0f + ((float) res.y * cell_size * _scale); // 调整ROI的y坐标
if (_roi.x >= image.cols - 1) _roi.x = image.cols - 1; // 确保ROI的x坐标不超过图像宽度
if (_roi.y >= image.rows - 1) _roi.y = image.rows - 1; // 确保ROI的y坐标不超过图像高度
if (_roi.x + _roi.width <= 0) _roi.x = -_roi.width + 2; // 确保ROI的x坐标加上宽度大于0
if (_roi.y + _roi.height <= 0) _roi.y = -_roi.height + 2; // 确保ROI的y坐标加上高度大于0
if (_roi.x >= image.cols - 1) _roi.x = image.cols - 1;
if (_roi.y >= image.rows - 1) _roi.y = image.rows - 1;
if (_roi.x + _roi.width <= 0) _roi.x = -_roi.width + 2;
if (_roi.y + _roi.height <= 0) _roi.y = -_roi.height + 2;
assert(_roi.width >= 0 && _roi.height >= 0); assert(_roi.width >= 0 && _roi.height >= 0);
cv::Mat x = getFeatures(image, 0); cv::Mat x = getFeatures(image, 0);
@ -261,30 +180,20 @@ cv::Point2f KCFTracker::detect(cv::Mat z, cv::Mat x, float &peak_value)
return p; return p;
} }
// train tracker with a single image // 使用单个图像训练跟踪器
void KCFTracker::train(cv::Mat x, float train_interp_factor) void KCFTracker::train(cv::Mat x, float train_interp_factor)
{ {
using namespace FFTTools; using namespace FFTTools; // 引入FFTTools命名空间可能包含相关函数和变量
cv::Mat k = gaussianCorrelation(x, x);
cv::Mat alphaf = complexDivision(_prob, (fftd(k) + lambda));
_tmpl = (1 - train_interp_factor) * _tmpl + (train_interp_factor) * x;
_alphaf = (1 - train_interp_factor) * _alphaf + (train_interp_factor) * alphaf;
/*cv::Mat kf = fftd(gaussianCorrelation(x, x)); cv::Mat k = gaussianCorrelation(x, x); // 计算x和x之间的高斯相关性
cv::Mat num = complexMultiplication(kf, _prob); cv::Mat alphaf = complexDivision(_prob, (fftd(k) + lambda)); // 计算高斯核的傅里叶变换,并除以(核变换 + lambda)
cv::Mat den = complexMultiplication(kf, kf + lambda);
_tmpl = (1 - train_interp_factor) * _tmpl + (train_interp_factor) * x;
_num = (1 - train_interp_factor) * _num + (train_interp_factor) * num;
_den = (1 - train_interp_factor) * _den + (train_interp_factor) * den;
_alphaf = complexDivision(_num, _den);*/ _tmpl = (1 - train_interp_factor) * _tmpl + (train_interp_factor) * x; // 更新模板
_alphaf = (1 - train_interp_factor) * _alphaf + (train_interp_factor) * alphaf; // 更新高斯核的傅里叶变换
} }
// Evaluates a Gaussian kernel with bandwidth SIGMA for all relative shifts between input images X and Y, which must both be MxN. They must also be periodic (ie., pre-processed with a cosine window). // Evaluates a Gaussian kernel with bandwidth SIGMA for all relative shifts between input images X and Y, which must both be MxN. They must also be periodic (ie., pre-processed with a cosine window).
cv::Mat KCFTracker::gaussianCorrelation(cv::Mat x1, cv::Mat x2) cv::Mat KCFTracker::gaussianCorrelation(cv::Mat x1, cv::Mat x2)
{ {
@ -321,27 +230,28 @@ cv::Mat KCFTracker::gaussianCorrelation(cv::Mat x1, cv::Mat x2)
return k; return k;
} }
// Create Gaussian Peak. Function called only in the first frame. // 在第一帧中创建高斯峰值
cv::Mat KCFTracker::createGaussianPeak(int sizey, int sizex) cv::Mat KCFTracker::createGaussianPeak(int sizey, int sizex)
{ {
cv::Mat_<float> res(sizey, sizex); cv::Mat_<float> res(sizey, sizex); // 创建一个浮点数矩阵大小为sizey x sizex
int syh = (sizey) / 2; int syh = (sizey) / 2; // 计算y方向的高斯峰值中心
int sxh = (sizex) / 2; int sxh = (sizex) / 2; // 计算x方向的高斯峰值中心
float output_sigma = std::sqrt((float) sizex * sizey) / padding * output_sigma_factor; float output_sigma = std::sqrt((float) sizex * sizey) / padding * output_sigma_factor; // 计算输出高斯核的标准差
float mult = -0.5 / (output_sigma * output_sigma); float mult = -0.5 / (output_sigma * output_sigma); // 计算高斯核的缩放因子
for (int i = 0; i < sizey; i++) for (int i = 0; i < sizey; i++) // 遍历矩阵的每一行
for (int j = 0; j < sizex; j++) for (int j = 0; j < sizex; j++) // 遍历矩阵的每一列
{ {
int ih = i - syh; int ih = i - syh; // 计算当前点与y方向高斯峰值中心的距离
int jh = j - sxh; int jh = j - sxh; // 计算当前点与x方向高斯峰值中心的距离
res(i, j) = std::exp(mult * (float) (ih * ih + jh * jh)); res(i, j) = std::exp(mult * (float) (ih * ih + jh * jh)); // 计算高斯函数的值
} }
return FFTTools::fftd(res); return FFTTools::fftd(res); // 返回高斯峰值的傅里叶变换
} }
// Obtain sub-window from image, with replication-padding and extract features // Obtain sub-window from image, with replication-padding and extract features
cv::Mat KCFTracker::getFeatures(const cv::Mat & image, bool inithann, float scale_adjust) cv::Mat KCFTracker::getFeatures(const cv::Mat & image, bool inithann, float scale_adjust)
{ {
@ -486,22 +396,25 @@ cv::Mat KCFTracker::getFeatures(const cv::Mat & image, bool inithann, float scal
return FeaturesMap; return FeaturesMap;
} }
// Initialize Hanning window. Function called only in the first frame. // 初始化汉明窗口。该函数仅在第一帧调用。
void KCFTracker::createHanningMats() void KCFTracker::createHanningMats()
{ {
cv::Mat hann1t = cv::Mat(cv::Size(size_patch[1],1), CV_32F, cv::Scalar(0)); cv::Mat hann1t = cv::Mat(cv::Size(size_patch[1],1), CV_32F, cv::Scalar(0));
cv::Mat hann2t = cv::Mat(cv::Size(1,size_patch[0]), CV_32F, cv::Scalar(0)); cv::Mat hann2t = cv::Mat(cv::Size(1,size_patch[0]), CV_32F, cv::Scalar(0));
// 创建汉明窗口的一维版本
for (int i = 0; i < hann1t.cols; i++) for (int i = 0; i < hann1t.cols; i++)
hann1t.at<float > (0, i) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann1t.cols - 1))); hann1t.at<float>(0, i) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann1t.cols - 1)));
for (int i = 0; i < hann2t.rows; i++) for (int i = 0; i < hann2t.rows; i++)
hann2t.at<float > (i, 0) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann2t.rows - 1))); hann2t.at<float>(i, 0) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann2t.rows - 1)));
// 将一维汉明窗口组合成二维汉明窗口
cv::Mat hann2d = hann2t * hann1t; cv::Mat hann2d = hann2t * hann1t;
// HOG features
// HOG特征
if (_hogfeatures) { if (_hogfeatures) {
cv::Mat hann1d = hann2d.reshape(1,1); // Procedure do deal with cv::Mat multichannel bug cv::Mat hann1d = hann2d.reshape(1,1); // 解决cv::Mat多通道bug的步骤
hann = cv::Mat(cv::Size(size_patch[0]*size_patch[1], size_patch[2]), CV_32F, cv::Scalar(0)); hann = cv::Mat(cv::Size(size_patch[0]*size_patch[1], size_patch[2]), CV_32F, cv::Scalar(0));
for (int i = 0; i < size_patch[2]; i++) { for (int i = 0; i < size_patch[2]; i++) {
for (int j = 0; j<size_patch[0]*size_patch[1]; j++) { for (int j = 0; j<size_patch[0]*size_patch[1]; j++) {
@ -509,15 +422,15 @@ void KCFTracker::createHanningMats()
} }
} }
} }
// Gray features // 灰度特征
else { else {
hann = hann2d; hann = hann2d;
} }
} }
// Calculate sub-pixel peak for one dimension // 为单维度计算亚像素峰值
float KCFTracker::subPixelPeak(float left, float center, float right) float KCFTracker::subPixelPeak(float left, float center, float right)
{ {
float divisor = 2 * center - right - left; float divisor = 2 * center - right - left;
if (divisor == 0) if (divisor == 0)

Loading…
Cancel
Save