7、 进行深度图像的区域生长处理,将动态区域生成一个掩码
cv::Mat Geometry::DepthRegionGrowing(const vector<DynKeyPoint> &vDynPoints, const cv::Mat &imDepth)
{
cv::Mat maskG = cv::Mat::zeros(480, 640, CV_32F);
if (!vDynPoints.empty())
{
mSegThreshold = 0.20;
for (size_t i(0); i < vDynPoints.size(); i++)
{
int xSeed = vDynPoints[i].mPoint.x;
int ySeed = vDynPoints[i].mPoint.y;
const float d = imDepth.at<float>(ySeed, xSeed);
if (maskG.at<float>(ySeed, xSeed) != 1. && d > 0)
{
cv::Mat J = RegionGrowing(imDepth, xSeed, ySeed, mSegThreshold);
maskG = maskG | J;
}
}
int dilation_size = 15;
cv::Mat kernel = getStructuringElement(cv::MORPH_ELLIPSE,
cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1),
cv::Point(dilation_size, dilation_size));
maskG.cv::Mat::convertTo(maskG, CV_8U);
cv::dilate(maskG, maskG, kernel);
}
else
{
maskG.cv::Mat::convertTo(maskG, CV_8U);
}
cv::Mat _maskG = cv::Mat::ones(480, 640, CV_8U);
maskG = _maskG - maskG;
return maskG;
}
该函数的目的是进行深度图像的区域生长处理,以根据给定的动态关键点(vDynPoints)和深度图像(imDepth)生成一个掩码(maskG),表示动态物体在图像中的区域。
函数的具体步骤如下:
- 首先创建一个与输入图像大小相同的空白的浮点型掩码(maskG)。
- 如果给定的动态关键点不为空(vDynPoints不为空),则进行以下操作:
- 设置 mSegThreshold 的值为 0.20,表示区域生长的阈值。
- 遍历所有的动态关键点。
- 对于每个动态关键点,获取其像素坐标(xSeed 和 ySeed)以及对应的深度值(d)。
- 如果掩码中对应位置的值不等于 1 且深度值大于 0,则执行区域生长(RegionGrowing)操作,并将结果与掩码进行逻辑或运算(maskG = maskG | J)。
- 区域生长操作的具体过程可能在其他函数(RegionGrowing)中实现。
- 创建一个指定大小和形状的结构元素(kernel)用于膨胀操作。
- 将掩码转换为 8 位无符号整数类型(CV_8U)。
- 对掩码进行膨胀操作,使用创建的结构元素(kernel)作为参数。
- 如果给定的动态关键点为空,则将掩码转换为 8 位无符号整数类型(CV_8U)。
- 创建一个与输入图像大小相同的全为 1 的掩码(_maskG)。
- 通过计算差集操作(_maskG - maskG),得到动态物体的区域掩码。
- 最后,返回生成的掩码(maskG)。
8、目的是将两个遮罩(mask)图像进行合并
void Geometry::CombineMasks(const ORB_SLAM2::Frame ¤tFrame, cv::Mat &mask)
{
cv::Mat _maskL = cv::Mat::ones(currentFrame.mImMask.size(), currentFrame.mImMask.type());
_maskL = _maskL - currentFrame.mImMask;
cv::Mat _maskG = cv::Mat::ones(mask.size(), mask.type());
_maskG = _maskG - mask;
cv::Mat _mask = _maskL | _maskG;
cv::Mat __mask = cv::Mat::ones(_mask.size(), _mask.type());
__mask = __mask - _mask;
mask = __mask;
}
以下是代码的解释:
- 首先,创建一个与
currentFrame.mImMask
图像大小相同的cv::Mat
对象_maskL
,并将其初始化为全1(白色)的图像。 - 然后,通过减去
currentFrame.mImMask
图像,将该图像中的前景区域(非零像素)置为0,背景区域(零像素)保持为1。结果存储在_maskL
中。 - 接下来,创建一个与
mask
图像大小相同的cv::Mat
对象_maskG
,并将其初始化为全1的图像。 - 同样地,通过减去
mask
图像,将该图像中的前景区域(非零像素)置为0,背景区域(零像素)保持为1。结果存储在_maskG
中。 - 接着,使用逻辑或(bitwise OR)操作符
|
将_maskL
和_maskG
图像合并为一个新的_mask
图像。这意味着合并后的图像中,只要有一个图像在相同位置上的像素为前景(非零值),那么合并后的图像该位置上的像素值也为前景(非零值)。 - 创建一个与
_mask
图像大小相同的cv::Mat
对象__mask
,并将其初始化为全1的图像。 - 同样地,通过减去
_mask
图像,将该图像中的前景区域(非零像素)置为0,背景区域(零像素)保持为1。结果存储在__mask
中。 - 最后,将
__mask
赋值给传入函数的mask
参数,以便将合并后的遮罩图像返回给函数调用处。
总之,该函数的作用是将两个遮罩图像合并为一个遮罩图像,并将合并后的结果保存在 mask
中。
9、该函数计算两个矩形框之间的重叠区域的面积。
float Area(float x1, float x2, float y1, float y2)
{
float xc1 = max(x1 - 0.5, x2 - 0.5);
float xc2 = min(x1 + 0.5, x2 + 0.5);
float yc1 = max(y1 - 0.5, y2 - 0.5);
float yc2 = min(y1 + 0.5, y2 + 0.5);
return (xc2 - xc1) * (yc2 - yc1);
}
函数的参数包括两个矩形框的坐标:x1
、x2
、y1
和 y2
。其中 (x1, y1)
表示第一个矩形框的左上角坐标,而 (x2, y2)
表示第二个矩形框的左上角坐标。
接下来,函数会根据矩形框的坐标计算出两个矩形框在 x 和 y 方向上的重叠区域的范围。
xc1
表示两个矩形框在 x 方向上的最大重叠起始点,即两个矩形框 x 坐标的较大值减去 0.5。xc2
表示两个矩形框在 x 方向上的最小重叠结束点,即两个矩形框 x 坐标的较小值加上 0.5。yc1
表示两个矩形框在 y 方向上的最大重叠起始点,即两个矩形框 y 坐标的较大值减去 0.5。yc2
表示两个矩形框在 y 方向上的最小重叠结束点,即两个矩形框 y 坐标的较小值加上 0.5。
最后,函数返回两个矩形框重叠区域的面积,即将重叠区域的宽度 (xc2 - xc1)
乘以高度 (yc2 - yc1)
。
总之,这段代码实现了一个计算两个矩形框重叠区域面积的函数。
10、
void Geometry::FillRGBD(const ORB_SLAM2::Frame ¤tFrame, cv::Mat &mask, cv::Mat &imGray, cv::Mat &imDepth)
{
cv::Mat imGrayAccumulator = imGray.mul(mask);
imGrayAccumulator.convertTo(imGrayAccumulator, CV_32F);
cv::Mat imCounter;
mask.convertTo(imCounter, CV_32F);
cv::Mat imDepthAccumulator = imDepth.mul(imCounter);
imDepthAccumulator.convertTo(imDepthAccumulator, CV_32F);
cv::Mat imMinDepth = cv::Mat::zeros(imDepth.size(), CV_32F) + 100.0;
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;
它用于填充RGB-D数据。
函数接受了四个参数:
currentFrame
是一个引用传递的ORB_SLAM2::Frame
类型的对象,表示当前帧的数据。mask
是一个引用传递的cv::Mat
类型的图像掩码,用于指定哪些像素需要进行处理。imGray
是一个引用传递的cv::Mat
类型的灰度图像,表示图像的灰度值。imDepth
是一个引用传递的cv::Mat
类型的深度图像,表示图像的深度值。
下面是函数的具体实现:
- 创建一个新的
cv::Mat
对象imGrayAccumulator
,它是根据灰度图像imGray
和掩码mask
的元素逐元素相乘得到的。通过调用mul()
函数实现相乘操作。然后将结果转换为CV_32F
类型。 - 创建一个新的
cv::Mat
对象imCounter
,它是将掩码mask
转换为CV_32F
类型的结果。 - 创建一个新的
cv::Mat
对象imDepthAccumulator
,它是根据深度图像imDepth
和imCounter
的元素逐元素相乘得到的结果。同样,将结果转换为CV_32F
类型。 - 创建一个新的
cv::Mat
对象imMinDepth
,其大小与imDepth
相同,并用值100.0填充。
接下来,代码创建一个3x3
的CV_32F
类型的矩阵K
,表示相机的内参矩阵。通过将currentFrame
对象的成员变量赋值给K
的对应元素实现初始化。
currentFrame.fx
表示相机焦距在x方向上的分量;currentFrame.fy
表示相机焦距在y方向上的分量;currentFrame.cx
表示相机光心在x方向上的分量;currentFrame.cy
表示相机光心在y方向上的分量。
总之,这段代码实现了一个函数,用于根据RGB-D数据填充图像数据和计算相机内参矩阵。
for (int i(0); i < mDB.mNumElem; i++)
{
ORB_SLAM2::Frame refFrame = mDB.mvDataBase[i];
cv::Mat vPixels(640 * 480, 2, CV_32F);
cv::Mat mDepth(640 * 480, 1, CV_32F);
int n(0);
for (int j(0); j < 640 * 480; j++)
{
int x = (int)vAllPixels.at<float>(j, 0);
int y = (int)vAllPixels.at<float>(j, 1);
if ((int)refFrame.mImMask.at<uchar>(y, x) == 1)
{
const float d = refFrame.mImDepth.at<float>(y, x);
if (d > 0 && d < 7)
{
vPixels.at<float>(n, 0) = vAllPixels.at<float>(j, 0);
vPixels.at<float>(n, 1) = vAllPixels.at<float>(j, 1);
mDepth.at<float>(n, 0) = 1. / d;
n++;
}
}
}
vPixels = vPixels.rowRange(0, n);
mDepth = mDepth.rowRange(0, n);
hconcat(vPixels, cv::Mat::ones(n, 1, CV_32F), vPixels);
cv::Mat vMPRefFrame = K.inv() * vPixels.t();
vconcat(vMPRefFrame, mDepth.t(), vMPRefFrame);
这段代码是一个循环,对于一个数据库中的每个元素,执行以下操作:
-
从数据库中获取一个名为
refFrame
的ORB_SLAM2::Frame
对象,作为参考帧。 -
创建一个大小为
640x480
的CV_32F
类型的矩阵vPixels
,用于存储像素坐标。 -
创建一个大小为
640x480
的CV_32F
类型的矩阵mDepth
,用于存储深度数据。 -
初始化一个整型变量
n
为0,用于计数有效的像素。 -
遍历
640x480
个像素,对于每个像素,执行以下操作:- 获取该像素的x坐标和y坐标,存储在变量
x
和y
中。 - 通过检查参考帧的图像掩码(
refFrame.mImMask
)在坐标(y, x)
处的像素值是否为1,来判断该像素是否有效。 - 如果是有效像素,则获取参考帧的深度数据(
refFrame.mImDepth
)在坐标(y, x)
处的值,存储在变量d
中。 - 如果深度值
d
在大于0且小于7之间,则将该像素的坐标和深度值存储在vPixels
和mDepth
矩阵中,并将计数器n
递增。
- 获取该像素的x坐标和y坐标,存储在变量
-
利用计数器
n
,裁剪vPixels
和mDepth
矩阵,将它们的行范围缩减为有效像素的数量。 -
在
vPixels
矩阵的最后一列添加一列值为1的元素,用于扩展为齐次坐标。 -
创建一个矩阵
vMPRefFrame
,通过将矩阵vPixels
转置后与相机内参矩阵K
的逆相乘得到。这样可以将像素坐标转换为相机坐标。 -
将深度数据矩阵
mDepth
转置后与vMPRefFrame
垂直拼接,构成一个包含相机坐标和深度数据的矩阵vMPRefFrame
。
总结一下,这段代码的作用是根据参考帧的图像掩码和深度数据,提取有效的像素坐标和对应的深度值,并将其转换为相机坐标系下的齐次坐标和深度数据。
cv::Mat vMPw = refFrame.mTcw.inv() * vMPRefFrame;
cv::Mat vMPCurrentFrame = currentFrame.mTcw * vMPw;
// Divide by last column
for (int j(0); j < vMPCurrentFrame.cols; j++)
{
vMPCurrentFrame.at<float>(0, j) /= vMPCurrentFrame.at<float>(3, j);
vMPCurrentFrame.at<float>(1, j) /= vMPCurrentFrame.at<float>(3, j);
vMPCurrentFrame.at<float>(2, j) /= vMPCurrentFrame.at<float>(3, j);
vMPCurrentFrame.at<float>(3, j) /= vMPCurrentFrame.at<float>(3, j);
}
cv::Mat matProjDepth = vMPCurrentFrame.row(2);
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 vProjPixels(matCurrentFrame.cols, 2, CV_32F);
cv::Mat _matProjDepth(matCurrentFrame.cols, 1, CV_32F);
cv::Mat _vPixels(matCurrentFrame.cols, 2, CV_32F);
这段代码执行了一系列矩阵计算和转换操作,具体含义如下:
-
cv::Mat vMPw = refFrame.mTcw.inv() * vMPRefFrame;
对参考帧的位姿矩阵(refFrame.mTcw
)的逆与之前计算得到的相机坐标矩阵(vMPRefFrame
)相乘,得到相机坐标系下的三维点坐标矩阵vMPw
。 -
cv::Mat vMPCurrentFrame = currentFrame.mTcw * vMPw;
根据当前帧的位姿矩阵(currentFrame.mTcw
)和前面计算得到的vMPw
矩阵,计算当前帧中对应的相机坐标系下的三维点坐标矩阵,存储在vMPCurrentFrame
中。 -
// Divide by last column
对vMPCurrentFrame
进行归一化处理,将每一列除以该列的最后一个元素,实现齐次坐标的归一化。 -
cv::Mat matProjDepth = vMPCurrentFrame.row(2);
从vMPCurrentFrame
中提取第3行(深度信息),得到一个矩阵matProjDepth
,表示当前帧中相机坐标系下的点的深度。 -
cv::Mat aux; cv::hconcat(cv::Mat::eye(3, 3, CV_32F), cv::Mat::zeros(3, 1, CV_32F), aux);
创建一个辅助矩阵aux
,其中前半部分为一个3x3的单位矩阵,后半部分为一个3x1的零向量。 -
cv::Mat matCurrentFrame = K * aux * vMPCurrentFrame;
利用相机内参矩阵K
、辅助矩阵aux
和vMPCurrentFrame
,计算当前帧中投影到图像平面上的像素坐标矩阵matCurrentFrame
。 -
cv::Mat vProjPixels(matCurrentFrame.cols, 2, CV_32F);
创建一个大小为matCurrentFrame.cols x 2
的矩阵vProjPixels
,用于存储当前帧中的投影像素坐标。 -
cv::Mat _matProjDepth(matCurrentFrame.cols, 1, CV_32F);
创建一个大小为matCurrentFrame.cols x 1
的矩阵_matProjDepth
,用于存储当前帧中像素坐标对应的深度信息。 -
cv::Mat _vPixels(matCurrentFrame.cols, 2, CV_32F);
创建一个大小为matCurrentFrame.cols x 2
的矩阵_vPixels
,用于存储当前帧中的像素坐标。
这段代码的目的是将参考帧中的相机坐标转换到当前帧中,并计算当前帧中像素的投影坐标、深度信息等,以进行后续的处理和计算
int p(0);
for (int j(0); j < matCurrentFrame.cols; j++)
{
float x = matCurrentFrame.at<float>(0, j) / matCurrentFrame.at<float>(2, j);
float y = matCurrentFrame.at<float>(1, j) / matCurrentFrame.at<float>(2, j);
bool inFrame = (x > 1 && x < (currentFrame.mImDepth.cols - 1) && y > 1 && y < (currentFrame.mImDepth.rows - 1));
if (inFrame && (mask.at<uchar>(y, x) == 0))
{
vProjPixels.at<float>(p, 0) = x;
vProjPixels.at<float>(p, 1) = y;
_matProjDepth.at<float>(p, 0) = matProjDepth.at<float>(0, j);
_vPixels.at<float>(p, 0) = vPixels.at<float>(j, 0);
_vPixels.at<float>(p, 1) = vPixels.at<float>(j, 1);
p++;
}
}
vProjPixels = vProjPixels.rowRange(0, p);
matProjDepth = _matProjDepth.rowRange(0, p);
vPixels = _vPixels.rowRange(0, p);
这段代码执行了以下操作:
-
int p(0);
初始化变量p
为0,用于记录满足条件的点的数量。 -
循环遍历
matCurrentFrame
的每一列,其中j
表示列索引。 -
在循环中,首先计算当前列对应的像素坐标
(x, y)
,通过将matCurrentFrame
中的第一行元素除以第三行元素得到x
,将第二行元素除以第三行元素得到y
。 -
判断计算得到的像素坐标
(x, y)
是否在帧的有效范围内,即x
在 (1, currentFrame.mImDepth.cols - 1) 之间且y
在 (1, currentFrame.mImDepth.rows - 1) 之间,同时判断mask
中对应像素位置(y, x)
的值是否为0。 -
如果上述条件都成立,则说明该点符合要求,将其保存到结果矩阵中。
-
将满足条件的点的像素坐标保存在矩阵
vProjPixels
中的第p
行的第一列和第二列,将对应点的深度信息保存在_matProjDepth
中的第p
行的第一列,将输入参数vPixels
中对应点的坐标保存在_vPixels
中的第p
行的第一列和第二列。 -
在每次满足条件时,将变量
p
的值增加1,以表示满足条件点的数量。 -
循环结束后,得到满足条件的点的总数。
-
根据得到的满足条件的点的数量
p
,截取矩阵vProjPixels
、matProjDepth
和vPixels
的前p
行作为最终的结果。
总的来说,这段代码的目的是筛选出满足条件的像素点,并将这些点的信息保存到对应的矩阵中。