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

Geometry. cc ：多视图几何检测动态物体

1、定义Geometry类并遍历图像中所有像素

Geometry::Geometry(){    vAllPixels = cv::Mat(640*480,2,CV_32F);    int m(0);    for (int i(0); i < 640; i++){        for (int j(0); j < 480; j++){            vAllPixels.at<float>(m,0) = i;            vAllPixels.at<float>(m,1) = j;            m++;        }    }}

以下是代码的解释：

• vAllPixels = cv::Mat(640*480,2,CV_32F); - 创建一个名为vAllPixels的cv::Mat对象，用于存储像素坐标。该对象是一个640*480的矩阵，每个像素坐标有两个浮点数表示，数据类型为CV_32F（单精度浮点数）。
• int m(0); - 声明并初始化一个整数变量m，用于追踪在矩阵中插入像素坐标的位置。
• for (int i(0); i < 640; i++) - 循环遍历i从0到639之间的整数值。
• for (int j(0); j < 480; j++) - 在每个i值的循环中，内嵌一个循环遍历j从0到479之间的整数值。
• vAllPixels.at<float>(m,0) = i; - 在矩阵vAllPixels的第m行、第0列的位置处，将像素的x坐标值设置为i。
• vAllPixels.at<float>(m,1) = j; - 在矩阵vAllPixels的第m行、第1列的位置处，将像素的y坐标值设置为j。
• m++; - 增加变量m的值，以移动到下一个矩阵位置，以便存储下一个像素坐标。

该 构造函数  的目的是创建一个表示所有像素坐标的矩阵。通过嵌套的循环，它将从左上角开始遍历所有像素，逐个将每个像素的x和y坐标值存储到矩阵vAllPixels中，以便进一步的几何计算和处理。

2、生成一个动态区域的掩膜mask

void Geometry::GeometricModelCorrection(const ORB_SLAM2::Frame ¤tFrame, cv::Mat &imDepth, cv::Mat &mask){    if(currentFrame.mTcw.empty()){        std::cout << "Geometry not working." << std::endl;    }    else if (mDB.mNumElem >= ELEM_INITIAL_MAP){        vector<ORB_SLAM2::Frame> vRefFrames = GetRefFrames(currentFrame);        vector<DynKeyPoint> vDynPoints = ExtractDynPoints(vRefFrames,currentFrame);        mask = DepthRegionGrowing(vDynPoints,imDepth);        CombineMasks(currentFrame,mask);    }}

以下是代码的解释：

• void Geometry::GeometricModelCorrection(const ORB_SLAM2::Frame ¤tFrame, cv::Mat &imDepth, cv::Mat &mask) - 这是Geometry类的成员函数，它接受currentFrame、imDepth和mask作为输入参数。
• if (currentFrame.mTcw.empty()) - 检查currentFrame.mTcw是否为空。currentFrame.mTcw是一个用于存储相机位姿的变换矩阵。如果为空，表示相机位姿未初始化或未计算，输出"Geometry not working."。
• else if (mDB.mNumElem >= ELEM_INITIAL_MAP) - 如果mDB.mNumElem（mDB是Geometry类中的一个成员变量，表示某种数据库）的值大于等于ELEM_INITIAL_MAP（一个常量），执行下面的代码块。
• vector<ORB_SLAM2::Frame> vRefFrames = GetRefFrames(currentFrame); - 调用GetRefFrames函数，该函数接受currentFrame作为输入，并返回一个存储currentFrame相关参考帧的向量vRefFrames。
• vector<DynKeyPoint> vDynPoints = ExtractDynPoints(vRefFrames, currentFrame); - 调用ExtractDynPoints函数，该函数接受vRefFrames和currentFrame作为输入，并返回一个存储动态关键点的向量vDynPoints。
• mask = DepthRegionGrowing(vDynPoints, imDepth); - 调用DepthRegionGrowing函数，该函数接受vDynPoints和imDepth作为输入，并返回一个用于表示动态区域的掩膜mask。
• CombineMasks(currentFrame, mask); - 调用CombineMasks函数，该函数接受currentFrame和mask作为输入，根据currentFrame的信息，将动态区域的掩膜mask与其他掩膜进行组合。

这段代码的目的是对几何模型进行修正。它会检查相机位姿是否可用，并根据已有的场景 地图  数据进行动态物体检测和几何模型修正。通过处理参考帧、提取动态关键点、进行深度区域生长和掩膜的组合，该函数生成一个动态区域的掩膜mask，用于后续的 图像处理  和分析。

3、根据各种 图像修复  掩膜，使得在图像中合理显示

void Geometry::InpaintFrames(const ORB_SLAM2::Frame ¤tFrame, cv::Mat &imGray, cv::Mat &imDepth, cv::Mat &imRGB, cv::Mat &mask){    FillRGBD(currentFrame,mask,imGray,imDepth,imRGB);}

以下是代码的解释：

• void Geometry::InpaintFrames(const ORB_SLAM2::Frame ¤tFrame, cv::Mat &imGray, cv::Mat &imDepth, cv::Mat &imRGB, cv::Mat &mask) - 这是Geometry类的成员函数，它接受currentFrame、imGray、imDepth、imRGB和mask作为输入参数。
• FillRGBD(currentFrame,mask,imGray,imDepth,imRGB); - 调用FillRGBD函数，该函数接受currentFrame、mask、imGray、imDepth和imRGB作为输入参数。
• FillRGBD函数的作用是根据当前帧的信息和掩膜，对灰度图像imGray、深度图像imDepth和彩色图像imRGB进行修复（inpainting）。修复的过程包括根据掩膜选择需要修复的像素，并使用适当的算法填补缺失的像素值，使得修复后的图像在动态区域中显示合理的内容。

总的来说，这段代码的目的是将当前帧的灰度图像、 深度  图像和彩色图像，根据动态区域的掩膜进行修复（inpaint），使得动态区域在图像中显示合理的内容。修复后的图像可以在后续的分析和处理中使用。

4、根据当前帧是否为关键帧，更新几何模型的数据库

void Geometry::GeometricModelUpdateDB(const ORB_SLAM2::Frame ¤tFrame){    if (currentFrame.mIsKeyFrame)    {        mDB.InsertFrame2DB(currentFrame);    }}

以下是代码的解释：

• void Geometry::GeometricModelUpdateDB(const ORB_SLAM2::Frame ¤tFrame) - 这是Geometry类的成员函数，它接受currentFrame作为输入参数。
• if (currentFrame.mIsKeyFrame) - 检查currentFrame是否为关键帧。currentFrame.mIsKeyFrame是一个布尔类型的成员变量，用于指示当前帧是否为关键帧。如果是关键帧，执行下面的代码块。
• mDB.InsertFrame2DB(currentFrame) - 调用mDB（Geometry类中的一个成员变量，可能表示几何模型的数据库）的InsertFrame2DB函数，将currentFrame插入到数据库中。

这段代码的目的是根据当前帧是否为关键帧，更新几何模型的数据库。如果当前帧是关键帧，将该帧插入到数据库中，以便后续的几何模型分析和处理。这样可以保持数据库与实际场景的一致性，并支持几何模型的更新和维护。

5、基于当前帧与数据库中每个元素的旋转和平移距离，选择一组参考帧。它通过计算欧氏距离来评估与当前帧最相似的帧，并根据距离值进行排序和归一化，选择一组最相关的参考帧。

vector<ORB_SLAM2::Frame> Geometry::GetRefFrames(const ORB_SLAM2::Frame ¤tFrame){     cv::Mat rot1 = currentFrame.mTcw.rowRange(0,3).colRange(0,3);    cv::Mat eul1 = rotm2euler(rot1);    cv::Mat trans1 = currentFrame.mTcw.rowRange(0,3).col(3);    cv::Mat vDist;    cv::Mat vRot;     for (int i(0); i < mDB.mNumElem; i++){        cv::Mat rot2 = mDB.mvDataBase[i].mTcw.rowRange(0,3).colRange(0,3);        cv::Mat eul2 = rotm2euler(rot2);        double distRot = cv::norm(eul2,eul1,cv::NORM_L2);        vRot.push_back(distRot);         cv::Mat trans2 = mDB.mvDataBase[i].mTcw.rowRange(0,3).col(3);        double dist = cv::norm(trans2,trans1,cv::NORM_L2);        vDist.push_back(dist);    }     double minvDist, maxvDist;    cv::minMaxLoc(vDist, &minvDist, &maxvDist);    vDist /= maxvDist;     double minvRot, maxvRot;    cv::minMaxLoc(vRot, &minvRot, &maxvRot);    vRot /= maxvRot;     vDist = 0.7*vDist + 0.3*vRot;    cv::Mat vIndex;    cv::sortIdx(vDist,vIndex,CV_SORT_EVERY_COLUMN + CV_SORT_DESCENDING);     mnRefFrames = std::min(MAX_REF_FRAMES,vDist.rows);     vector<ORB_SLAM2::Frame> vRefFrames;     for (int i(0); i < mnRefFrames; i++)    {        int ind = vIndex.at<int>(0,i);        vRefFrames.push_back(mDB.mvDataBase[ind]);    }     return(vRefFrames);}

以下是代码的解释：

• vector<ORB_SLAM2::Frame> Geometry::GetRefFrames(const ORB_SLAM2::Frame ¤tFrame) - 这是Geometry类的成员函数，它接受currentFrame作为输入参数，并返回一个vector类型的关键帧序列。
• cv::Mat rot1 = currentFrame.mTcw.rowRange(0,3).colRange(0,3); - 从currentFrame中提取旋转矩阵的子矩阵，表示当前帧的旋转部分。
• cv::Mat eul1 = rotm2euler(rot1); - 将旋转矩阵转换为欧拉角表示，得到当前帧的欧拉角。
• cv::Mat trans1 = currentFrame.mTcw.rowRange(0,3).col(3); - 从currentFrame中提取平移向量，表示当前帧的平移部分。
• for (int i(0); i < mDB.mNumElem; i++) - 对mDB中的数据库中的每个元素进行循环迭代。
• cv::Mat rot2 = mDB.mvDataBase[i].mTcw.rowRange(0,3).colRange(0,3); - 提取数据库中第i个元素的旋转矩阵子矩阵。
• cv::Mat eul2 = rotm2euler(rot2); - 将数据库中第i个元素的旋转矩阵转换为欧拉角。
• double distRot = cv::norm(eul2,eul1,cv::NORM_L2); - 计算当前帧的欧拉角与数据库中第i个元素的欧拉角之间的欧氏距离。
• vRot.push_back(distRot); - 将距离值加入vRot向量，用于后续的处理。
• cv::Mat trans2 = mDB.mvDataBase[i].mTcw.rowRange(0,3).col(3); - 提取数据库中第i个元素的平移向量。
• double dist = cv::norm(trans2,trans1,cv::NORM_L2); - 计算当前帧的平移向量与数据库中第i个元素的平移向量之间的欧氏距离。
• vDist.push_back(dist); - 将距离值加入vDist向量，用于后续处理。
• cv::minMaxLoc(vDist, &minvDist, &maxvDist); - 找出vDist向量中的最小值和最大值。
• vDist /= maxvDist; - 将vDist向量中的值进行归一化，除以最大值。
• cv::minMaxLoc(vRot, &minvRot, &maxvRot); - 找出vRot向量中的最小值和最大值。
• vRot /= maxvRot; - 将vRot向量中的值进行归一化，除以最大值。
• vDist = 0.7*vDist + 0.3*vRot; - 将vDist向量与vRot向量按照权重线性相加，生成最终的距离向量。
• cv::Mat vIndex; - 创建一个矩阵用于存储排序后的距离向量的索引。
• cv::sortIdx(vDist,vIndex,CV_SORT_EVERY_COLUMN + CV_SORT_DESCENDING); - 对距离向量进行降序排序，并将排序后的索引保存在vIndex中。
• mnRefFrames = std::min(MAX_REF_FRAMES,vDist.rows); - 根据最大参考帧数和距离向量的行数确定需要选择的参考帧数。
• vector<ORB_SLAM2::Frame> vRefFrames; - 创建一个用于存储最终选择的参考帧的向量。
• for (int i(0); i < mnRefFrames; i++) - 对每个需要选择的参考帧进行循环迭代。
• int ind = vIndex.at<int>(0,i); - 获取排序后的索引中第i个位置的值，表示选择的参考帧在数据库中的索引。
• vRefFrames.push_back(mDB.mvDataBase[ind]); - 将数据库中对应索引的帧添加到最终的参考帧向量中。
• return(vRefFrames); - 返回最终选择的参考帧向量。

这段代码的目的是基于当前帧与数据库中每个元素的旋转和平移距离，选择一组参考帧。它通过计算欧氏距离来评估与当前帧最相似的帧，并根据距离值进行排序和归一化，选择一组最相关的参考帧。

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