VDO-SLAM中的场景流强度计算
VDO-SLAM中的场景流强度计算
实验思路：
ORB特征点提取，获取特征点的2D坐标
通过2D坐标和深度图反投影出特征点的3D坐标。利用特征点的3D坐标计算场景流强度。
VDO中初始位姿估计：
1.P3P+RANSAC估计位姿：
先获取上一帧的 UnprojectStereoStat 3D静态点(在位姿优化部分获得，跟建图有关)
以及当前帧用来估计相机位姿的2D点，P3P+RANSAC计算位姿得到 Mod，并得到内点 inliers
2.运动模型估计位姿：
利用上一帧的位姿和速度估计当前帧的位姿 MotionModel
3.计算重投影误差选择模型
利用当前帧的位姿对上一帧的3D点投影，变成2D点之后与当前帧的2D点计算重投影误差，
误差小于阈值的存储到 MM_inlier
若内点inliers数目>MM_inlier，选择模型Mod
若内点inliers数目<=MM_inlier，选择模型MotionModel
VDO中将2D特征点恢复为3D特征点的相关函数
// Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates.
cv::Mat UnprojectStereo(const int &i);
cv::Mat UnprojectStereoStat(const int &i, const bool &addnoise);
cv::Mat UnprojectStereoObject(const int &i, const bool &addnoise);
cv::Mat UnprojectStereoObjectCamera(const int &i, const bool &addnoise);
cv::Mat UnprojectStereoObjectNoise(const int &i, const cv::Point2f of_error);
cv::Mat ObtainFlowDepthObject(const int &i, const bool &addnoise);
cv::Mat ObtainFlowDepthCamera(const int &i, const bool &addnoise);
std::vector<cv::KeyPoint> SampleKeyPoints(const int &rows, const int &cols);
函数具体实现：
//当某个特征点的深度信息或者双目信息有效时,将它反投影到三维世界坐标系中
cv::Mat Frame::UnprojectStereo(const int &i)
{
// KeyFrame::UnprojectStereo
// 貌似这里普通帧的反投影函数操作过程和关键帧的反投影函数操作过程有一些不同：
// mvDepth是在ComputeStereoMatches函数中求取的
// TODO 验证下面的这些内容. 虽然现在我感觉是理解错了,但是不确定;如果确定是真的理解错了,那么就删除下面的内容
// mvDepth对应的校正前的特征点，可这里却是对校正后特征点反投影
// KeyFrame::UnprojectStereo中是对校正前的特征点mvKeys反投影
// 在ComputeStereoMatches函数中应该对校正后的特征点求深度？？ (wubo???)
// NOTE 不过我记得好像上面的ComputeStereoMatches函数就是对于双目相机设计的，而双目相机的图像默认都是经过了校正的啊
/** 步骤如下: */
/** 获取这个特征点的深度（这里的深度可能是通过双目视差得出的，也可能是直接通过深度图像的出来的） */
const float z = mvDepth[i];
/** 判断这个深度是否合法 */
//（其实这里也可以不再进行判断，因为在计算或者生成这个深度的时候都是经过检查了的_不行,RGBD的不是）
if(z>0)
{
/** 如果合法,就利用矫正后的特征点的坐标 Frame::mvKeysUn 和相机的内参数,通过反投影和位姿变换得到空间点的坐标 */
//获取像素坐标，注意这里是矫正后的特征点的坐标
const float u = mvKeysUn[i].pt.x;
const float v = mvKeysUn[i].pt.y;
//计算在当前相机坐标系下的坐标
const float x = (u-cx)*z*invfx;
const float y = (v-cy)*z*invfy;
//生成三维点（在当前相机坐标系下）
cv::Mat x3Dc = (cv::Mat_<float>(3,1) << x, y, z);
//然后计算这个点在世界坐标系下的坐标，这里是对的。
//但是公式还是要斟酌一下。首先变换成在没有旋转的相机坐标系下，最后考虑相机坐标系相对于世界坐标系的平移
//mOw为相机原点
return mRwc*x3Dc+mOw;
}
else
/** 如果深度值不合法，那么就返回一个空矩阵,表示计算失败 */
return cv::Mat();
/** */
/** */
}
} //namespace ORB_SLAM
这里对应的反投影函数为：
场景流计算在Tracking::GetSceneFlowObj中：
// get the 3d flow
cv::Mat x3D_p = mLastFrame.UnprojectStereoObject(i,0);
cv::Mat x3D_c = mCurrentFrame.UnprojectStereoObject(i,0);
pts_p3d[i] << x3D_p.at<float>(0), x3D_p.at<float>(1), x3D_p.at<float>(2);
// cout << "3d points: " << x3D_p << " " << x3D_c << endl;
cv::Point3f flow3d;
flow3d.x = x3D_c.at<float>(0) - x3D_p.at<float>(0);
flow3d.y = x3D_c.at<float>(1) - x3D_p.at<float>(1);
flow3d.z = x3D_c.at<float>(2) - x3D_p.at<float>(2);
pts_vel[i] << flow3d.x, flow3d.y, flow3d.z;
// cout << "3d points: " << mCurrentFrame.vFlow_3d[i] << endl;
// // threshold the velocity
// if(cv::norm(flow3d)*fps > max_velocity_ms)
// {
// mCurrentFrame.vObjLabel[i]=-1;
// continue;
// }
mCurrentFrame.vFlow_3d[i] = flow3d;
计算后将场景流大小存储为vFlow_3d向量，下一步判断大小是否超过阈值
相关代码在std::vector
中：
// Check scene flow distribution of each object and keep the dynamic object
//为每一个物体检验场景流并保留动态目标物体
std::vector<std::vector<int> > ObjIdNew;
std::vector<int> SemPosNew, obj_dis_tres(sem_posi.size(),0);
for (int i = 0; i < ObjId.size(); ++i)
//遍历目标的每一个特征点
{
float obj_center_depth = 0, sf_min=100, sf_max=0, sf_mean=0, sf_count=0;
std::vector<int> sf_range(10,0);
for (int j = 0; j < ObjId[i].size(); ++j)
{
obj_center_depth = obj_center_depth + mCurrentFrame.mvObjDepth[ObjId[i][j]];
// const float sf_norm = cv::norm(mCurrentFrame.vFlow_3d[ObjId[i][j]]);
//求场景流的1范数也就是向量大小，没有考虑y方向
float sf_norm = std::sqrt(mCurrentFrame.vFlow_3d[ObjId[i][j]].x*mCurrentFrame.vFlow_3d[ObjId[i][j]].x + mCurrentFrame.vFlow_3d[ObjId[i][j]].z*mCurrentFrame.vFlow_3d[ObjId[i][j]].z);
if (sf_norm<fSFMgThres)//如果场景流小于静态阈值0.12 为静态点
// sf_count存储的是静态点
sf_count = sf_count+1;
if(sf_norm<sf_min)
sf_min = sf_norm;
if(sf_norm>sf_max)
sf_max = sf_norm;
sf_mean = sf_mean + sf_norm;
{
if (0.0<=sf_norm && sf_norm<0.05)
sf_range[0] = sf_range[0] + 1;
else if (0.05<=sf_norm && sf_norm<0.1)
sf_range[1] = sf_range[1] + 1;
else if (0.1<=sf_norm && sf_norm<0.2)
sf_range[2] = sf_range[2] + 1;
else if (0.2<=sf_norm && sf_norm<0.4)
sf_range[3] = sf_range[3] + 1;
else if (0.4<=sf_norm && sf_norm<0.8)
sf_range[4] = sf_range[4] + 1;
else if (0.8<=sf_norm && sf_norm<1.6)
sf_range[5] = sf_range[5] + 1;
else if (1.6<=sf_norm && sf_norm<3.2)
sf_range[6] = sf_range[6] + 1;
else if (3.2<=sf_norm && sf_norm<6.4)
sf_range[7] = sf_range[7] + 1;
else if (6.4<=sf_norm && sf_norm<12.8)
sf_range[8] = sf_range[8] + 1;
else if (12.8<=sf_norm && sf_norm<25.6)
sf_range[9] = sf_range[9] + 1;
}
}
// cout << "scene flow distribution:" << endl;
// for (int j = 0; j < sf_range.size(); ++j)
// cout << sf_range[j] << " ";
// cout << endl;
if (sf_count/ObjId[i].size()>fSFDsThres)//如果静态点的比例大于70% 则目标为静态背景
{
// label this object as static background
for (int k = 0; k < ObjId[i].size(); ++k)
mCurrentFrame.vObjLabel[ObjId[i][k]] = 0;
continue;
}
else if (obj_center_depth/ObjId[i].size()>mThDepthObj || ObjId[i].size()<150)
{
obj_dis_tres[i]=-1;
// cout << "object " << sem_posi[i] <<" is too far away or too small! " << obj_center_depth/ObjId[i].size() << endl;
// label this object as far away object
for (int k = 0; k < ObjId[i].size(); ++k)
mCurrentFrame.vObjLabel[ObjId[i][k]] = -1;
continue;
}
else
{
// cout << "get new objects!" << endl;
ObjIdNew.push_back(ObjId[i]);
SemPosNew.push_back(sem_posi[i]);
}
}
整理简化后的计算函数：
void Tracking::GetSceneFlowObj()
{
// Initialization
int N = mCurrentFrame.N;
mCurrentFrame.vFlow_3d.resize(N);
float sf_min=100, sf_max=0, sf_mean=0, sf_count=0;
// Main loop
if (mLastFrame.N!=0 and mCurrentFrame.N!=0) {
for (int i = 0; i < N; ++i) {
// get the 3d flow
cv::Mat x3D_p = mLastFrame.UnprojectStereo(i);
cv::Mat x3D_c = mCurrentFrame.UnprojectStereo(i);
if (!x3D_p.empty() and !x3D_c.empty())
{
//cout << "3d points: " << x3D_p << " " << x3D_c << endl;
cv::Point3f flow3d;
flow3d.x = x3D_c.at<float>(0) - x3D_p.at<float>(0);
flow3d.y = x3D_c.at<float>(1) - x3D_p.at<float>(1);
flow3d.z = x3D_c.at<float>(2) - x3D_p.at<float>(2);
mCurrentFrame.vFlow_3d[i] = flow3d;
//cout << "3d points: " << mCurrentFrame.vFlow_3d[i] << endl;
float sf_norm = std::sqrt(mCurrentFrame.vFlow_3d[i].x*mCurrentFrame.vFlow_3d[i].x + mCurrentFrame.vFlow_3d[i].z*mCurrentFrame.vFlow_3d[i].z);
//cout<<"sf_norm="<
if (sf_norm<1.2)//如果场景流小于静态阈值0.12 为静态点
// sf_count存储的是静态点
sf_count = sf_count+1;
else
{
//去除动态点
//mCurrentFrame.mvbOutlier[i]=1;
//mCurrentFrame.mvKeys.push_back(mCurrentFrame.mvKeys[i]);
//mCurrentFrame.mDescriptors.push_back(mCurrentFrame.mDescriptors.row(i));
//mCurrentFrame.mvKeysUn.push_back(mCurrentFrame.mvKeysUn[i]);
}
if(sf_norm<sf_min)
sf_min = sf_norm;
if(sf_norm>sf_max)
sf_max = sf_norm;
sf_mean = sf_mean + sf_norm;
}
}
}
}