11、查找给定坐标 (x, y) 最近的非空像素的坐标，并将结果保存到 _x 和 _y 变量中
void Geometry::GetClosestNonEmptyCoordinates(const cv::Mat &mask, const int &x, const int &y, int &_x, int &_y)
{
cv::Mat neigbIni(4, 2, CV_32F);
neigbIni.at<float>(0, 0) = -1;
neigbIni.at<float>(0, 1) = 0;
neigbIni.at<float>(1, 0) = 1;
neigbIni.at<float>(1, 1) = 0;
neigbIni.at<float>(2, 0) = 0;
neigbIni.at<float>(2, 1) = -1;
neigbIni.at<float>(3, 0) = 0;
neigbIni.at<float>(3, 1) = 1;
cv::Mat neigb = neigbIni;
bool found = false;
int f(2);
while (!found)
{
for (int j(0); j < 4; j++)
{
int xn = x + neigb.at<float>(j, 0);
int yn = y + neigb.at<float>(j, 1);
bool ins = ((xn >= 0) && (yn >= 0) && (xn <= mask.cols) && (yn <= mask.rows));
if (ins && ((int)mask.at<uchar>(yn, xn) == 1))
{
found = true;
_x = xn;
_y = yn;
}
}
neigb = f * neigbIni;
f++;
}
}这段代码实现了一个函数 GetClosestNonEmptyCoordinates，用于查找离给定坐标 (x, y) 最近的非空像素的坐标。
代码解读如下：
-
首先，创建了一个
4x2的矩阵neigbIni，数据类型为CV_32F，用于存储相邻像素的偏移量。偏移量的值为：(-1, 0), (1, 0), (0, -1), (0, 1)。这样定义的偏移量表示了图像中一个像素的上、下、左、右四个相邻位置。 -
创建一个变量
neigb，并将其初始化为neigbIni。neigb变量将用于在后续的循环中逐步扩展相邻像素的偏移量。 -
初始化一个布尔变量
found并置为false，用于标记是否找到非空像素。 -
初始化一个整数变量
f并设置为 2，用于控制相邻像素偏移量的扩展。 -
进入一个
while循环，条件为!found，即当尚未找到非空像素时循环执行以下代码块：-
在一个
for循环中，遍历上述定义的四个相邻位置。对于每个位置，根据当前(x, y)的坐标和相邻位置的偏移量，计算得到新的相邻位置(xn, yn)。 -
判断新的相邻位置
(xn, yn)是否在图像范围内（不越界）并且是否对应mask图像中的非零像素。 -
如果满足条件，即
(xn, yn)不越界且对应mask图像中的非零像素，则将found置为true，同时将(xn, yn)分配给变量_x和_y，即保存最近的非空像素的坐标。 -
退出
for循环。 -
更新
neigb变量的值，通过乘以f来扩展相邻像素的偏移量。这样可以在下一次循环中遍历更远的相邻位置。 -
增加
f的值，以控制相邻像素偏移量的扩展。
-
-
while循环结束后，如果找到了最近的非空像素，那么_x和_y将保存最终结果；如果没有找到，那么_x和_y的值将保持不变。
这段代码的作用是查找给定坐标 (x, y) 最近的非空像素的坐标，并将结果保存到 _x 和 _y 变量中。这在图像处理和计算机视觉领域中常用于寻找邻近的特征点或目标。
12、 在数据库中插入帧数据
void Geometry::DataBase::InsertFrame2DB(const ORB_SLAM2::Frame ¤tFrame)
{
if (!IsFull())
{
mvDataBase[mFin] = currentFrame;
mFin = (mFin + 1) % MAX_DB_SIZE;
mNumElem += 1;
}
else
{
mvDataBase[mIni] = currentFrame;
mFin = mIni;
mIni = (mIni + 1) % MAX_DB_SIZE;
}
}
这段代码是一个函数 InsertFrame2DB，它属于 Geometry 命名空间中的 DataBase 类的成员函数。
该函数用于向一个数据库中插入帧数据。数据库由一个大小为 MAX_DB_SIZE 的固定长度的数组 mvDataBase 表示。
代码解读如下：
-
首先，通过传入的参数
currentFrame作为要插入的帧数据。 -
使用条件判断语句，通过调用
IsFull()函数来判断数据库是否已满。 -
如果数据库未满，执行以下代码块：
-
将当前帧数据
currentFrame插入到数据库数组mvDataBase的索引位置mFin处。 -
更新
mFin为下一个位置，通过(mFin + 1) % MAX_DB_SIZE计算。这样做是为了循环利用数组空间，使得新的数据可以插入到数组的开头位置。 -
增加数据库中元素的计数器
mNumElem的值。
-
-
如果数据库已满，执行以下代码块：
-
将当前帧数据
currentFrame插入到数据库数组mvDataBase的索引位置mIni处。这将覆盖原有的数据。 -
更新
mFin的值为mIni，用于保证插入新数据后，最近插入的数据始终在数据库的末尾。 -
更新
mIni为下一个位置，通过(mIni + 1) % MAX_DB_SIZE计算。
-
这段代码的功能是在数据库中插入帧数据。如果数据库未满，将数据插入到数据库末尾；如果数据库已满，将数据插入到数据库开头，并覆盖原有数据。通过循环利用数组空间，保持数据库中最近插入的数据位于末尾。
13、 判断数据库是否已满
bool Geometry::DataBase::IsFull()
{
return (mIni == (mFin + 1) % MAX_DB_SIZE);
} 这段代码是一个函数 IsFull()，它属于 Geometry 命名空间中的 DataBase 类的成员函数。
该函数用于判断数据库是否已满。返回值为布尔类型，如果数据库已满则返回 true，否则返回 false。
代码解读如下：
-
首先，通过
(mFin + 1) % MAX_DB_SIZE计算出下一个可插入数据的位置，即索引mFin值的下一个位置。这里使用取模运算(mFin + 1) % MAX_DB_SIZE是为了循环利用数组空间。 -
将计算出的下一个位置与索引
mIni进行比较。 -
如果
mIni等于计算出的下一个位置，说明数据库已满，返回true表示数据库已满。 -
如果
mIni不等于计算出的下一个位置，说明数据库未满，返回false表示数据库未满。
这段代码的功能是判断数据库是否已满。它通过比较索引 mIni 和计算出的下一个可插入数据的位置来判断数据库是否已满。如果 mIni 等于下一个位置，表示数据库已满；否则，表示数据库未满。
14、
cv::Mat Geometry::rotm2euler(const cv::Mat &R)
{
assert(isRotationMatrix(R));
float sy = sqrt(R.at<double>(0, 0) * R.at<double>(0, 0) + R.at<double>(1, 0) * R.at<double>(1, 0));
bool singular = sy < 1e-6;
float x, y, z;
if (!singular)
{
x = atan2(R.at<double>(2, 1), R.at<double>(2, 2));
y = atan2(-R.at<double>(2, 0), sy);
z = atan2(R.at<double>(1, 0), R.at<double>(0, 0));
}
else
{
x = atan2(-R.at<double>(1, 2), R.at<double>(1, 1));
y = atan2(-R.at<double>(2, 0), sy);
z = 0;
}
cv::Mat res = (cv::Mat_<double>(1, 3) << x, y, z);
return res;
} 这段代码是一个函数 rotm2euler()，它属于 Geometry 命名空间中的类的成员函数。
这个函数用于将一个旋转矩阵 R 转换为欧拉角表示。它接受一个 cv::Mat 类型的参数 R，表示输入的旋转矩阵。
该函数的实现包括以下步骤：
-
assert(isRotationMatrix(R))：这是一个断言语句，用于确保输入的旋转矩阵R是一个合法的旋转矩阵。具体的合法性检查逻辑在isRotationMatrix()函数中实现。 -
float sy = sqrt(R.at<double>(0, 0) * R.at<double>(0, 0) + R.at<double>(1, 0) * R.at<double>(1, 0))：计算旋转矩阵的第一列与第二列在 y 方向上的分量的平方和的平方根。这个值用于判断旋转矩阵的 singularity。 -
bool singular = sy < 1e-6：判断旋转矩阵是否处于 singularity 的状态。当sy的值小于给定的阈值1e-6时，认为旋转矩阵处于 singularity。 -
根据
singular的值，决定如何计算欧拉角：- 如果旋转矩阵不处于 singularity 状态，执行以下计算：
x = atan2(R.at<double>(2, 1), R.at<double>(2, 2))：计算绕 x 轴的旋转角度。y = atan2(-R.at<double>(2, 0), sy)：计算绕 y 轴的旋转角度。z = atan2(R.at<double>(1, 0), R.at<double>(0, 0))：计算绕 z 轴的旋转角度。
- 如果旋转矩阵处于 singularity 状态，执行以下计算：
x = atan2(-R.at<double>(1, 2), R.at<double>(1, 1))：计算绕 x 轴的旋转角度。y = atan2(-R.at<double>(2, 0), sy)：计算绕 y 轴的旋转角度。z = 0：由于旋转矩阵处于 singularity，所以绕 z 轴的旋转角度被设为 0。
- 如果旋转矩阵不处于 singularity 状态，执行以下计算：
-
cv::Mat res = (cv::Mat_<double>(1, 3) << x, y, z)：构造一个大小为 1x3 的cv::Mat对象res，并将计算得到的欧拉角依次填充到矩阵的元素中。 -
返回欧拉角矩阵
res。
总而言之，这段代码实现了将旋转矩阵转换为欧拉角表示的功能，包括对输入旋转矩阵的合法性检查和 singularity 状态的处理。
15、该函数用于检查给定的矩阵 R 是否为一个合法的旋转矩阵
bool Geometry::isRotationMatrix(const cv::Mat &R)
{
cv::Mat Rt;
transpose(R, Rt);
cv::Mat shouldBeIdentity = Rt * R;
cv::Mat I = cv::Mat::eye(3, 3, shouldBeIdentity.type());
return norm(I, shouldBeIdentity) < 1e-6;
} 该函数用于检查给定的矩阵 R 是否为一个合法的旋转矩阵
它接受一个 cv::Mat 类型的参数 R，表示待检查的矩阵。
函数的实现包括以下步骤：
-
cv::Mat Rt; transpose(R, Rt);：将矩阵R进行转置操作，将结果存储在矩阵Rt中。这一步是为了得到旋转矩阵的转置矩阵。 -
cv::Mat shouldBeIdentity = Rt * R;：将转置矩阵Rt与矩阵R相乘，得到结果矩阵shouldBeIdentity。这一步是为了检查两个矩阵的乘积是否接近单位矩阵。 -
cv::Mat I = cv::Mat::eye(3, 3, shouldBeIdentity.type());：创建一个3x3的单位矩阵I，并与shouldBeIdentity具有相同的数据类型。 -
return norm(I, shouldBeIdentity) < 1e-6;：计算矩阵I与shouldBeIdentity之间的差异，并将其范数与一个极小值1e-6进行比较。如果差异较小，则认为矩阵R是一个合法的旋转矩阵，返回true，否则返回false。
总而言之，isRotationMatrix() 函数用于检查给定的矩阵是否满足旋转矩阵的性质，即它的转置矩阵和自身的乘积接近单位矩阵。这个函数在其他功能函数（如 rotm2euler() 函数）中被使用，以确保输入的旋转矩阵的合法性。
16、该函数用于检查给定的坐标 (x, y) 是否在给定的帧 Frame 的有效范围内
bool Geometry::IsInFrame(const float &x, const float &y, const ORB_SLAM2::Frame &Frame)
{
mDmax = 20;
return (x > (mDmax + 1) && x < (Frame.mImDepth.cols - mDmax - 1) && y > (mDmax + 1) && y < (Frame.mImDepth.rows - mDmax - 1));
}这段代码是 Geometry 命名空间中的 IsInFrame() 函数的实现。
该函数用于检查给定的坐标 (x, y) 是否在给定的帧 Frame 的有效范围内。函数接受两个 float 类型的参数 x 和 y，表示待检查的坐标，以及一个 ORB_SLAM2::Frame 类型的参数 Frame，表示待检查的帧。
函数的实现包括以下步骤：
-
mDmax = 20;：将类成员变量mDmax的值设置为 20。这个变量表示距离的最大值。在该函数中，它被用作边界值的偏移量。 -
(x > (mDmax + 1) && x < (Frame.mImDepth.cols - mDmax - 1) && y > (mDmax + 1) && y < (Frame.mImDepth.rows - mDmax - 1))：通过一系列条件语句来判断给定的坐标(x, y)是否在帧Frame的有效范围内。具体地，条件要求x坐标在(mDmax + 1)和(Frame.mImDepth.cols - mDmax - 1)之间，而y坐标在(mDmax + 1)和(Frame.mImDepth.rows - mDmax - 1)之间。如果满足这些条件，则返回true，表示坐标在帧的有效范围内，否则返回false。
总而言之，IsInFrame() 函数用于判断给定的坐标 (x, y) 是否在给定的帧 Frame 的有效范围内。函数使用了类成员变量 mDmax 来指定距离的最大值，并通过条件语句来判断坐标是否在有效范围内。这个函数通常用于对坐标进行边界检查，以确保它们不超出图像的范围。
17、该函数用于检查给定的坐标 (x, y) 是否在给定的图像 image 的有效范围内
bool Geometry::IsInImage(const float &x, const float &y, const cv::Mat image)
{
return (x >= 0 && x < (image.cols) && y >= 0 && y < image.rows);
}这段代码是 Geometry 命名空间中的函数 IsInImage() 的实现。
该函数用于检查给定的坐标 (x, y) 是否在给定的图像 image 的有效范围内。函数接受两个 float 类型的参数 x 和 y，表示待检查的坐标，以及一个 cv::Mat 类型的参数 image，表示待检查的图像。
函数的实现如下：
(x >= 0 && x < (image.cols) && y >= 0 && y < image.rows)：通过一系列条件语句来判断给定的坐标 (x, y) 是否在图像 image 的有效范围内。具体地，条件要求 x 坐标在大于等于 0 并且小于图像的列数 image.cols，而 y 坐标在大于等于 0 并且小于图像的行数 image.rows。如果满足这些条件，则返回 true，表示坐标在图像的有效范围内，否则返回 false。
总而言之，IsInImage() 函数用于判断给定的坐标 (x, y) 是否在给定的图像 image 的有效范围内。函数通过条件语句来检查坐标是否在图像的有效范围内，以确保它们不超出图像的尺寸范围。常用于边界检查，确保不访问超出图像边界的像素。
18、使用区域生长算法来扩展从给定坐标 (x, y) 开始的区域
cv::Mat Geometry::RegionGrowing(const cv::Mat &im, int &x, int &y, const float ®_maxdist)
{
cv::Mat J = cv::Mat::zeros(im.size(), CV_32F);
float reg_mean = im.at<float>(y, x);
int reg_size = 1;
int _neg_free = 10000;
int neg_free = 10000;
int neg_pos = -1;
cv::Mat neg_list = cv::Mat::zeros(neg_free, 3, CV_32F);
double pixdist = 0;
// Neighbor locations (footprint)
cv::Mat neigb(4, 2, CV_32F);
neigb.at<float>(0, 0) = -1;
neigb.at<float>(0, 1) = 0;
neigb.at<float>(1, 0) = 1;
neigb.at<float>(1, 1) = 0;
neigb.at<float>(2, 0) = 0;
neigb.at<float>(2, 1) = -1;
neigb.at<float>(3, 0) = 0;
neigb.at<float>(3, 1) = 1;
while (pixdist < reg_maxdist && reg_size < im.total())
{
for (int j(0); j < 4; j++)
{
// Calculate the neighbour coordinate
int xn = x + neigb.at<float>(j, 0);
int yn = y + neigb.at<float>(j, 1);
bool ins = ((xn >= 0) && (yn >= 0) && (xn < im.cols) && (yn < im.rows));
if (ins && (J.at<float>(yn, xn) == 0.))
{
neg_pos++;
neg_list.at<float>(neg_pos, 0) = xn;
neg_list.at<float>(neg_pos, 1) = yn;
neg_list.at<float>(neg_pos, 2) = im.at<float>(yn, xn);
J.at<float>(yn, xn) = 1.;
}
}
// Add a new block of free memory
if ((neg_pos + 10) > neg_free)
{
cv::Mat _neg_list = cv::Mat::zeros(_neg_free, 3, CV_32F);
neg_free += 10000;
vconcat(neg_list, _neg_list, neg_list);
}
// Add pixel with intensity nearest to the mean of the region, to the region
cv::Mat dist;
for (int i(0); i < neg_pos; i++)
{
double d = abs(neg_list.at<float>(i, 2) - reg_mean);
dist.push_back(d);
}
double max;
cv::Point ind, maxpos;
cv::minMaxLoc(dist, &pixdist, &max, &ind, &maxpos);
int index = ind.y;
if (index != -1)
{
J.at<float>(y, x) = -1.;
reg_size += 1;
// Calculate the new mean of the region
reg_mean = (reg_mean * reg_size + neg_list.at<float>(index, 2)) / (reg_size + 1);
// Save the x and y coordinates of the pixel (for the neighbour add proccess)
x = neg_list.at<float>(index, 0);
y = neg_list.at<float>(index, 1);
// Remove the pixel from the neighbour (check) list
neg_list.at<float>(index, 0) = neg_list.at<float>(neg_pos, 0);
neg_list.at<float>(index, 1) = neg_list.at<float>(neg_pos, 1);
neg_list.at<float>(index, 2) = neg_list.at<float>(neg_pos, 2);
neg_pos -= 1;
}
else
{
pixdist = reg_maxdist;
}
}
J = cv::abs(J);
return (J);
}这段代码实现了 Geometry 命名空间中的 RegionGrowing 函数。该函数使用区域生长算法来扩展从给定坐标 (x, y) 开始的区域。
函数接受以下参数：
im：输入图像，类型为cv::Mat。x和y：起始坐标，类型为int引用。reg_maxdist：区域生长的最大距离阈值，类型为float。
在函数内部，首先创建了一个大小与输入图像相同、类型为 CV_32F 的全零矩阵 J。该矩阵用于标记已经生长过的像素。
接下来，定义了一些变量，包括 reg_mean、reg_size、_neg_free、neg_free、neg_pos 以及 neg_list，用于记录生长过程中的信息。
-
reg_mean：该变量用于记录当前区域的像素值均值。它初始化为起始像素(x, y)处的像素值im.at<float>(y, x)，在区域生长过程中会不断更新。 -
reg_size：该变量用于记录当前区域的像素数量。它初始化为1，表示起始像素(x, y)。随着区域生长，每次将一个新的像素加入区域时，reg_size会增加。 -
_neg_free和neg_free：这两个变量用于控制neg_list的大小。_neg_free是初始大小，neg_free是实际使用的大小。这两个变量的设定为了减少动态扩展neg_list的次数，提高效率。 -
neg_pos：该变量用于记录neg_list中最后一个被添加的像素的索引。初始值为 -1，表示neg_list是空的。 -
neg_list：这是一个二维矩阵，用于存储待探索的像素的坐标以及对应的像素值。每行包含三个元素：像素的 x 坐标、像素的 y 坐标和像素的值。neg_pos变量用于指示最后一个被添加的像素在neg_list中的索引。
然后，定义了一个大小为 4x2 的矩阵 neigb，用于指定四个相邻像素的偏移量。这些偏移量分别为上下左右四个方向。
在主循环中，通过迭代判断条件来进行区域生长。首先，遍历 neigb 矩阵的每个元素，计算得到相邻像素的坐标 (xn, yn)。然后，通过判断 (xn, yn) 是否在图像范围内以及对应位置的矩阵 J 的值是否为零，判断该像素是否应该加入到区域中。
如果满足条件，将该像素的坐标 (xn, yn) 以及对应的像素值 im.at<float>(yn, xn) 记录到 neg_list 中，并在矩阵 J 中标记该像素为已生长。
主循环会在满足以下两个条件之一时终止：
pixdist（像素距离）超过reg_maxdist。- 区域已经包含了整个图像。
最终，函数返回矩阵 J，其中像素值为 1 的位置表示参与到区域生长的像素。
总之，RegionGrowing 函数使用区域生长算法从给定坐标开始，通过判断相邻像素是否满足条件，不断扩展区域，直到满足终止条件。
-
if ((neg_pos + 10) > neg_free)：这个条件判断用于检查是否需要添加新的内存块。如果待探索像素列表neg_list已经接近满了，即neg_pos加上 10 大于neg_free，则会添加一个新的内存块_neg_list。 -
cv::Mat _neg_list = cv::Mat::zeros(_neg_free, 3, CV_32F);：在内存中创建一个大小为_neg_free行、3 列的零矩阵_neg_list，用于存储新的像素坐标和像素值。 -
neg_free += 10000;：增加neg_free的大小，以便容纳更多的待探索像素。 -
vconcat(neg_list, _neg_list, neg_list);：将新的内存块_neg_list连接到原始的待探索像素列表neg_list中。 -
接下来的代码使用了像素距离度量，在待探索像素列表
neg_list中找到像素值与当前区域均值reg_mean最接近的像素，并将其添加到区域中。 -
cv::Mat dist;：创建一个空矩阵dist，用于存储像素距离。 -
for (int i(0); i < neg_pos; i++)：遍历待探索像素列表neg_list中的像素。 -
double d = abs(neg_list.at<float>(i, 2) - reg_mean);：计算当前像素与当前区域均值之间的像素距离，并将其保存在变量d中。 -
dist.push_back(d);：将像素距离d添加到距离矩阵dist中。 -
cv::minMaxLoc(dist, &pixdist, &max, &ind, &maxpos);：找到距离矩阵dist中的最小值并记录其位置。 -
int index = ind.y;：获取最小距离的像素在待探索像素列表neg_list中的索引。 -
if (index != -1)：如果找到了最小距离的像素。 -
J.at<float>(y, x) = -1.;：将当前像素(x, y)的像素值设置为 -1，表示它已经被添加到区域中。 -
reg_size += 1;：增加区域大小。 -
reg_mean = (reg_mean * reg_size + neg_list.at<float>(index, 2)) / (reg_size + 1);：计算更新后的区域均值。 -
x = neg_list.at<float>(index, 0);和y = neg_list.at<float>(index, 1);：保存像素(x, y)的坐标，用于之后的邻域添加。 -
neg_list.at<float>(index, 0) = neg_list.at<float>(neg_pos, 0);和其他两行代码：从待探索像素列表neg_list中移除已经被添加到区域的像素。 -
pixdist = reg_maxdist;：如果没有找到满足条件的像素，则将pixdist设置为reg_maxdist。 -
J = cv::abs(J);：将像素值矩阵J中的所有像素取绝对值。 -
return (J);：返回更新后的像素值矩阵J。
这段代码的作用是实现区域生长算法的一部分，尝试将与当前区域均值最接近的像素添加到区域中，并更新区域的均值。最后将像素值矩阵取绝对值后返回。
扫一扫
6192
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
