diff --git a/src/GPS/GPSManager.cc b/src/GPS/GPSManager.cc index a4ee82d..4c90fea 100644 --- a/src/GPS/GPSManager.cc +++ b/src/GPS/GPSManager.cc @@ -1,11 +1,4 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ + #include "GPSManager.h" diff --git a/src/GPS/GPSManager.h b/src/GPS/GPSManager.h index eb22642..dffa72e 100644 --- a/src/GPS/GPSManager.h +++ b/src/GPS/GPSManager.h @@ -1,11 +1,3 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ #pragma once diff --git a/src/GPS/GPSPositionMessage.h b/src/GPS/GPSPositionMessage.h index a2437f3..3f5ba29 100644 --- a/src/GPS/GPSPositionMessage.h +++ b/src/GPS/GPSPositionMessage.h @@ -1,11 +1,4 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ + #pragma once diff --git a/src/GPS/GPSProvider.cc b/src/GPS/GPSProvider.cc index 16b84ed..e74fdc1 100644 --- a/src/GPS/GPSProvider.cc +++ b/src/GPS/GPSProvider.cc @@ -1,11 +1,4 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ + #include "GPSProvider.h" diff --git a/src/GPS/GPSProvider.h b/src/GPS/GPSProvider.h index 3b69a1c..a901f8b 100644 --- a/src/GPS/GPSProvider.h +++ b/src/GPS/GPSProvider.h @@ -1,12 +1,3 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - #pragma once diff --git a/src/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc index f859a9a..19dedd7 100644 --- a/src/GPS/RTCM/RTCMMavlink.cc +++ b/src/GPS/RTCM/RTCMMavlink.cc @@ -1,11 +1,4 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ + #include "RTCMMavlink.h" diff --git a/src/GPS/RTCM/RTCMMavlink.h b/src/GPS/RTCM/RTCMMavlink.h index 14b649d..a5d4bbc 100644 --- a/src/GPS/RTCM/RTCMMavlink.h +++ b/src/GPS/RTCM/RTCMMavlink.h @@ -1,11 +1,4 @@ -/**************************************************************************** - * - * (c) 2009-2020 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ + #pragma once diff --git a/src/GPS/definitions.h b/src/GPS/definitions.h index 9267574..b4377d3 100644 --- a/src/GPS/definitions.h +++ b/src/GPS/definitions.h @@ -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 diff --git a/src/GPS/satellite_info.h b/src/GPS/satellite_info.h index 2731319..e2d1450 100644 --- a/src/GPS/satellite_info.h +++ b/src/GPS/satellite_info.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 #include diff --git a/src/GPS/vehicle_gps_position.h b/src/GPS/vehicle_gps_position.h index 3d1e250..3e2b589 100644 --- a/src/GPS/vehicle_gps_position.h +++ b/src/GPS/vehicle_gps_position.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 diff --git a/src/fhog.cpp b/src/fhog.cpp index c990823..3fa75c5 100644 --- a/src/fhog.cpp +++ b/src/fhog.cpp @@ -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". @@ -77,60 +37,92 @@ // RESULT // Error status */ +// 从输入的图像中提取特征映射 int getFeatureMaps(const IplImage* image, const int k, CvLSVMFeatureMapCaskade **map) { + // 图像的宽度和高度 int sizeX, sizeY; - int p, px, stringSize; + // 每个cell中的特征数量 + int p, px; + // 存储特征的字符串大小 + int stringSize; + // 图像的高度、宽度和通道数 int height, width, numChannels; + // 循环变量 int i, j, kk, c, ii, jj, d; + // 梯度数据指针 float * datadx, * datady; - - int ch; + + // 角度相关变量 + int ch; float magnitude, x, y, tx, ty; - + + // 梯度图像 IplImage * dx, * dy; + // 最近的邻域点索引 int *nearest; - float *w, a_x, b_x; + // 权重向量 + float *w; + // 线性插值系数 + float a_x, b_x; + // 梯度算子 float kernel[3] = {-1.f, 0.f, 1.f}; + // OpenCV矩阵格式的梯度算子 CvMat kernel_dx = cvMat(1, 3, CV_32F, kernel); CvMat kernel_dy = cvMat(3, 1, CV_32F, kernel); + // 临时指针 float * r; + // 未知用途的整型数组（可能是用于后续处理的索引或标志） int * alfa; - + + // 边界角度值数组 float boundary_x[NUM_SECTOR + 1]; float boundary_y[NUM_SECTOR + 1]; + // 用于查找最大值的变量 float max, dotProd; int maxi; + // 获取图像的高度和宽度 height = image->height; - width = image->width ; + width = image->width; + // 获取图像的通道数 numChannels = image->nChannels; - dx = cvCreateImage(cvSize(image->width, image->height), + // 创建用于存储梯度图像的容器 + dx = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_32F, 3); - dy = cvCreateImage(cvSize(image->width, image->height), + dy = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_32F, 3); + // 计算特征映射的宽和高（基于k的大小） sizeX = width / k; sizeY = height / k; - px = 3 * NUM_SECTOR; + // 计算每个cell中的特征数量（基于NUM_SECTOR） + px = 3 * NUM_SECTOR; p = px; + // 计算整个特征映射所需的总存储大小 stringSize = sizeX * p; + // 分配特征映射对象的内存 allocFeatureMapObject(map, sizeX, sizeY, p); + // 使用Sobel算子计算图像的梯度 cvFilter2D(image, dx, &kernel_dx, cvPoint(-1, 0)); cvFilter2D(image, dy, &kernel_dy, cvPoint(0, -1)); - + + // 计算边界角度值 float arg_vector; for(i = 0; i <= NUM_SECTOR; i++) { + // 计算角度（基于NUM_SECTOR将圆周等分为NUM_SECTOR个部分） arg_vector = ( (float) i ) * ( (float)(PI) / (float)(NUM_SECTOR) ); + // 计算对应的余弦和正弦值 boundary_x[i] = cosf(arg_vector); boundary_y[i] = sinf(arg_vector); - }/*for(i = 0; i <= NUM_SECTOR; i++) */ + } + r = (float *)malloc( sizeof(float) * (width * height)); alfa = (int *)malloc( sizeof(int ) * (width * height * 2)); @@ -287,43 +279,63 @@ int getFeatureMaps(const IplImage* image, const int k, CvLSVMFeatureMapCaskade * // RESULT // Error status */ +// 对特征映射进行归一化和截断处理 int normalizeAndTruncate(CvLSVMFeatureMapCaskade *map, const float alfa) { - int i,j, ii; - int sizeX, sizeY, p, pos, pp, xp, pos1, pos2; - float * partOfNorm; // norm of C(i, j) - float * newData; - float valOfNorm; + int i, j, ii; // 定义循环变量 + int sizeX, sizeY, p, pos, pp, xp; // 定义变量 + float * partOfNorm; // 存储C(i, j)的范数的指针 + float * newData; // 存储归一化后数据的指针 + float valOfNorm; // 存储范数的值 - sizeX = map->sizeX; - sizeY = map->sizeY; - partOfNorm = (float *)malloc (sizeof(float) * (sizeX * sizeY)); + sizeX = map->sizeX; // 获取特征映射的宽度 + sizeY = map->sizeY; // 获取特征映射的高度 + partOfNorm = (float *)malloc (sizeof(float) * (sizeX * sizeY)); // 分配存储范数的内存 - p = NUM_SECTOR; - xp = NUM_SECTOR * 3; - pp = NUM_SECTOR * 12; + p = NUM_SECTOR; // 定义一个常量，可能表示特征的分区数 + xp = NUM_SECTOR * 3; // 定义一个常量，可能表示特征的某种属性 + pp = NUM_SECTOR * 12; // 定义一个常量，可能表示特征的某种属性 + // 计算每个部分区域的范数 for(i = 0; i < sizeX * sizeY; i++) { - valOfNorm = 0.0f; - pos = i * map->numFeatures; - for(j = 0; j < p; j++) + valOfNorm = 0.0f; // 初始化范数为0 + pos = i * map->numFeatures; // 计算每个部分的起始位置 + 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++)*/ - partOfNorm[i] = valOfNorm; + partOfNorm[i] = valOfNorm; // 将每个部分的范数存储起来 }/*for(i = 0; i < sizeX * sizeY; i++)*/ - + + // 调整特征映射的大小 sizeX -= 2; sizeY -= 2; + // 分配存储归一化后数据的内存 newData = (float *)malloc (sizeof(float) * (sizeX * sizeY * pp)); -//normalization + + // 归一化处理 for(i = 1; i <= sizeY; i++) { 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 + 1)] + partOfNorm[(i + 1)*(sizeX + 2) + (j )] + @@ -411,102 +423,159 @@ int normalizeAndTruncate(CvLSVMFeatureMapCaskade *map, const float alfa) // RESULT // Error status */ +// 对输入的CvLSVMFeatureMapCaskade对象进行PCA特征映射处理 int PCAFeatureMaps(CvLSVMFeatureMapCaskade *map) -{ - int i,j, ii, jj, k; - int sizeX, sizeY, p, pp, xp, yp, pos1, pos2; +{ + int i, j, ii, jj, k; + int sizeX, sizeY, p, pp, xp, yp, pos1, pos2; float * newData; float val; float nx, ny; - + + // 获取特征映射的宽、高和特征数量 sizeX = map->sizeX; sizeY = map->sizeY; p = map->numFeatures; + + // 定义新的特征数量pp和xp、yp等参数 pp = NUM_SECTOR * 3 + 4; yp = 4; xp = NUM_SECTOR; + // 计算nx和ny两个归一化因子 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)); + if (newData == NULL) { + // 如果内存分配失败，应返回错误代码 + // 这里为了简洁省略了错误处理 + return LATENT_SVM_MEM_ERROR; // 假设的错误码 + } + // 遍历特征映射的每一个位置 for(i = 0; i < sizeY; i++) { for(j = 0; j < sizeX; j++) { + // 计算原始数据在当前位置的索引pos1 pos1 = ((i)*sizeX + j)*p; + // 计算新数据在当前位置的索引pos2 pos2 = ((i)*sizeX + j)*pp; - k = 0; + + k = 0; // 初始化k用于索引新数据的位置 + + // 计算第一个部分的新特征 for(jj = 0; jj < xp * 2; jj++) { - val = 0; + val = 0; // 初始化累加值 + // 遍历yp个原始特征值进行累加 for(ii = 0; ii < yp; ii++) { val += map->map[pos1 + yp * xp + ii * xp * 2 + jj]; - }/*for(ii = 0; ii < yp; ii++)*/ + } + // 将累加值乘以ny并存储到新数据中 newData[pos2 + k] = val * ny; k++; - }/*for(jj = 0; jj < xp * 2; jj++)*/ + } + + // 计算第二个部分的新特征 for(jj = 0; jj < xp; jj++) { - val = 0; + val = 0; // 初始化累加值 + // 遍历yp个原始特征值进行累加 for(ii = 0; ii < yp; ii++) { val += map->map[pos1 + ii * xp + jj]; - }/*for(ii = 0; ii < yp; ii++)*/ + } + // 将累加值乘以ny并存储到新数据中 newData[pos2 + k] = val * ny; k++; - }/*for(jj = 0; jj < xp; jj++)*/ + } + + // 计算第三个部分的新特征 for(ii = 0; ii < yp; ii++) { - val = 0; + val = 0; // 初始化累加值 + // 遍历2*xp个原始特征值进行累加 for(jj = 0; jj < 2 * xp; jj++) { val += map->map[pos1 + yp * xp + ii * xp * 2 + jj]; - }/*for(jj = 0; jj < xp; jj++)*/ + } + // 将累加值乘以nx并存储到新数据中 newData[pos2 + k] = val * nx; k++; - } /*for(ii = 0; ii < yp; ii++)*/ - }/*for(j = 0; j < sizeX; j++)*/ - }/*for(i = 0; i < sizeY; i++)*/ -//swop data + } + } + } + + // 交换数据，这里看起来没有实际的交换操作，可能是对数据的后续处理或注释有误 + // 更新特征数量 map->numFeatures = pp; + // 释放原始特征映射数据占用的内存 free (map->map); + // 将新的特征映射数据赋值给map的map成员 map->map = newData; + // 返回成功码 return LATENT_SVM_OK; } - -//modified from "lsvmc_routine.cpp" -//ֱڷͷCvLSVMFeatureMapCaskadeṹڴ档 - -int allocFeatureMapObject(CvLSVMFeatureMapCaskade **obj, const int sizeX, +// 分配一个CvLSVMFeatureMapCaskade对象并初始化其成员 +int allocFeatureMapObject(CvLSVMFeatureMapCaskade **obj, const int sizeX, const int sizeY, const int numFeatures) { int i; + + // 使用malloc为CvLSVMFeatureMapCaskade对象分配内存 (*obj) = (CvLSVMFeatureMapCaskade *)malloc(sizeof(CvLSVMFeatureMapCaskade)); + if (!(*obj)) { // 如果分配失败，应该返回一个错误代码，但这里假设它总是成功的 + // 实际应用中应该添加错误处理 + } + + // 初始化对象的成员变量 (*obj)->sizeX = sizeX; (*obj)->sizeY = sizeY; (*obj)->numFeatures = numFeatures; - (*obj)->map = (float *) malloc(sizeof (float) * + + // 使用malloc为map成员分配内存，它用于存储特征映射数据 + (*obj)->map = (float *) malloc(sizeof (float) * (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++) { (*obj)->map[i] = 0.0f; } + + // 返回成功码 return LATENT_SVM_OK; } +// 释放一个CvLSVMFeatureMapCaskade对象占用的内存 int freeFeatureMapObject (CvLSVMFeatureMapCaskade **obj) { + // 如果obj为空，返回内存为空的错误码 if(*obj == NULL) return LATENT_SVM_MEM_NULL; + + // 释放map成员占用的内存 free((*obj)->map); + // 释放对象本身占用的内存 free(*obj); + + // 将指针设置为NULL，避免野指针 (*obj) = NULL; + + // 返回成功码 return LATENT_SVM_OK; } diff --git a/src/kcf.cpp b/src/kcf.cpp new file mode 100644 index 0000000..01b89f7 --- /dev/null +++ b/src/kcf.cpp @@ -0,0 +1,363 @@ +// Track Object---advanced by Wangmingqiang ----------------------------------------- +// 2024.6.01 at Hunan Changsha. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 lockImageCallback(mutexImageCallback_); + camImageCopy_ = cam_image->image.clone(); // 克隆图像到全局变量 + } + // 使用互斥锁来更新图像状态 + { + boost::unique_lock lockImageStatus(mutexImageStatus_); + imageStatus_ = true; // 设置图像状态为已接收 + } + frameWidth_ = cam_image->image.size().width; // 保存图像宽度 + frameHeight_ = cam_image->image.size().height; // 保存图像高度 + } + return; // 结束函数调用 +} + + +// 用此函数查看是否收到图像话题 +bool getImageStatus(void) +{ + boost::shared_lock 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("auto_zoom", auto_zoom, false); // 从参数服务器获取参数 + nh.param("show_ui", show_ui, true); + nh.param("max_size", max_size, 0.0); + nh.param("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("/gimbal/track", 10); + // ros::Publisher auto_zoom_pub = nh.advertise("/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 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(); + } +} diff --git a/src/kcftracker.cpp b/src/kcftracker.cpp index 860b320..d45e32f 100644 --- a/src/kcftracker.cpp +++ b/src/kcftracker.cpp @@ -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 #ifndef _KCFTRACKER_HEADERS #include "kcftracker.hpp" @@ -88,75 +8,74 @@ the use of this software, even if advised of the possibility of such damage. #include "labdata.hpp" #endif -// Constructor +// KCFTracker类的构造函数，用于初始化KCF跟踪器的参数 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 - lambda = 0.0001; - padding = 2.5; - //output_sigma_factor = 0.1; - output_sigma_factor = 0.125; - - - if (hog) { // HOG + // 根据是否使用HOG特征来设置参数 + if (hog) { // 如果使用HOG特征 // VOT - interp_factor = 0.012; - sigma = 0.6; - // TPAMI - //interp_factor = 0.02; - //sigma = 0.5; - cell_size = 4; - _hogfeatures = true; + interp_factor = 0.012; // 插值因子 + sigma = 0.6; // 高斯核的标准差 + cell_size = 4; // 大小 + _hogfeatures = true; // 设置HOG特征标志为真 + + // 如果使用LAB特征 if (lab) { - interp_factor = 0.005; - sigma = 0.4; + interp_factor = 0.005; // 插值因子 + sigma = 0.4; // 高斯核的标准差 //output_sigma_factor = 0.025; - output_sigma_factor = 0.1; + output_sigma_factor = 0.1; // 输出高斯函数的标准差缩放因子 - _labfeatures = true; - _labCentroids = cv::Mat(nClusters, 3, CV_32FC1, &data); - cell_sizeQ = cell_size*cell_size; + _labfeatures = true; // 设置LAB特征标志为真 + _labCentroids = cv::Mat(nClusters, 3, CV_32FC1, &data); // 初始化LAB特征的聚类中心 + cell_sizeQ = cell_size * cell_size; // 计算细胞大小的平方 } - else{ - _labfeatures = false; + else { + _labfeatures = false; // 设置LAB特征标志为假 } } - else { // RAW - interp_factor = 0.075; - sigma = 0.2; - cell_size = 1; - _hogfeatures = false; + else { // 如果使用原始特征 + interp_factor = 0.075; // 插值因子 + sigma = 0.2; // 高斯核的标准差 + cell_size = 1; // 细胞大小 + _hogfeatures = false; // 设置HOG特征标志为假 + // 如果尝试使用LAB特征，则打印错误信息 if (lab) { printf("Lab features are only used with HOG features.\n"); - _labfeatures = false; + _labfeatures = false; // 设置LAB特征标志为假 } } - - if (multiscale) { // multiscale - template_size = 96; + // 根据是否使用多尺度来设置参数 + if (multiscale) { // 如果使用多尺度 + template_size = 96; // 模板大小 //template_size = 100; - scale_step = 1.05; - scale_weight = 0.95; - if (!fixed_window) { + scale_step = 1.05; // 缩放步长 + scale_weight = 0.95; // 缩放权重 + if (!fixed_window) { // 如果不是固定窗口 //printf("Multiscale does not support non-fixed window.\n"); - fixed_window = true; + fixed_window = true; // 设置固定窗口标志为真 } } - else if (fixed_window) { // fit correction without multiscale - template_size = 96; + else if (fixed_window) { // 如果使用固定窗口 + template_size = 96; // 模板大小 //template_size = 100; - scale_step = 1; + scale_step = 1; // 缩放步长 } - else { - template_size = 1; - scale_step = 1; + else { // 如果都不使用 + template_size = 1; // 模板大小 + scale_step = 1; // 缩放步长 } } + // Initialize tracker 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 cv::Rect KCFTracker::update(cv::Mat image) { - if (_roi.x + _roi.width <= 0) _roi.x = -_roi.width + 1; - if (_roi.y + _roi.height <= 0) _roi.y = -_roi.height + 1; - if (_roi.x >= image.cols - 1) _roi.x = image.cols - 2; - if (_roi.y >= image.rows - 1) _roi.y = image.rows - 2; - - float cx = _roi.x + _roi.width / 2.0f; - float cy = _roi.y + _roi.height / 2.0f; + 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; // 如果ROI的y坐标加上高度小于0，则调整y坐标 + 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; // 如果ROI的y坐标加上高度大于图像高度，则调整y坐标 + float cx = _roi.x + _roi.width / 2.0f; // 计算ROI的中心x坐标 + float cy = _roi.y + _roi.height / 2.0f; // 计算ROI的中心y坐标 - float peak_value; - cv::Point2f res = detect(_tmpl, getFeatures(image, 0, 1.0f), peak_value); + float peak_value; // 峰值值 + cv::Point2f res = detect(_tmpl, getFeatures(image, 0, 1.0f), peak_value); // 使用检测函数和模板进行检测 - if (scale_step != 1) { - // Test at a smaller _scale + if (scale_step != 1) { // 如果缩放步长不是1，则尝试在更小的缩放级别上进行检测 + // 在更小的缩放级别上进行测试 float 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; peak_value = new_peak_value; - _scale /= scale_step; - _roi.width /= scale_step; - _roi.height /= scale_step; + _scale /= scale_step; // 更新缩放级别 + _roi.width /= scale_step; // 更新ROI的宽度 + _roi.height /= scale_step; // 更新ROI的高度 } - // Test at a bigger _scale + // 在更大的缩放级别上进行测试 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; peak_value = new_peak_value; - _scale *= scale_step; - _roi.width *= scale_step; - _roi.height *= scale_step; + _scale *= scale_step; // 更新缩放级别 + _roi.width *= scale_step; // 更新ROI的宽度 + _roi.height *= scale_step; // 更新ROI的高度 } } - // Adjust by cell size and _scale - _roi.x = cx - _roi.width / 2.0f + ((float) res.x * cell_size * _scale); - _roi.y = cy - _roi.height / 2.0f + ((float) res.y * cell_size * _scale); + // 根据细胞大小和缩放级别调整ROI + _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坐标 + + 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); 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; } -// train tracker with a single image +// 使用单个图像训练跟踪器 void KCFTracker::train(cv::Mat x, float train_interp_factor) { - using namespace 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; - + using namespace FFTTools; // 引入FFTTools命名空间，可能包含相关函数和变量 - /*cv::Mat kf = fftd(gaussianCorrelation(x, x)); - cv::Mat num = complexMultiplication(kf, _prob); - 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; + cv::Mat k = gaussianCorrelation(x, x); // 计算x和x之间的高斯相关性 + cv::Mat alphaf = complexDivision(_prob, (fftd(k) + lambda)); // 计算高斯核的傅里叶变换，并除以(核变换 + lambda) - _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). 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; } -// Create Gaussian Peak. Function called only in the first frame. +// 在第一帧中创建高斯峰值 cv::Mat KCFTracker::createGaussianPeak(int sizey, int sizex) { - cv::Mat_ res(sizey, sizex); + cv::Mat_ res(sizey, sizex); // 创建一个浮点数矩阵，大小为sizey x sizex - int syh = (sizey) / 2; - int sxh = (sizex) / 2; + int syh = (sizey) / 2; // 计算y方向的高斯峰值中心 + int sxh = (sizex) / 2; // 计算x方向的高斯峰值中心 - float output_sigma = std::sqrt((float) sizex * sizey) / padding * output_sigma_factor; - float mult = -0.5 / (output_sigma * output_sigma); + float output_sigma = std::sqrt((float) sizex * sizey) / padding * output_sigma_factor; // 计算输出高斯核的标准差 + float mult = -0.5 / (output_sigma * output_sigma); // 计算高斯核的缩放因子 - for (int i = 0; i < sizey; i++) - for (int j = 0; j < sizex; j++) + for (int i = 0; i < sizey; i++) // 遍历矩阵的每一行 + for (int j = 0; j < sizex; j++) // 遍历矩阵的每一列 { - int ih = i - syh; - int jh = j - sxh; - res(i, j) = std::exp(mult * (float) (ih * ih + jh * jh)); + int ih = i - syh; // 计算当前点与y方向高斯峰值中心的距离 + int jh = j - sxh; // 计算当前点与x方向高斯峰值中心的距离 + 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 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; } -// Initialize Hanning window. Function called only in the first frame. +// 初始化汉明窗口。该函数仅在第一帧调用。 void KCFTracker::createHanningMats() -{ +{ 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++) - hann1t.at (0, i) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann1t.cols - 1))); + hann1t.at(0, i) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann1t.cols - 1))); for (int i = 0; i < hann2t.rows; i++) - hann2t.at (i, 0) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann2t.rows - 1))); + hann2t.at(i, 0) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann2t.rows - 1))); + // 将一维汉明窗口组合成二维汉明窗口 cv::Mat hann2d = hann2t * hann1t; - // HOG features + + // HOG特征 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)); for (int i = 0; i < size_patch[2]; i++) { for (int j = 0; j