SLAM是个坑…
经过一段时间对SLAM的了解,原来是我理解有错误,得到的尺度已经是统一的,只是不知道它的真实尺度是多少。
贴一个不错的视觉里程计简介的链接:
https://blog.csdn.net/gzj2013/article/details/100878053
关于下面代码中单目视觉里程计中尺度的理解:
https://github.com/yueying/LearningVO/blob/master/src/visual_odometry.cpp
截取片段:
double VisualOdometry::getAbsoluteScale(int frame_id)
{
std::string line;
int i = 0;
std::ifstream ground_truth("C:/dataset/00/00.txt");
double x = 0, y = 0, z = 0;
double x_prev, y_prev, z_prev;
if (ground_truth.is_open())
{
while ((std::getline(ground_truth, line)) && (i <= frame_id))
{
z_prev = z;
x_prev = x;
y_prev = y;
std::istringstream in(line);
for (int j = 0; j < 12; j++) {
in >> z;
if (j == 7) y = z;
if (j == 3) x = z;
}
i++;
}
ground_truth.close();
}
else {
std::cerr<< "Unable to open file";
return 0;
}
return sqrt((x - x_prev)*(x - x_prev) + (y - y_prev)*(y - y_prev) + (z - z_prev)*(z - z_prev));
}
从以上代码来看,数据集KITTI-00的真实值pose.txt,里面每行是12列数据,12列数据很容易想到是3个平移量和一个3x3的旋转矩阵,这样想没错,但是其排列方式却不是这样的,而是一个3*4的矩阵,其排列方式为一个增广矩阵:[R|t]
也就是说,每一行的最后一列数据为平移的t的数据。
那么是如何得到尺度因子的呢?众所周知,单目无法得到真实尺度的信息,不具有单位的概念,因此上面的基于单目的视觉里程计其尺度信息是来自于groundtruth的,也就是事先知道的真实尺度,如何计算:获取当前帧真实位置与前一帧的真实位置的距离作为尺度值。
也就是最后return的值,其实就是当前帧的(x,y,z)减去上一帧的(x,y,z)这个真实距离作为真实尺度。
只是我不明白为什么更新尺度的时候是这样计算的呢?
bool VisualOdometry::processFrame(int frame_id)
{
double scale = 1.00;
featureTracking(last_frame_, new_frame_, px_ref_, px_cur_, disparities_);
cv::Mat E, R, t, mask;
E = cv::findEssentialMat(px_cur_, px_ref_, focal_, pp_, cv::RANSAC, 0.999, 1.0, mask);
cv::recoverPose(E, px_cur_, px_ref_, R, t, focal_, pp_, mask);
scale = getAbsoluteScale(frame_id);
if (scale > 0.1)
{
cur_t_ = cur_t_ + scale*(cur_R_*t);
cur_R_ = R*cur_R_;
}
if (px_ref_.size() < kMinNumFeature)
{
featureDetection(new_frame_, px_ref_);
featureTracking(last_frame_, new_frame_, px_ref_, px_cur_, disparities_);
}
px_ref_ = px_cur_;
return true;
}
其中第11行和第12行的代码我不是很懂,得到了实际尺度,为什么要那样更新呢?我本以为使用如下这样的更新方式就行了:
if (scale > 0.1)
{
cur_t_ = cur_t_ + scale*t;
cur_R_ = R*cur_R_;
}
我的理解是:先旋转后平移再加上原来的平移量才是真实的平移,如上注释。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)