VINS-Mono无ROS代码阅读（4）：视差检测

最新推荐文章于 2023-10-31 13:41:07 发布

草莓味的月亮先生

最新推荐文章于 2023-10-31 13:41:07 发布

阅读量491

点赞数

文章标签： c++

本文链接：https://blog.csdn.net/weixin_44754915/article/details/123498821

版权

进图像数据的循环

参考：VINS-mono代码阅读 -- processImage_半亩园的博客-CSDN博客

VINS-Mono代码阅读笔记（六）：vins_estimator中图像处理1_文科升的博客-CSDN博客

在进行图像处理之前，先对接收到的特征点信息进行封装：特征点id、camera_id（哪个camera拍的）、该特征点在三维世界中的x,y,z坐标值、该特征点在二维图像帧中的像素坐标值u,v、该特征点在像素坐标上x,y方向上的速度。

将这些属性组织起来按照特征点的id值为index存放在map类型的image变量中，然后调用processImage的时候将image作为参数传入

map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
// 遍历img_msg 中的特征点
for (unsigned int i = 0; i < img_msg->points.size(); i++)
{
    int v = img_msg->channels[0].values[i] + 0.5;
    int feature_id = v / NUM_OF_CAM;
    int camera_id = v % NUM_OF_CAM;
    //归一化坐标
    double x = img_msg->points[i].x;
    double y = img_msg->points[i].y;
    double z = img_msg->points[i].z;
	//像素坐标
    double p_u = img_msg->channels[1].values[i];
    double p_v = img_msg->channels[2].values[i];
	//像素速度
    double velocity_x = img_msg->channels[3].values[i];
    double velocity_y = img_msg->channels[4].values[i];
    ROS_ASSERT(z == 1);//如果z != 1,就出错了
    Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
    xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
    image[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
}
estimator.processImage(image, img_msg->header);

process image

void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double header)
{
1. 检测视差，根据视差决定marg掉新帧还是老帧
    if (f_manager.addFeatureCheckParallax(frame_count, image, td))
        marginalization_flag = MARGIN_OLD;
    else
        marginalization_flag = MARGIN_SECOND_NEW;

    Headers[frame_count] = header;

    ImageFrame imageframe(image, header);
    imageframe.pre_integration = tmp_pre_integration;
    all_image_frame.insert(make_pair(header, imageframe));
    tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
2. 外参标定
    if (ESTIMATE_EXTRINSIC == 2)
    {
        
    }
3. 初始化
    if (solver_flag == INITIAL)
    {
        
    }
4. 非线性优化
    else
    {
        
    }
}

1. 检测视差

边缘化策略：根据次新帧是否为关键帧，分成两种边缘化策略：通过对比次新帧和次次新帧的视差量来决定marg掉次新帧还是最老帧。

检测视差的函数：addFeatureCheckParallax

函数返回值是bool运算。

平均视差大，返回值为true，marg old

平均视差小，返回值为false，marg new

bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{
    double parallax_sum = 0;// 所有特征点的视差总和
    int parallax_num = 0; //满足某些条件的跟踪点的个数
    last_track_num = 0;//被跟踪点的个数

	遍历image中所有的特征点，和已经记录了特征点的容器feature中进行比较
    for (auto &id_pts : image)
    {

    }

	加入到窗口中的帧个数取值为1或者0,或者能够跟踪到的特征点数目小于20个  
    if (frame_count < 2 || last_track_num < 20)
        return true;
	//遍历每一个feature
    for (auto &it_per_id : feature)
    {
		
    }

    if (parallax_num == 0)
    {
        return true;
    }
    else
    {
		//视差总和除以参与计算视差的特征点的个数，表示平均视差
        return parallax_sum / parallax_num >= MIN_PARALLAX;//MIN_PARALLAX = 10.0/460
    }
}

1. 第一个for循环：遍历图像中所有特征

(1)FeaturePerFrame是一个类（特征点管理器）存储特征点格式：首先按照特征点ID，一个一个存储，每个ID会包含其在不同帧上的位置，存储信息包括： x, y, z, p_u, p_v, velocity_x, velocity_y

FeaturePerFrame f_per_fra(id_pts.second[0].second, td);

在键值对中，key是关键字，value是值。

.first 得到key， .second 得到value

这里需要调试的是：.second[0]是什么？.second[0].second是什么？

(2)获取特征id

int feature_id = id_pts.first; 获取feature_id

(3)在feature中查找该feature_id的feature是否存在

auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
                          {
            return it.feature_id == feature_id;
                          });

根据下面find_if() 函数的理解，这里的find_if() 函数返回的是什么呢？应该是布尔值？

find_if() 函数会根据指定的查找规则，在指定区域内查找第一个符合该函数要求（使函数返回 true）的元素

InputIterator find_if (InputIterator first, InputIterator last, UnaryPredicate pred);

参考：C++ find_if()和find_if_not()函数用法详解
其中，first 和 last 都为输入迭代器，其组合 [first, last) 用于指定要查找的区域；pred 用于自定义查找规则。
值得一提的是，由于 first 和 last 都为输入迭代器，意味着该函数适用于所有的序列式容器。甚至当采用适当的谓词函数时，该函数还适用于所有的关联式容器（包括哈希容器）。

同时，该函数会返回一个输入迭代器，当查找成功时，该迭代器指向的是第一个符合查找规则的元素；反之，如果 find_if() 函数查找失败，则该迭代器的指向和 last 迭代器相同。

例子：

#include <iostream>     // std::cout
#include <algorithm>    // std::find_if
#include <vector>       // std::vector
using namespace std;
//自定义一元谓词函数
bool mycomp(int i) {
    return ((i % 2) == 1);
}
//以函数对象的形式定义一个 find_if() 函数的查找规则
class mycomp2 {
public:
    bool operator()(const int& i) {
        return ((i % 2) == 1);
    }
};
int main() {
    vector<int> myvector{ 4,2,3,1,5 };
    //调用 find_if() 函数，并以 IsOdd() 一元谓词函数作为查找规则
    vector<int>::iterator it = find_if(myvector.begin(), myvector.end(), mycomp2());
    cout << "*it = " << *it;
    return 0;
}


结果：*it = 3

（4）如果没有找到该特征点的id，就在管理器中增加此特征点。

这里的判断语句没有看懂？

if (it == feature.end())
{
 feature.push_back(FeaturePerId(feature_id, frame_count));//frame_count 是该特征点的起始帧
 feature.back().feature_per_frame.push_back(f_per_fra);
 //feature_per_frame表示每个FeaturePerId类型元素
}

feature的声明为：

list<FeasturePerId> feature //FeasturePerId是一个类

feature是个list类型的容器，里面每个元素类型为FeaturePerId

（5）如果找到了相同特征点，就在其FeaturePerFrame内增加此特征点在此帧的位置以及其他信息，
it的feature_per_frame容器中存放的是该feature能够被哪些帧看到，存放的是在这些帧中该特征点的信息
所以，feature_per_frame.size的大小就表示有多少个帧可以看到该特征点

else if (it->feature_id == feature_id)
 {
      it->feature_per_frame.push_back(f_per_fra);
      //last_track_num，表示此帧有多少个和其他帧中相同的特征点能够被追踪到
      last_track_num++;
}

2 第一个判断

如果加入到窗口中的帧个数取值为0或者1的时候，或者能够跟踪到的特征点数量少于20个，返回true，边缘化old帧

 if (frame_count < 2 || last_track_num < 20)
     return true;

3 遍历特征点

for (auto &it_per_id : feature)
    {
        //计算能被当前帧和其前两帧共同看到的特征点视差
        //it_per_id.feature_per_frame.size()表示该特征点能够被多少帧共视
        if (it_per_id.start_frame <= frame_count - 2 &&
            it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)
        {
            //计算特征点it_per_id在倒数第二帧和倒数第三帧之间的视差，并求所有视差的累加和
            parallax_sum += compensatedParallax2(it_per_id, frame_count);
            parallax_num++;
        }
    }

4 总结

参考：VINS-mono代码阅读 -- processImag_半亩园的博客-CSDN博客

VINS-Mono 代码详细解读——feature_manager.cpp_try_again_later的博客-CSDN博客

FeatureManager管理所有特征点，通过list容器存储特征点属性

FeaturePerId指的是某feature_id下的所有FeaturePerFrame。常用feature_id和观测第一帧start_frame、最后一帧endFrame()

FeaturePerFrame指的是每帧基本的数据：特征点[x,y,z,u,v,vx,vy]和td IMU与cam同步时间差

程序执行的过程：

我们来看一下程序执行的过程：

对image种的每一个特征点，按照feature_id，feature(list)中查找，
如果没有找到这个feature_id，说明之前没有帧看到过这个特征点，那么就用这个点的id，frame_count(start_frame)新建一个FeaturePerId，然后把这个特征点的观测(f_per_fra)push到feature->feature_per_frame中。
如果找到了这个feature_id,说明前面的帧已经看到了这个点，就只需要把当前帧观测到的信息(f_per_fra)push到这个特征点对应的feature->feature_per_frame中。然后跟踪到的点的数目++。
加到滑窗中的帧的数目小于2，或者跟踪到的点的数目小于20个，返回true。
遍历每一个feature，对能被当前帧和前两帧观测到的点进行视差计算，调用compensatedParallax2
总视差等于0，返回true，
返回平均视差 >= 最小视差

参考：VINS-mono代码阅读 -- processImage_半亩园的博客-CSDN博客

草莓味的月亮先生

关注

0
点赞
踩
2

收藏

觉得还不错? 一键收藏
0
评论
VINS-Mono无ROS代码阅读（4）：视差检测

进图像数据的循环参考：VINS-mono代码阅读 -- processImage_半亩园的博客-CSDN博客在进行图像处理之前，先对接收到的特征点信息进行封装，image是建立每个特征点和(camera_id,[x,y,z,u,v,vx,vy])构成的map，索引为feature_id。然后调用processImagemap<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;/
复制链接

扫一扫