Dyna-SLAM代码解读:Geometry.cc(五)

for (int j(0); j < p; j++)
            {

                int _x = (int)vPixels.at<float>(j, 0);
                int _y = (int)vPixels.at<float>(j, 1);
                float x = vProjPixels.at<float>(j, 0); // x of *
                float y = vProjPixels.at<float>(j, 1); // y of *
                /*
                    -----------
                    | A  | B  |
                    ----*------ y
                    | C  | D  |
                    -----------
                         x
                */
                float x_a = floor(x);
                float y_a = floor(y);
                float x_b = ceil(x);
                float y_b = floor(y);
                float x_c = floor(x);
                float y_c = ceil(y);
                float x_d = ceil(x);
                float y_d = ceil(y);

                float weight = 0;

定义一些权值

if (IsInImage(x_a, y_a, imGrayAccumulator))
                {
                    if (abs(imMinDepth.at<float>(y_a, x_a) - matProjDepth.at<float>(j, 0)) < MIN_DEPTH_THRESHOLD)
                    {
                        weight = Area(x, x_a, y, y_a);
                        imCounter.at<float>(int(y_a), int(x_a)) += weight;
                        imGrayAccumulator.at<float>(int(y_a), int(x_a)) += weight * (float)refFrame.mImGray.at<uchar>(_y, _x);
                        imDepthAccumulator.at<float>(int(y_a), int(x_a)) += weight * matProjDepth.at<float>(j, 0);
                        mask.at<uchar>(int(y_a), int(x_a)) = 1;
                    }
                    else if ((imMinDepth.at<float>(y_a, x_a) - matProjDepth.at<float>(j, 0)) > 0)
                    {
                        weight = Area(x, x_a, y, y_a);
                        imCounter.at<float>(int(y_a), int(x_a)) = weight;
                        imGrayAccumulator.at<float>(int(y_a), int(x_a)) = weight * (float)refFrame.mImGray.at<uchar>(_y, _x);
                        imDepthAccumulator.at<float>(int(y_a), int(x_a)) = weight * matProjDepth.at<float>(j, 0);
                        mask.at<uchar>(int(y_a), int(x_a)) = 1;
                    }
                    imMinDepth.at<float>(y_a, x_a) = min(imMinDepth.at<float>(y_a, x_a), matProjDepth.at<float>(j, 0));
                }

这段代码是根据条件对不同的像素进行处理和更新。让我解释一下每个部分的含义:

  1. IsInImage(x_a, y_a, imGrayAccumulator): 这是一个条件判断函数,用于检查坐标 (x_a, y_a) 是否在目标图像的范围内。如果在图像范围内,则执行下面的代码段。

  2. abs(imMinDepth.at<float>(y_a, x_a) - matProjDepth.at<float>(j, 0)) < MIN_DEPTH_THRESHOLD: 这是一个条件判断语句,检查 imMinDepth 图像在坐标 (y_a, x_a) 处的深度值与 matProjDepth 的第 j 行第一列元素的差的绝对值是否小于 MIN_DEPTH_THRESHOLD。如果满足条件,则执行相应的代码块。

  3. weight = Area(x, x_a, y, y_a): 这个语句计算权重值,根据目标像素 (x, y) 与像素点 A 的相对位置 (x_a, y_a),使用某种计算公式来确定权重值。

  4. imCounter.at<float>(int(y_a), int(x_a)) += weight: 这行代码将权重值加到 imCounter 图像的坐标 (int(y_a), int(x_a)) 处的像素值上。

  5. imGrayAccumulator.at<float>(int(y_a), int(x_a)) += weight * (float)refFrame.mImGray.at<uchar>(_y, _x): 这行代码将权重值乘以参考帧 refFrame 的灰度图像在坐标 (_y, _x) 处的像素值,并加到 imGrayAccumulator 图像的坐标 (int(y_a), int(x_a)) 处的像素值上。

  6. imDepthAccumulator.at<float>(int(y_a), int(x_a)) += weight * matProjDepth.at<float>(j, 0): 这行代码将权重值乘以 matProjDepth 的第 j 行第一列元素的值,并加到 imDepthAccumulator 图像的坐标 (int(y_a), int(x_a)) 处的像素值上。

  7. mask.at<uchar>(int(y_a), int(x_a)) = 1: 这行代码将 mask 图像在坐标 (int(y_a), int(x_a)) 处的像素值设置为 1,表示此像素处于有效区域。

  8. imMinDepth.at<float>(y_a, x_a) = min(imMinDepth.at<float>(y_a, x_a), matProjDepth.at<float>(j, 0)): 这行代码比较 imMinDepth 图像在坐标 (y_a, x_a) 处的深度值和 matProjDepth 的第 j 行第一列元素的值,并将较小的值更新到 imMinDepth 图像的相应位置上。

总体来说,这段代码通过计算权重值并根据一些条件来更新不同图像和变量的值,可能涉及到图像深度处理、像素累积计算和掩码操作等

if (IsInImage(x_b, y_b, imGrayAccumulator) && (x_a != x_b))
                {
                    if (abs(imMinDepth.at<float>(y_b, x_b) - matProjDepth.at<float>(j, 0)) < MIN_DEPTH_THRESHOLD)
                    {
                        weight = Area(x, x_b, y, y_b);
                        imCounter.at<float>(int(y_b), int(x_b)) += weight;
                        imGrayAccumulator.at<float>(int(y_b), int(x_b)) += weight * (float)refFrame.mImGray.at<uchar>(_y, _x);
                        imDepthAccumulator.at<float>(int(y_b), int(x_b)) += weight * matProjDepth.at<float>(j, 0);
                        mask.at<uchar>(int(y_b), int(x_b)) = 1;
                    }
                    else if ((imMinDepth.at<float>(y_b, x_b) - matProjDepth.at<float>(j, 0)) > 0)
                    {
                        weight = Area(x, x_b, y, y_b);
                        imCounter.at<float>(int(y_b), int(x_b)) = weight;
                        imGrayAccumulator.at<float>(int(y_b), int(x_b)) = weight * (float)refFrame.mImGray.at<uchar>(_y, _x);
                        imDepthAccumulator.at<float>(int(y_b), int(x_b)) = weight * matProjDepth.at<float>(j, 0);
                        mask.at<uchar>(int(y_b), int(x_b)) = 1;
                    }
                    imMinDepth.at<float>(y_b, x_b) = min(imMinDepth.at<float>(y_b, x_b), matProjDepth.at<float>(j, 0));
                }

 

这段代码与上一段代码类似,是根据条件对不同的像素进行处理和更新。让我解释一下每个部分的含义:

  1. IsInImage(x_b, y_b, imGrayAccumulator): 这是一个条件判断函数,用于检查坐标 (x_b, y_b) 是否在目标图像的范围内。如果在图像范围内,则执行下面的代码段。

  2. (x_a != x_b): 这是一个条件判断语句,检查 x_a 是否不等于 x_b。如果两者不相等,则执行下面的代码段。

  3. abs(imMinDepth.at<float>(y_b, x_b) - matProjDepth.at<float>(j, 0)) < MIN_DEPTH_THRESHOLD: 这是一个条件判断语句,检查 imMinDepth 图像在坐标 (y_b, x_b) 处的深度值与 matProjDepth 的第 j 行第一列元素的差的绝对值是否小于 MIN_DEPTH_THRESHOLD。如果满足条件,则执行相应的代码块。

  4. weight = Area(x, x_b, y, y_b): 这个语句计算权重值,根据目标像素 (x, y) 与像素点 B 的相对位置 (x_b, y_b),使用某种计算公式来确定权重值。

  5. imCounter.at<float>(int(y_b), int(x_b)) += weight: 这行代码将权重值加到 imCounter 图像的坐标 (int(y_b), int(x_b)) 处的像素值上。

  6. imGrayAccumulator.at<float>(int(y_b), int(x_b)) += weight * (float)refFrame.mImGray.at<uchar>(_y, _x): 这行代码将权重值乘以参考帧 refFrame 的灰度图像在坐标 (_y, _x) 处的像素值,并加到 imGrayAccumulator 图像的坐标 (int(y_b), int(x_b)) 处的像素值上。

  7. imDepthAccumulator.at<float>(int(y_b), int(x_b)) += weight * matProjDepth.at<float>(j, 0): 这行代码将权重值乘以 matProjDepth 的第 j 行第一列元素的值,并加到 imDepthAccumulator 图像的坐标 (int(y_b), int(x_b)) 处的像素值上。

  8. mask.at<uchar>(int(y_b), int(x_b)) = 1: 这行代码将 mask 图像在坐标 (int(y_b), int(x_b)) 处的像素值设置为 1,表示此像素处于有效区域。

  9. imMinDepth.at<float>(y_b, x_b) = min(imMinDepth.at<float>(y_b, x_b), matProjDepth.at<float>(j, 0)): 这行代码比较 imMinDepth 图像在坐标 (y_b, x_b) 处的深度值和 matProjDepth 的第 j 行第一列元素的值,并将较小的值更新到 imMinDepth 图像的相应位置上。

这段代码的作用是根据条件将权重值和一些变量更新到不同的图像和变量中。

if (IsInImage(x_c, y_c, imGrayAccumulator) && (y_a != y_c) && (x_b != x_c && y_b != y_c))
                {
                    if (abs(imMinDepth.at<float>(y_c, x_c) - matProjDepth.at<float>(j, 0)) < MIN_DEPTH_THRESHOLD)
                    {
                        weight = Area(x, x_c, y, y_c);
                        imCounter.at<float>(int(y_c), int(x_c)) += weight;
                        imGrayAccumulator.at<float>(int(y_c), int(x_c)) += weight * (float)refFrame.mImGray.at<uchar>(_y, _x);
                        imDepthAccumulator.at<float>(int(y_c), int(x_c)) += weight * matProjDepth.at<float>(j, 0);
                        mask.at<uchar>(int(y_c), int(x_c)) = 1;
                    }
                    else if ((imMinDepth.at<float>(y_c, x_c) - matProjDepth.at<float>(j, 0)) > 0)
                    {
                        weight = Area(x, x_c, y, y_c);
                        imCounter.at<float>(int(y_c), int(x_c)) = weight;
                        imGrayAccumulator.at<float>(int(y_c), int(x_c)) = weight * (float)refFrame.mImGray.at<uchar>(_y, _x);
                        imDepthAccumulator.at<float>(int(y_c), int(x_c)) = weight * matProjDepth.at<float>(j, 0);
                        mask.at<uchar>(int(y_c), int(x_c)) = 1;
                    }
                    imMinDepth.at<float>(y_c, x_c) = min(imMinDepth.at<float>(y_c, x_c), matProjDepth.at<float>(j, 0));
                }

 

这段代码与前面类似,在特定的条件下对像素进行处理和更新。让我逐步解释每个部分的含义:

  1. IsInImage(x_c, y_c, imGrayAccumulator): 这是一个条件判断函数,用于检查坐标 (x_c, y_c) 是否在目标图像的范围内。

  2. (y_a != y_c) && (x_b != x_c && y_b != y_c): 这是一个条件判断语句,同时检查 y_a 是否不等于 y_c,以及 x_by_b 是否都不等于 x_cy_c。如果这些条件都满足,则执行下面的代码。

  3. abs(imMinDepth.at<float>(y_c, x_c) - matProjDepth.at<float>(j, 0)) < MIN_DEPTH_THRESHOLD: 这是一个条件判断语句,检查 imMinDepth 图像在坐标 (y_c, x_c) 处的深度值与 matProjDepth 的第 j 行第一列元素的差的绝对值是否小于 MIN_DEPTH_THRESHOLD。如果满足条件,则执行相应的代码块。

  4. weight = Area(x, x_c, y, y_c): 这个语句计算权重值,根据目标像素 (x, y) 与像素点 C 的相对位置 (x_c, y_c),使用某种计算公式来确定权重值。

  5. imCounter.at<float>(int(y_c), int(x_c)) += weight: 这行代码将权重值加到 imCounter 图像的坐标 (int(y_c), int(x_c)) 处的像素值上。

  6. imGrayAccumulator.at<float>(int(y_c), int(x_c)) += weight * (float)refFrame.mImGray.at<uchar>(_y, _x): 这行代码将权重值乘以参考帧 refFrame 的灰度图像在坐标 (_y, _x) 处的像素值,并加到 imGrayAccumulator 图像的坐标 (int(y_c), int(x_c)) 处的像素值上。

  7. imDepthAccumulator.at<float>(int(y_c), int(x_c)) += weight * matProjDepth.at<float>(j, 0): 这行代码将权重值乘以 matProjDepth 的第 j 行第一列元素的值,并加到 imDepthAccumulator 图像的坐标 (int(y_c), int(x_c)) 处的像素值上。

  8. mask.at<uchar>(int(y_c), int(x_c)) = 1: 这行代码将 mask 图像在坐标 (int(y_c), int(x_c)) 处的像素值设置为 1,表示此像素处于有效区域。

  9. imMinDepth.at<float>(y_c, x_c) = min(imMinDepth.at<float>(y_c, x_c), matProjDepth.at<float>(j, 0)): 这行代码比较 imMinDepth 图像在坐标 (y_c, x_c) 处的深度值和 matProjDepth 的第 j 行第一列元素的值,并将较小的值更新到 imMinDepth 图像的相应位置上。

这段代码的作用是根据一系列条件对像素进行处理和更新,将权重值和一些变量更新到不同的图像和变量中。

if (IsInImage(x_d, y_d, imGrayAccumulator) && (x_a != x_d && y_a != y_d) && (y_b != y_d) && (x_d != x_c))
                {
                    if (abs(imMinDepth.at<float>(y_d, x_d) - matProjDepth.at<float>(j, 0)) < MIN_DEPTH_THRESHOLD)
                    {
                        weight = Area(x, x_d, y, y_d);
                        imCounter.at<float>(int(y_d), int(x_d)) += weight;
                        imGrayAccumulator.at<float>(int(y_d), int(x_d)) += weight * (float)refFrame.mImGray.at<uchar>(_y, _x);
                        imDepthAccumulator.at<float>(int(y_d), int(x_d)) += weight * matProjDepth.at<float>(j, 0);
                        mask.at<uchar>(int(y_d), int(x_d)) = 1;
                    }
                    else if ((imMinDepth.at<float>(y_d, x_d) - matProjDepth.at<float>(j, 0)) > 0)
                    {
                        weight = Area(x, x_d, y, y_d);
                        imCounter.at<float>(int(y_d), int(x_d)) = weight;
                        imGrayAccumulator.at<float>(int(y_d), int(x_d)) = weight * (float)refFrame.mImGray.at<uchar>(_y, _x);
                        imDepthAccumulator.at<float>(int(y_d), int(x_d)) = weight * matProjDepth.at<float>(j, 0);
                        mask.at<uchar>(int(y_d), int(x_d)) = 1;
                    }
                    imMinDepth.at<float>(y_d, x_d) = min(imMinDepth.at<float>(y_d, x_d), matProjDepth.at<float>(j, 0));
                }

 

这段代码与前面的代码类似,也是在特定条件下对像素进行处理和更新。让我逐步解释每个部分的含义:

  1. IsInImage(x_d, y_d, imGrayAccumulator): 这是一个条件判断函数,用于检查坐标 (x_d, y_d) 是否在目标图像的范围内。

  2. (x_a != x_d && y_a != y_d) && (y_b != y_d) && (x_d != x_c): 这是一个条件判断语句,同时检查 x_ay_a 是否都不等于 x_dy_d,且 y_b 不能等于 y_d,还不能等于 x_dx_c。如果这些条件都满足,则执行下面的代码。

  3. abs(imMinDepth.at<float>(y_d, x_d) - matProjDepth.at<float>(j, 0)) < MIN_DEPTH_THRESHOLD: 这是一个条件判断语句,检查 imMinDepth 图像在坐标 (y_d, x_d) 处的深度值与 matProjDepth 的第 j 行第一列元素的差的绝对值是否小于 MIN_DEPTH_THRESHOLD。如果满足条件,则执行相应的代码块。

  4. weight = Area(x, x_d, y, y_d): 这个语句计算权重值,根据目标像素 (x, y) 与像素点 D 的相对位置 (x_d, y_d),使用某种计算公式来确定权重值。

  5. imCounter.at<float>(int(y_d), int(x_d)) += weight: 这行代码将权重值加到 imCounter 图像的坐标 (int(y_d), int(x_d)) 处的像素值上。

  6. imGrayAccumulator.at<float>(int(y_d), int(x_d)) += weight * (float)refFrame.mImGray.at<uchar>(_y, _x): 这行代码将权重值乘以参考帧 refFrame 的灰度图像在坐标 (_y, _x) 处的像素值,并加到 imGrayAccumulator 图像的坐标 (int(y_d), int(x_d)) 处的像素值上。

  7. imDepthAccumulator.at<float>(int(y_d), int(x_d)) += weight * matProjDepth.at<float>(j, 0): 这行代码将权重值乘以 matProjDepth 的第 j 行第一列元素的值,并加到 imDepthAccumulator 图像的坐标 (int(y_d), int(x_d)) 处的像素值上。

  8. mask.at<uchar>(int(y_d), int(x_d)) = 1: 这行代码将 mask 图像在坐标 (int(y_d), int(x_d)) 处的像素值设置为 1,表示此像素处于有效区域。

  9. imMinDepth.at<float>(y_d, x_d) = min(imMinDepth.at<float>(y_d, x_d), matProjDepth.at<float>(j, 0)): 这行代码比较 imMinDepth 图像在坐标 (y_d, x_d) 处的深度值和 matProjDepth 的第 j 行第一列元素的值,并将较小的值更新到 imMinDepth 图像的相应位置上。

这段代码的作用是根据一系列条件对像素进行处理和更新,将权重值和一些变量更新到不同的图像和变量中。

imGrayAccumulator = imGrayAccumulator.mul(1 / imCounter);
        imDepthAccumulator = imDepthAccumulator.mul(1 / imCounter);

        imGrayAccumulator.convertTo(imGrayAccumulator, CV_8U);
        imGray = imGray * 0;
        imGrayAccumulator.copyTo(imGray, mask);
        imDepth = imDepth * 0;
        imDepthAccumulator.copyTo(imDepth, mask);

 

这段代码是对图像进行计算和复制操作的一部分。让我逐步解释每个部分的含义:

  1. imGrayAccumulator = imGrayAccumulator.mul(1 / imCounter): 这行代码将 imGrayAccumulator 图像中的每个像素值与 imCounter 图像对应位置的像素值做除法运算。这相当于将 imGrayAccumulator 中的每个像素值除以 imCounter 中对应位置的像素值。

  2. imDepthAccumulator = imDepthAccumulator.mul(1 / imCounter): 这行代码将 imDepthAccumulator 图像中的每个像素值与 imCounter 图像对应位置的像素值做除法运算。这相当于将 imDepthAccumulator 中的每个像素值除以 imCounter 中对应位置的像素值。

  3. imGrayAccumulator.convertTo(imGrayAccumulator, CV_8U): 这行代码将 imGrayAccumulator 图像的数据类型转换为 8 位无符号整数型(CV_8U)。这可能是为了将图像的像素值转换为合适的范围或准备后续的操作。

  4. imGray = imGray * 0: 这行代码将 imGray 图像中的所有像素值乘以 0,相当于将所有像素值设置为 0。这可能是为了清空图像。

  5. imGrayAccumulator.copyTo(imGray, mask): 这行代码将 imGrayAccumulator 图像中的非零像素值复制到 imGray 图像对应位置,并且只复制 mask 图像中对应位置为非零的像素值。这可能是为了将计算得到的累加结果 imGrayAccumulator 应用到 imGray 图像上。

  6. imDepth = imDepth * 0: 这行代码将 imDepth 图像中的所有像素值乘以 0,相当于将所有像素值设置为 0。这可能是为了清空图像。

  7. imDepthAccumulator.copyTo(imDepth, mask): 这行代码将 imDepthAccumulator 图像中的非零像素值复制到 imDepth 图像对应位置,并且只复制 mask 图像中对应位置为非零的像素值。这可能是为了将计算得到的累加结果 imDepthAccumulator 应用到 imDepth 图像上。

总体来说,这段代码的作用是通过除以 imCounter 图像中的像素值,将累加器图像中的值归一化。然后,将归一化后的结果复制到对应的目标图像 imGrayimDepth 上,同时只复制 mask 中对应位置为非零的像素值。这可能是为了根据某种条件选择性地更新目标图像中的像素值,并清空累加器图像以备下一轮计算使用。

  • 1
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值