Dyna-SLAM代码解读：Geometry.cc（六）

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) 最近的非空像素的坐标。

代码解读如下：

1. 首先，创建了一个 4x2 的矩阵 neigbIni，数据类型为 CV_32F，用于存储相邻像素的偏移量。偏移量的值为：(-1, 0), (1, 0), (0, -1), (0, 1)。这样定义的偏移量表示了图像中一个像素的上、下、左、右四个相邻位置。
2. 创建一个变量 neigb，并将其初始化为 neigbIni。neigb 变量将用于在后续的循环中逐步扩展相邻像素的偏移量。
3. 初始化一个布尔变量 found 并置为 false，用于标记是否找到非空像素。
4. 初始化一个整数变量 f 并设置为 2，用于控制相邻像素偏移量的扩展。
5. 进入一个 while 循环，条件为 !found，即当尚未找到非空像素时循环执行以下代码块： 在一个 for 循环中，遍历上述定义的四个相邻位置。对于每个位置，根据当前 (x, y) 的坐标和相邻位置的偏移量，计算得到新的相邻位置 (xn, yn)。 判断新的相邻位置 (xn, yn) 是否在图像范围内（不越界）并且是否对应 mask 图像中的非零像素。 如果满足条件，即 (xn, yn) 不越界且对应 mask 图像中的非零像素，则将 found 置为 true，同时将 (xn, yn) 分配给变量 _x 和 _y，即保存最近的非空像素的坐标。 退出 for 循环。 更新 neigb 变量的值，通过乘以 f 来扩展相邻像素的偏移量。这样可以在下一次循环中遍历更远的相邻位置。 增加 f 的值，以控制相邻像素偏移量的扩展。
6. 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 表示。

代码解读如下：

1. 首先，通过传入的参数 currentFrame 作为要插入的帧数据。
2. 使用条件判断语句，通过调用 IsFull() 函数来判断数据库是否已满。
3. 如果数据库未满，执行以下代码块： 将当前帧数据 currentFrame 插入到数据库数组 mvDataBase 的索引位置 mFin 处。 更新 mFin 为下一个位置，通过 (mFin + 1) % MAX_DB_SIZE 计算。这样做是为了循环利用数组空间，使得新的数据可以插入到数组的开头位置。 增加数据库中元素的计数器 mNumElem 的值。
4. 如果数据库已满，执行以下代码块： 将当前帧数据 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。

代码解读如下：

1. 首先，通过 (mFin + 1) % MAX_DB_SIZE 计算出下一个可插入数据的位置，即索引 mFin 值的下一个位置。这里使用取模运算 (mFin + 1) % MAX_DB_SIZE 是为了循环利用数组空间。
2. 将计算出的下一个位置与索引 mIni 进行比较。
3. 如果 mIni 等于计算出的下一个位置，说明数据库已满，返回 true 表示数据库已满。
4. 如果 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，表示输入的旋转矩阵。

该函数的实现包括以下步骤：

1. assert(isRotationMatrix(R))：这是一个断言语句，用于确保输入的旋转矩阵 R 是一个合法的旋转矩阵。具体的合法性检查逻辑在 isRotationMatrix() 函数中实现。
2. 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。
3. bool singular = sy < 1e-6：判断旋转矩阵是否处于 singularity 的状态。当 sy 的值小于给定的阈值 1e-6 时，认为旋转矩阵处于 singularity。
4. 根据 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。
5. cv::Mat res = (cv::Mat_<double>(1, 3) << x, y, z)：构造一个大小为 1x3 的 cv::Mat 对象 res，并将计算得到的欧拉角依次填充到矩阵的元素中。
6. 返回欧拉角矩阵 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，表示待检查的矩阵。

函数的实现包括以下步骤：

1. cv::Mat Rt; transpose(R, Rt);：将矩阵 R 进行转置操作，将结果存储在矩阵 Rt 中。这一步是为了得到旋转矩阵的转置矩阵。
2. cv::Mat shouldBeIdentity = Rt * R;：将转置矩阵 Rt 与矩阵 R 相乘，得到结果矩阵 shouldBeIdentity。这一步是为了检查两个矩阵的乘积是否接近单位矩阵。
3. cv::Mat I = cv::Mat::eye(3, 3, shouldBeIdentity.type());：创建一个3x3的单位矩阵 I，并与 shouldBeIdentity 具有相同的数据类型。
4. 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，表示待检查的帧。

函数的实现包括以下步骤：

1. mDmax = 20;：将类成员变量 mDmax 的值设置为 20。这个变量表示距离的最大值。在该函数中，它被用作边界值的偏移量。
2. (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 中标记该像素为已生长。

主循环会在满足以下两个条件之一时终止：

1. pixdist（像素距离）超过reg_maxdist。
2. 区域已经包含了整个图像。

最终，函数返回矩阵 J，其中像素值为 1 的位置表示参与到区域生长的像素。

总之，RegionGrowing 函数使用区域生长算法从给定坐标开始，通过判断相邻像素是否满足条件，不断扩展区域，直到满足终止条件。

1. if ((neg_pos + 10) > neg_free)：这个条件判断用于检查是否需要添加新的内存块。如果待探索像素列表 neg_list 已经接近满了，即 neg_pos 加上 10 大于 neg_free，则会添加一个新的内存块 _neg_list。
2. cv::Mat _neg_list = cv::Mat::zeros(_neg_free, 3, CV_32F);：在内存中创建一个大小为 _neg_free 行、3 列的零矩阵 _neg_list，用于存储新的像素坐标和像素值。
3. neg_free += 10000;：增加 neg_free 的大小，以便容纳更多的待探索像素。
4. vconcat(neg_list, _neg_list, neg_list);：将新的内存块 _neg_list 连接到原始的待探索像素列表 neg_list 中。
5. 接下来的代码使用了像素距离度量，在待探索像素列表 neg_list 中找到像素值与当前区域均值 reg_mean 最接近的像素，并将其添加到区域中。
6. cv::Mat dist;：创建一个空矩阵 dist，用于存储像素距离。
7. for (int i(0); i < neg_pos; i++)：遍历待探索像素列表 neg_list 中的像素。
8. double d = abs(neg_list.at<float>(i, 2) - reg_mean);：计算当前像素与当前区域均值之间的像素距离，并将其保存在变量 d 中。
9. dist.push_back(d);：将像素距离 d 添加到距离矩阵 dist 中。
10. cv::minMaxLoc(dist, &pixdist, &max, &ind, &maxpos);：找到距离矩阵 dist 中的最小值并记录其位置。
11. int index = ind.y;：获取最小距离的像素在待探索像素列表 neg_list 中的索引。
12. if (index != -1)：如果找到了最小距离的像素。
13. J.at<float>(y, x) = -1.;：将当前像素 (x, y) 的像素值设置为 -1，表示它已经被添加到区域中。
14. reg_size += 1;：增加区域大小。
15. reg_mean = (reg_mean * reg_size + neg_list.at<float>(index, 2)) / (reg_size + 1);：计算更新后的区域均值。
16. x = neg_list.at<float>(index, 0); 和 y = neg_list.at<float>(index, 1);：保存像素 (x, y) 的坐标，用于之后的邻域添加。
17. neg_list.at<float>(index, 0) = neg_list.at<float>(neg_pos, 0); 和其他两行代码：从待探索像素列表 neg_list 中移除已经被添加到区域的像素。
18. pixdist = reg_maxdist;：如果没有找到满足条件的像素，则将 pixdist 设置为 reg_maxdist。
19. J = cv::abs(J);：将像素值矩阵 J 中的所有像素取绝对值。
20. return (J);：返回更新后的像素值矩阵 J。

这段代码的作用是实现区域生长算法的一部分，尝试将与当前区域均值最接近的像素添加到区域中，并更新区域的均值。最后将像素值矩阵取绝对值后返回。

免责声明：本文系网络转载或改编，未找到原创作者，版权归原作者所有。如涉及版权，请联系删