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

vector<Geometry::DynKeyPoint> Geometry::ExtractDynPoints(const vector<ORB_SLAM2::Frame> &vRefFrames,                                                         const ORB_SLAM2::Frame ¤tFrame){    cv::Mat K = cv::Mat::eye(3,3,CV_32F);    K.at<float>(0,0) = currentFrame.fx;    K.at<float>(1,1) = currentFrame.fy;    K.at<float>(0,2) = currentFrame.cx;    K.at<float>(1,2) = currentFrame.cy;     cv::Mat vAllMPw;    cv::Mat vAllMatRefFrame;    cv::Mat vAllLabels;    cv::Mat vAllDepthRefFrame;     for (int i(0); i < mnRefFrames; i++)    {        ORB_SLAM2::Frame refFrame = vRefFrames[i];         // Fill matrix with points        cv::Mat matRefFrame(refFrame.N,3,CV_32F);        cv::Mat matDepthRefFrame(refFrame.N,1,CV_32F);        cv::Mat matInvDepthRefFrame(refFrame.N,1,CV_32F);        cv::Mat vLabels(refFrame.N,1,CV_32F);        int k(0);        for(int j(0); j < refFrame.N; j++){            const cv::KeyPoint &kp = refFrame.mvKeys[j];            const float &v = kp.pt.y;            const float &u = kp.pt.x;            const float d = refFrame.mImDepth.at<float>(v,u);            if (d > 0 && d < 6){                matRefFrame.at<float>(k,0) = refFrame.mvKeysUn[j].pt.x;                matRefFrame.at<float>(k,1) = refFrame.mvKeysUn[j].pt.y;                matRefFrame.at<float>(k,2) = 1.;                matInvDepthRefFrame.at<float>(k,0) = 1./d;                matDepthRefFrame.at<float>(k,0) = d;                vLabels.at<float>(k,0) = i;                k++;            }        }

这段代码是一个循环，用于遍历参考帧向量vRefFrames中的每个参考帧，并填充一些矩阵变量。

在循环中，首先通过ORB_SLAM2::Frame refFrame = vRefFrames[i];将当前参考帧赋值给refFrame。

然后，以下是循环中的每一步操作：

• cv::Mat matRefFrame(refFrame.N, 3, CV_32F); - 创建一个大小为refFrame.N×3的浮点型矩阵matRefFrame，用于存储参考帧的特征点在相机坐标系中的坐标。
• cv::Mat matDepthRefFrame(refFrame.N, 1, CV_32F); - 创建一个大小为refFrame.N×1的浮点型矩阵matDepthRefFrame，用于存储参考帧特征点的深度值。
• cv::Mat matInvDepthRefFrame(refFrame.N, 1, CV_32F); - 创建一个大小为refFrame.N×1的浮点型矩阵matInvDepthRefFrame，用于存储参考帧特征点的逆深度值。
• cv::Mat vLabels(refFrame.N, 1, CV_32F); - 创建一个大小为refFrame.N×1的浮点型矩阵vLabels，用于存储特征点的标签。
• 初始化整型变量k为0，k用于追踪有效特征点的数量。
• 在for循环中，遍历参考帧refFrame中的每个特征点j。
• const cv::KeyPoint &kp = refFrame.mvKeys[j]; - 获取当前特征点的关键点信息。
• 提取当前特征点的像素坐标u和v，以及深度值d。const float &v = kp.pt.y;和const float &u = kp.pt.x;用于获取像素坐标，const float d = refFrame.mImDepth.at<float>(v, u);用于获取深度值。
• 如果深度值d在大于0且小于6的范围内，则表示该点是有效的动态点。
• 将有效的动态点的像素坐标和相关信息填充到矩阵变量中，即：
• matRefFrame.at<float>(k, 0) = refFrame.mvKeysUn[j].pt.x;：将当前特征点在无畸变像素坐标系下的x坐标存储在matRefFrame中。
• matRefFrame.at<float>(k, 1) = refFrame.mvKeysUn[j].pt.y;：将当前特征点在无畸变像素坐标系下的y坐标存储在matRefFrame中。
• matRefFrame.at<float>(k, 2) = 1.;：给定参考帧特征点在相机坐标系中的z坐标为1。
• matInvDepthRefFrame.at<float>(k, 0) = 1. / d;：将特征点的逆深度值存储在matInvDepthRefFrame中。
• matDepthRefFrame.at<float>(k, 0) = d;：将特征点的深度值存储在matDepthRefFrame中。
• vLabels.at<float>(k, 0) = i;：将当前特征点的标签（即参考帧索引）存储在vLabels中。
• 每次成功填充一个有效动态点时，递增k的值。

这段代码的目的是在每个参考帧中提取出有效的动态点，并将相关的信息存储在相应的矩阵中。这些信息将在后续操作中用于提取和处理动态点的信息。

       matRefFrame = matRefFrame.rowRange(0, k);            matInvDepthRefFrame = matInvDepthRefFrame.rowRange(0, k);            matDepthRefFrame = matDepthRefFrame.rowRange(0, k);            vLabels = vLabels.rowRange(0, k);            cv::Mat vMPRefFrame = K.inv() * matRefFrame.t();            cv::vconcat(vMPRefFrame, matInvDepthRefFrame.t(), vMPRefFrame);            cv::Mat vMPw = refFrame.mTcw.inv() * vMPRefFrame;            cv::Mat _vMPw = cv::Mat(4, vMPw.cols, CV_32F);            cv::Mat _vLabels = cv::Mat(vLabels.rows, 1, CV_32F);            cv::Mat _matRefFrame = cv::Mat(matRefFrame.rows, 3, CV_32F);            cv::Mat _matDepthRefFrame = cv::Mat(matDepthRefFrame.rows, 1, CV_32F);int h(0);            mParallaxThreshold = 30;            for (int j(0); j < k; j++)            {                cv::Mat mp = cv::Mat(3, 1, CV_32F);                mp.at<float>(0, 0) = vMPw.at<float>(0, j) / matInvDepthRefFrame.at<float>(0, j);                mp.at<float>(1, 0) = vMPw.at<float>(1, j) / matInvDepthRefFrame.at<float>(0, j);                mp.at<float>(2, 0) = vMPw.at<float>(2, j) / matInvDepthRefFrame.at<float>(0, j);                cv::Mat tRefFrame = refFrame.mTcw.rowRange(0, 3).col(3);                 cv::Mat tCurrentFrame = currentFrame.mTcw.rowRange(0, 3).col(3);                cv::Mat nMPRefFrame = mp - tRefFrame;                cv::Mat nMPCurrentFrame = mp - tCurrentFrame;                 double dotProduct = nMPRefFrame.dot(nMPCurrentFrame);                double normMPRefFrame = cv::norm(nMPRefFrame, cv::NORM_L2);                double normMPCurrentFrame = cv::norm(nMPCurrentFrame, cv::NORM_L2);                double angle = acos(dotProduct / (normMPRefFrame * normMPCurrentFrame)) * 180 / M_PI;                if (angle < mParallaxThreshold)                {                    _vMPw.at<float>(0, h) = vMPw.at<float>(0, j);                    _vMPw.at<float>(1, h) = vMPw.at<float>(1, j);                    _vMPw.at<float>(2, h) = vMPw.at<float>(2, j);                    _vMPw.at<float>(3, h) = vMPw.at<float>(3, j);                    _vLabels.at<float>(h, 0) = vLabels.at<float>(j, 0);                    _matRefFrame.at<float>(h, 0) = matRefFrame.at<float>(j, 0);                    _matRefFrame.at<float>(h, 1) = matRefFrame.at<float>(j, 1);                    _matRefFrame.at<float>(h, 2) = matRefFrame.at<float>(j, 2);                    _matDepthRefFrame.at<float>(h, 0) = matDepthRefFrame.at<float>(j, 0);                    h++;                }            }

以下是代码的 详细解释  ：

• int h(0); - 初始化整型变量h为0，用作新矩阵的索引。
• mParallaxThreshold = 30; - 设置变量mParallaxThreshold的值为30。此阈值用于筛选特征点的视差角度。
• for (int j(0); j < k; j++) - 对原始特征点进行遍历，k代表特征点的数量。
• 在每次循环中，首先创建一个3×1的浮点型矩阵mp，并根据表达式mp.at<float>(0, 0) = vMPw.at<float>(0, j) / matInvDepthRefFrame.at<float>(0, j);等计算特征点的值。
• cv::Mat tRefFrame = refFrame.mTcw.rowRange(0, 3).col(3); - 从参考帧的转换矩阵中提取平移向量。
• cv::Mat tCurrentFrame = currentFrame.mTcw.rowRange(0, 3).col(3); - 从当前帧的转换矩阵中提取平移向量。
• cv::Mat nMPRefFrame = mp - tRefFrame; - 计算特征点在参考帧坐标系中的归一化坐标。
• cv::Mat nMPCurrentFrame = mp - tCurrentFrame; - 计算特征点在当前帧坐标系中的归一化坐标。
• double dotProduct = nMPRefFrame.dot(nMPCurrentFrame); - 计算两个归一化坐标之间的点积。
• double normMPRefFrame = cv::norm(nMPRefFrame, cv::NORM_L2); - 计算归一化坐标在参考帧坐标系中的模长。
• double normMPCurrentFrame = cv::norm(nMPCurrentFrame, cv::NORM_L2); - 计算归一化坐标在当前帧坐标系中的模长。
• double angle = acos(dotProduct / (normMPRefFrame * normMPCurrentFrame)) * 180 / M_PI; - 计算归一化坐标之间的夹角，并将结果转换为角度。
• if (angle < mParallaxThreshold) - 检查角度是否小于视差阈值。
• 如果角度小于视差阈值，则将特征点和相关的数据存储到新的矩阵中。代码中使用了索引h来指定新矩阵的行号。

综上所述，该部分代码的主要目的是通过计算特征点之间的角度，并根据视差阈值对这些特征点进行筛选。被筛选出的特征点和相关数据将存储在新的矩阵中，供后续处理使用。

            vMPw = _vMPw.colRange(0, h);            vLabels = _vLabels.rowRange(0, h);            matRefFrame = _matRefFrame.rowRange(0, h);            matDepthRefFrame = _matDepthRefFrame.rowRange(0, h);             if (vAllMPw.empty())            {                vAllMPw = vMPw;                vAllMatRefFrame = matRefFrame;                vAllLabels = vLabels;                vAllDepthRefFrame = matDepthRefFrame;            }            else            {                if (!vMPw.empty())                {                    hconcat(vAllMPw, vMPw, vAllMPw);                    vconcat(vAllMatRefFrame, matRefFrame, vAllMatRefFrame);                    vconcat(vAllLabels, vLabels, vAllLabels);                    vconcat(vAllDepthRefFrame, matDepthRefFrame, vAllDepthRefFrame);                }            }

这段代码是根据之前的筛选结果将不同变量的数据合并到一个整体的数据集中。以下是代码的解释：

• vMPw = _vMPw.colRange(0, h); - 将 _vMPw 矩阵的列范围从 0 到 h 提取出来，赋值给 vMPw 变量。这将得到被筛选出来的特征点的位置信息。
• vLabels = _vLabels.rowRange(0, h); - 将 _vLabels 矩阵的行范围从 0 到 h 提取出来，赋值给 vLabels 变量。这将得到被筛选出来的特征点的标签信息。
• matRefFrame = _matRefFrame.rowRange(0, h); - 将 _matRefFrame 矩阵的行范围从 0 到 h 提取出来，赋值给 matRefFrame 变量。这将得到被筛选出来的特征点在参考帧坐标系中的位置信息。
• matDepthRefFrame = _matDepthRefFrame.rowRange(0, h); - 将 _matDepthRefFrame 矩阵的行范围从 0 到 h 提取出来，赋值给 matDepthRefFrame 变量。这将得到被筛选出来的特征点的参考帧深度信息。
• 接下来是一个条件语句的判断。如果 vAllMPw 是空的（即还没有初始化），则将之前筛选得到的数据赋值给对应的整体数据变量 vAllMPw、vAllMatRefFrame、vAllLabels 和 vAllDepthRefFrame。
• 如果 vAllMPw 不为空，且当前的 vMPw 也不为空，则使用 hconcat() 和 vconcat() 函数将当前的筛选结果与之前的整体数据进行水平和垂直拼接，分别存储到 vAllMPw、vAllMatRefFrame、vAllLabels 和 vAllDepthRefFrame 中。

综上所述，这段代码的作用是将之前筛选出来的特征点和相关的数据合并到一个整体的数据集中，以便进行后续处理或分析。

        cv::Mat vLabels = vAllLabels;         if (!vAllMPw.empty())        {            cv::Mat vMPCurrentFrame = currentFrame.mTcw * vAllMPw;             // Divide by last column            for (int i(0); i < vMPCurrentFrame.cols; i++)            {                vMPCurrentFrame.at<float>(0, i) /= vMPCurrentFrame.at<float>(3, i);                vMPCurrentFrame.at<float>(1, i) /= vMPCurrentFrame.at<float>(3, i);                vMPCurrentFrame.at<float>(2, i) /= vMPCurrentFrame.at<float>(3, i);                vMPCurrentFrame.at<float>(3, i) /= vMPCurrentFrame.at<float>(3, i);            }            cv::Mat matProjDepth = vMPCurrentFrame.row(2);             cv::Mat _vMPCurrentFrame = cv::Mat(vMPCurrentFrame.size(), CV_32F);            cv::Mat _vAllMatRefFrame = cv::Mat(vAllMatRefFrame.size(), CV_32F);            cv::Mat _vLabels = cv::Mat(vLabels.size(), CV_32F);            cv::Mat __vAllDepthRefFrame = cv::Mat(vAllDepthRefFrame.size(), CV_32F);            int h(0);            cv::Mat __matProjDepth = cv::Mat(matProjDepth.size(), CV_32F);

这段代码的作用如下：

• cv::Mat vLabels = vAllLabels; - 创建一个名为 vLabels 的新变量，并将 vAllLabels 的值复制给它。这实际上是创建了一个命名别名，两个变量引用的是同一个数据，对 vLabels 的修改也将同时影响到 vAllLabels。
• if (!vAllMPw.empty()) - 这是一个条件语句，检查 vAllMPw 是否不为空。如果不为空，执行下面的代码块。
• cv::Mat vMPCurrentFrame = currentFrame.mTcw * vAllMPw; - 计算当前帧的变换矩阵 currentFrame.mTcw 与 vAllMPw 的矩阵相乘，得到变换后的特征点位置矩阵 vMPCurrentFrame。
• for (int i(0); i < vMPCurrentFrame.cols; i++) - 这是一个循环语句，遍历 vMPCurrentFrame 矩阵的列数。
• 循环内部的代码用于对 vMPCurrentFrame 进行归一化处理，即将每个特征点的坐标除以其最后一列的值，实现将其第四维的值统一为1。
• cv::Mat matProjDepth = vMPCurrentFrame.row(2); - 从 vMPCurrentFrame 矩阵的第3行（索引为2）提取出来，赋值给 matProjDepth 变量。这将得到变换后的特征点的深度信息。
• 接下来创建了一系列新的 cv::Mat 变量，包括 _vMPCurrentFrame、 _vAllMatRefFrame、_vLabels、 __vAllDepthRefFrame 和 __matProjDepth。这些变量的大小与相应的矩阵相同，并且数据类型为 CV_32F，即单精度浮点型。

总体而言，这段代码主要完成了以下功能：根据保存的变换矩阵将之前筛选出来的特征点位置矩阵 vAllMPw 进行变换，然后进行归一化处理，并提取 深度  信息。同时，创建了一些新的 cv::Mat 变量，用于存储相应的数据。

for (int i(0); i < matProjDepth.cols; i++)            {                if (matProjDepth.at<float>(0, i) < 7)                {                    __matProjDepth.at<float>(0, h) = matProjDepth.at<float>(0, i);                     _vMPCurrentFrame.at<float>(0, h) = vMPCurrentFrame.at<float>(0, i);                    _vMPCurrentFrame.at<float>(1, h) = vMPCurrentFrame.at<float>(1, i);                    _vMPCurrentFrame.at<float>(2, h) = vMPCurrentFrame.at<float>(2, i);                    _vMPCurrentFrame.at<float>(3, h) = vMPCurrentFrame.at<float>(3, i);                     _vAllMatRefFrame.at<float>(h, 0) = vAllMatRefFrame.at<float>(i, 0);                    _vAllMatRefFrame.at<float>(h, 1) = vAllMatRefFrame.at<float>(i, 1);                    _vAllMatRefFrame.at<float>(h, 2) = vAllMatRefFrame.at<float>(i, 2);                     _vLabels.at<float>(h, 0) = vLabels.at<float>(i, 0);                     __vAllDepthRefFrame.at<float>(h, 0) = vAllDepthRefFrame.at<float>(i, 0);                     h++;                }            }

这段代码是一个循环，遍历了matProjDepth矩阵的每一列。

在循环的每次迭代中，首先检查matProjDepth矩阵的当前列（index为i）的值是否小于7。如果满足这个条件，那么执行以下操作：

• 将matProjDepth矩阵的当前列的值复制到__matProjDepth矩阵的第h列中。
• 将vMPCurrentFrame矩阵的当前列的前三个元素分别复制到_vMPCurrentFrame矩阵的第h列的前三个位置。
• 将vMPCurrentFrame矩阵的当前列的第四个元素（齐次坐标的第四维）复制到_vMPCurrentFrame矩阵的第h列的第四个位置。
• 将vAllMatRefFrame矩阵的当前行的三个元素分别复制到_vAllMatRefFrame矩阵的第h行的三个位置。
• 将vLabels矩阵的当前行的元素复制到_vLabels矩阵的第h行的第一个位置。
• 将vAllDepthRefFrame矩阵的当前行的元素复制到__vAllDepthRefFrame矩阵的第h行的第一个位置。
• 增加h的值，以便在下次迭代中将数据写入到下一个可用的位置。

总体来说，这段代码的目的是根据matProjDepth矩阵的值来筛选和复制相关的数据到不同的矩阵和向量中。具体的选择和复制依赖于满足条件的元素的位置。

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