matProjDepth = __matProjDepth.colRange(0, h);
vMPCurrentFrame = _vMPCurrentFrame.colRange(0, h);
vAllMatRefFrame = _vAllMatRefFrame.rowRange(0, h);
vLabels = _vLabels.rowRange(0, h);
vAllDepthRefFrame = __vAllDepthRefFrame.rowRange(0, h);
cv::Mat aux;
cv::hconcat(cv::Mat::eye(3, 3, CV_32F), cv::Mat::zeros(3, 1, CV_32F), aux);
cv::Mat matCurrentFrame = K * aux * vMPCurrentFrame;
cv::Mat mat2CurrentFrame(matCurrentFrame.cols, 2, CV_32F);
cv::Mat v2AllMatRefFrame(matCurrentFrame.cols, 3, CV_32F);
cv::Mat mat2ProjDepth(matCurrentFrame.cols, 1, CV_32F);
cv::Mat v2Labels(matCurrentFrame.cols, 1, CV_32F);
cv::Mat _vAllDepthRefFrame(matCurrentFrame.cols, 1, CV_32F);
int j = 0;
for (int i(0); i < matCurrentFrame.cols; i++)
{
float x = ceil(matCurrentFrame.at<float>(0, i) / matCurrentFrame.at<float>(2, i));
float y = ceil(matCurrentFrame.at<float>(1, i) / matCurrentFrame.at<float>(2, i));
if (IsInFrame(x, y, currentFrame))
{
const float d = currentFrame.mImDepth.at<float>(y, x);
if (d > 0)
{
mat2CurrentFrame.at<float>(j, 0) = x;
mat2CurrentFrame.at<float>(j, 1) = y;
v2AllMatRefFrame.at<float>(j, 0) = vAllMatRefFrame.at<float>(i, 0);
v2AllMatRefFrame.at<float>(j, 1) = vAllMatRefFrame.at<float>(i, 1);
v2AllMatRefFrame.at<float>(j, 2) = vAllMatRefFrame.at<float>(i, 2);
_vAllDepthRefFrame.at<float>(j, 0) = vAllDepthRefFrame.at<float>(i, 0);
float d1 = matProjDepth.at<float>(0, i);
mat2ProjDepth.at<float>(j, 0) = d1;
v2Labels.at<float>(j, 0) = vLabels.at<float>(i, 0);
j++;
}
}
}这段代码的主要作用是根据之前代码段中的筛选和复制操作后得到的数据，对一些新的矩阵进行初始化和赋值。
具体解释如下：
matProjDepth = __matProjDepth.colRange(0, h);：将之前_matProjDepth矩阵中的列范围从0到h的部分复制给matProjDepth矩阵。vMPCurrentFrame = _vMPCurrentFrame.colRange(0, h);：将之前_vMPCurrentFrame矩阵中的列范围从0到h的部分复制给vMPCurrentFrame矩阵。vAllMatRefFrame = _vAllMatRefFrame.rowRange(0, h);：将之前_vAllMatRefFrame矩阵中的行范围从0到h的部分复制给vAllMatRefFrame矩阵。vLabels = _vLabels.rowRange(0, h);：将之前_vLabels矩阵中的行范围从0到h的部分复制给vLabels矩阵。vAllDepthRefFrame = __vAllDepthRefFrame.rowRange(0, h);：将之前__vAllDepthRefFrame矩阵中的行范围从0到h的部分复制给vAllDepthRefFrame矩阵。
这些操作是为了截取之前复制并筛选的矩阵的有效范围。
接下来，代码创建了几个新的矩阵并进行了初始化：
aux矩阵是一个3×4的矩阵，通过在3×3的单位矩阵右边添加一个3×1的零矩阵得到。matCurrentFrame矩阵通过将矩阵aux与矩阵vMPCurrentFrame相乘，并再次与矩阵K相乘得到。mat2CurrentFrame矩阵是一个大小为matCurrentFrame列数×2的矩阵。v2AllMatRefFrame矩阵是一个大小为matCurrentFrame列数×3的矩阵。mat2ProjDepth矩阵是一个大小为matCurrentFrame列数×1的矩阵。v2Labels矩阵是一个大小为matCurrentFrame列数×1的矩阵。_vAllDepthRefFrame矩阵是一个大小为matCurrentFrame列数×1的矩阵。
循环，根据一些条件筛选和赋值操作，通过将符合条件的数据从一个矩阵复制到另外一些矩阵中。
具体的解释如下：
int j = 0;：初始化一个变量 j 为 0，用于记录符合条件数据的索引。for (int i(0); i < matCurrentFrame.cols; i++)：循环遍历 matCurrentFrame 矩阵的列数，i 从 0 开始递增。float x = ceil(matCurrentFrame.at<float>(0, i) / matCurrentFrame.at<float>(2, i));：计算将 matCurrentFrame 矩阵第一行第 i 列的值除以第三行第 i 列的值，并向上取整，得到 x 坐标。float y = ceil(matCurrentFrame.at<float>(1, i) / matCurrentFrame.at<float>(2, i));：计算将 matCurrentFrame 矩阵第二行第 i 列的值除以第三行第 i 列的值，并向上取整，得到 y 坐标。if (IsInFrame(x, y, currentFrame))：判断坐标 (x, y) 是否在 currentFrame 的帧范围内。- 在上一步的条件满足时，继续执行以下操作：
const float d = currentFrame.mImDepth.at<float>(y, x);：获取 currentFrame 的深度图像中坐标 (y, x) 处的深度值并赋值给变量 d。if (d > 0)：判断深度值是否大于 0。- 在上一步的条件满足时，执行以下赋值操作：
mat2CurrentFrame.at<float>(j, 0) = x;：将 x 坐标值赋给 mat2CurrentFrame 矩阵第 j 行第 0 列。mat2CurrentFrame.at<float>(j, 1) = y;：将 y 坐标值赋给 mat2CurrentFrame 矩阵第 j 行第 1 列。- 将相关的值从其他矩阵复制到目标矩阵，例如：
v2AllMatRefFrame.at<float>(j, 0) = vAllMatRefFrame.at<float>(i, 0);_vAllDepthRefFrame.at<float>(j, 0) = vAllDepthRefFrame.at<float>(i, 0);float d1 = matProjDepth.at<float>(0, i); mat2ProjDepth.at<float>(j, 0) = d1;v2Labels.at<float>(j, 0) = vLabels.at<float>(i, 0);
- 最后，增加 j 的值使其递增。
这段代码的目的是根据条件筛选并复制符合条件的数据到新的矩阵中，以便进行后续处理和计算。
vAllDepthRefFrame = _vAllDepthRefFrame.rowRange(0, j);
vAllMatRefFrame = v2AllMatRefFrame.rowRange(0, j);
matProjDepth = mat2ProjDepth.rowRange(0, j);
matCurrentFrame = mat2CurrentFrame.rowRange(0, j);
vLabels = v2Labels.rowRange(0, j);
cv::Mat u1((2 * mDmax + 1) * (2 * mDmax + 1), 2, CV_32F);
int m(0);
for (int i(-mDmax); i <= mDmax; i++)
{
for (int j(-mDmax); j <= mDmax; j++)
{
u1.at<float>(m, 0) = i;
u1.at<float>(m, 1) = j;
m++;
}
}
这段代码涉及到一些矩阵操作和循环。
具体的解释如下：
vAllDepthRefFrame = _vAllDepthRefFrame.rowRange(0, j);：将_vAllDepthRefFrame矩阵的前 j 行（索引从 0 到 j-1）复制给vAllDepthRefFrame矩阵。vAllMatRefFrame = v2AllMatRefFrame.rowRange(0, j);：将v2AllMatRefFrame矩阵的前 j 行复制给vAllMatRefFrame矩阵。matProjDepth = mat2ProjDepth.rowRange(0, j);：将mat2ProjDepth矩阵的前 j 行复制给matProjDepth矩阵。matCurrentFrame = mat2CurrentFrame.rowRange(0, j);：将mat2CurrentFrame矩阵的前 j 行复制给matCurrentFrame矩阵。vLabels = v2Labels.rowRange(0, j);：将v2Labels矩阵的前 j 行复制给vLabels矩阵。- 上述操作将通过之前的循环筛选出来的部分数据赋值给新的矩阵。
cv::Mat u1((2 * mDmax + 1) * (2 * mDmax + 1), 2, CV_32F);：创建一个CV_32F类型的矩阵u1，其行数为(2 * mDmax + 1) * (2 * mDmax + 1)，列数为 2。int m(0);：初始化一个变量m为 0，用于记录索引。for (int i(-mDmax); i <= mDmax; i++)：循环遍历i，从-mDmax开始，每次增加 1，直到mDmax。- 在上一步的循环中，继续执行以下操作：
for (int j(-mDmax); j <= mDmax; j++)：循环遍历j，从-mDmax开始，每次增加 1，直到mDmax。- 在内部循环中，执行以下操作：
u1.at<float>(m, 0) = i;：将变量i的值赋给矩阵u1的第m行第 0 列。u1.at<float>(m, 1) = j;：将变量j的值赋给矩阵u1的第m行第 1 列。- 增加变量
m的值使其递增。
这段代码的目的是根据一些参数和条件，进行矩阵操作、数据筛选和数值赋值。最后，在一个循环中生成一个新的矩阵 u1，其中的值根据循环变量 i 和 j 的值依次进行赋值。
cv::Mat matDepthCurrentFrame(matCurrentFrame.rows, 1, CV_32F);
cv::Mat _matProjDepth(matCurrentFrame.rows, 1, CV_32F);
cv::Mat _matCurrentFrame(matCurrentFrame.rows, 2, CV_32F);
int _s(0);
for (int i(0); i < matCurrentFrame.rows; i++)
{
int s(0);
cv::Mat _matDiffDepth(u1.rows, 1, CV_32F);
cv::Mat _matDepth(u1.rows, 1, CV_32F);
for (int j(0); j < u1.rows; j++)
{
int x = (int)matCurrentFrame.at<float>(i, 0) + (int)u1.at<float>(j, 0);
int y = (int)matCurrentFrame.at<float>(i, 1) + (int)u1.at<float>(j, 1);
float _d = currentFrame.mImDepth.at<float>(y, x);
if ((_d > 0) && (_d < matProjDepth.at<float>(i, 0)))
{
_matDepth.at<float>(s, 0) = _d;
_matDiffDepth.at<float>(s, 0) = matProjDepth.at<float>(i, 0) - _d;
s++;
}
}
if (s > 0)
{
_matDepth = _matDepth.rowRange(0, s);
_matDiffDepth = _matDiffDepth.rowRange(0, s);
double minVal, maxVal;
cv::Point minIdx, maxIdx;
cv::minMaxLoc(_matDiffDepth, &minVal, &maxVal, &minIdx, &maxIdx);
int xIndex = minIdx.x;
int yIndex = minIdx.y;
matDepthCurrentFrame.at<float>(_s, 0) = _matDepth.at<float>(yIndex, 0);
_matProjDepth.at<float>(_s, 0) = matProjDepth.at<float>(i, 0);
_matCurrentFrame.at<float>(_s, 0) = matCurrentFrame.at<float>(i, 0);
_matCurrentFrame.at<float>(_s, 1) = matCurrentFrame.at<float>(i, 1);
_s++;
}
}
这段代码执行了一些计算，涉及到矩阵的操作、条件判断和循环。
具体的解释如下：
cv::Mat matDepthCurrentFrame(matCurrentFrame.rows, 1, CV_32F);：创建一个大小为matCurrentFrame.rows行 1 列的浮点型矩阵matDepthCurrentFrame。cv::Mat _matProjDepth(matCurrentFrame.rows, 1, CV_32F);：创建一个大小为matCurrentFrame.rows行 1 列的浮点型矩阵_matProjDepth。cv::Mat _matCurrentFrame(matCurrentFrame.rows, 2, CV_32F);：创建一个大小为matCurrentFrame.rows行 2 列的浮点型矩阵_matCurrentFrame。
以上步骤是为了创建存储结果的矩阵。
int _s(0);：初始化一个变量_s为 0，用于记录索引。for (int i(0); i < matCurrentFrame.rows; i++)：循环遍历i，从 0 开始，逐个增加，直到matCurrentFrame矩阵的行数。- 在上述循环中，执行以下操作：
int s(0);：初始化一个变量s为 0，用于记录索引。cv::Mat _matDiffDepth(u1.rows, 1, CV_32F);：创建一个大小为u1.rows行 1 列的浮点型矩阵_matDiffDepth。cv::Mat _matDepth(u1.rows, 1, CV_32F);：创建一个大小为u1.rows行 1 列的浮点型矩阵_matDepth。- 在内部循环中，执行以下操作：
- 根据当前循环中的
i值计算出x和y坐标。 - 获取
currentFrame.mImDepth矩阵中坐标为(y, x)处的值，并赋给变量_d。 - 如果
_d大于 0 并且小于matProjDepth矩阵中对应位置的值，则进入条件判断内部。 - 在条件成立的情况下，将
_d赋值给_matDepth矩阵的第s行第 0 列，并计算差值赋给_matDiffDepth矩阵的第s行第 0 列。然后递增s的值。
- 根据当前循环中的
- 如果
s大于 0，表示有满足条件的数据，进入条件判断内部。 - 在条件成立的情况下，截取
_matDepth和_matDiffDepth矩阵的前s行。 - 通过
cv::minMaxLoc函数计算_matDiffDepth矩阵中的最小值、最大值及其位置。 - 获取最小值位置的横坐标
xIndex和纵坐标yIndex。 - 根据索引值，将对应位置的数据赋给结果矩阵
matDepthCurrentFrame、_matProjDepth和_matCurrentFrame。 - 递增
_s的值。
这段代码的目的是根据一些条件和数据进行筛选和计算，从而得到最终的结果矩阵和索引值。
matDepthCurrentFrame = matDepthCurrentFrame.rowRange(0, _s);
matProjDepth = _matProjDepth.rowRange(0, _s);
matCurrentFrame = _matCurrentFrame.rowRange(0, _s);
mDepthThreshold = 0.6;
cv::Mat matDepthDifference = matProjDepth - matDepthCurrentFrame;
mVarThreshold = 0.001; // 0.040;
vector<Geometry::DynKeyPoint> vDynPoints;
for (int i(0); i < matCurrentFrame.rows; i++)
{
if (matDepthDifference.at<float>(i, 0) > mDepthThreshold)
{
int xIni = (int)matCurrentFrame.at<float>(i, 0) - mDmax;
int yIni = (int)matCurrentFrame.at<float>(i, 1) - mDmax;
int xEnd = (int)matCurrentFrame.at<float>(i, 0) + mDmax + 1;
int yEnd = (int)matCurrentFrame.at<float>(i, 1) + mDmax + 1;
cv::Mat patch = currentFrame.mImDepth.rowRange(yIni, yEnd).colRange(xIni, xEnd);
cv::Mat mean, stddev;
cv::meanStdDev(patch, mean, stddev);
double _stddev = stddev.at<double>(0, 0);
double var = _stddev * _stddev;
if (var < mVarThreshold)
{
DynKeyPoint dynPoint;
dynPoint.mPoint.x = matCurrentFrame.at<float>(i, 0);
dynPoint.mPoint.y = matCurrentFrame.at<float>(i, 1);
dynPoint.mRefFrameLabel = vLabels.at<float>(i, 0);
vDynPoints.push_back(dynPoint);
}
}
}
return vDynPoints;
}
else
{
vector<Geometry::DynKeyPoint> vDynPoints;
return vDynPoints;
}
}这段代码是一个函数，根据给定的输入参数进行深度图像处理，并返回动态关键点的向量。
函数首先对一些变量进行初始化，包括matDepthCurrentFrame、matProjDepth和matCurrentFrame（这些都是OpenCV的cv::Mat对象）。然后，定义了一些阈值（mDepthThreshold和mVarThreshold）。
接下来，通过对matProjDepth和matDepthCurrentFrame进行减法操作，计算了matDepthDifference。然后，使用一个循环遍历matCurrentFrame的每一行。
在循环中，首先检查matDepthDifference当前行的第一个元素是否大于mDepthThreshold。如果是，则进一步处理该点。
然后，根据matCurrentFrame当前行的前两个元素（即坐标）计算出一个矩形区域，并提取当前帧的深度图像中对应区域的一个小块（patch）。
接下来，使用cv::meanStdDev函数计算patch的均值和标准差，并根据标准差计算方差（var）。
最后，如果方差小于mVarThreshold，则创建一个DynKeyPoint对象，将相关信息（包括位置和参考帧标签）存储在其中，并将该对象添加到vDynPoints向量中。
最后，在函数的末尾，如果输入参数不满足某些条件（判断条件看不到提供的完整代码部分），则返回一个空的vDynPoints向量。
综上所述，该函数的作用是从给定的深度图像中提取满足一定条件的动态关键点，并将这些关键点以DynKeyPoint对象的形式存储在一个向量中返回。
扫一扫
1037
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
