文章目录
在视惯初始化的时候，判断imu是否有足够的激励，通过计算平均加速度、标准差来进行
AX=0 构成超定方程，可以通过SVD进行求解；也可以是AX=B–>A’AX=A’B 进行ldlt进行求解。
计算视差角
https://www.cnblogs.com/cmt/p/14580194.html?from=https%3A%2F%2Fwww.cnblogs.com%2Fxuyi911204%2Fp%2F13154612.html&blogId=507077&postId=13154612
判断是不是纯旋转约束：
初始化的第一帧的标准
在滑窗(0-9)中找到第一个满足要求的帧(第l帧)，它与最新一帧(frame_count=10)有足够的共视点和平行度，并求出这两帧之间的相对位置变化关系 (1)定义容器
opencv中封装的关于slam位姿解算过程中用到的函数
cv::findFundamentalMat
https://blog.csdn.net/u011867581/article/details/24051885
Mat points1和Mat points2 32F
cv::recoverPose
bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
{
if (corres.size() >= 15)
{
vector<cv::Point2f> ll, rr;
for (int i = 0; i < int(corres.size()); i++)
{
ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
}
cv::Mat mask; //因为这里的ll,rr是归一化坐标，所以得到的是本质矩阵
cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
//得到的是ll向rr旋转的矩阵
cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
cv::Mat rot, trans;
int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);
//cout << "inlier_cnt " << inlier_cnt << endl;
//下面这段是cv::Mat向eigen的转化
Eigen::Matrix3d R;
Eigen::Vector3d T;
for (int i = 0; i < 3; i++)
{
T(i) = trans.at<double>(i, 0);
for (int j = 0; j < 3; j++)
R(i, j) = rot.at<double>(i, j);
}
Rotation = R.transpose();
Translation = -R.transpose() * T;//从第一帧向最新帧旋转的矩阵
if(inlier_cnt > 12)
return true;
else
return false;
}
return false;
}
/**
* Mat cv::findFundamentalMat( 返回通过RANSAC算法求解两幅图像之间的本质矩阵E
* nputArray points1, 第一幅图像点的数组
* InputArray points2, 第二幅图像点的数组
* int method = FM_RANSAC, RANSAC 算法
* double param1 = 3., 点到对极线的最大距离，超过这个值的点将被舍弃
* double param2 = 0.99, 矩阵正确的可信度
* OutputArray mask = noArray() 输出在计算过程中没有被舍弃的点
* )
*/
/**
* int cv::recoverPose ( 通过本质矩阵得到Rt，返回通过手性校验的内点个数
* InputArray E, 本质矩阵
* InputArray points1, 第一幅图像点的数组
* InputArray points2, 第二幅图像点的数组
* InputArray cameraMatrix, 相机内参
* OutputArray R, 第一帧坐标系到第二帧坐标系的旋转矩阵
* OutputArray t, 第一帧坐标系到第二帧坐标系的平移向量
* InputOutputArray mask = noArray() 在findFundamentalMat()中没有被舍弃的点
* )
*/
邻域内一致性检测
- 方法一
注意dx、dy两个数组，对这两个数组的对应序列遍历就得到了前后两幅图像的8邻域灰度值检测。
if(state[i] != 0)
{
int dx[10] = { -1, 0, 1, -1, 0, 1, -1, 0, 1 };
int dy[10] = { -1, -1, -1, 0, 0, 0, 1, 1, 1 };
int x1 = prepoint[i].x, y1 = prepoint[i].y;
int x2 = nextpoint[i].x, y2 = nextpoint[i].y;
if ((x1 < limit_edge_corner || x1 >= imgray.cols - limit_edge_corner || x2 < limit_edge_corner || x2 >= imgray.cols - limit_edge_corner
|| y1 < limit_edge_corner || y1 >= imgray.rows - limit_edge_corner || y2 < limit_edge_corner || y2 >= imgray.rows - limit_edge_corner))
{
state[i] = 0;
continue;
}
double sum_check = 0;
for (int j = 0; j < 9; j++)
sum_check += abs(imGrayPre.at<uchar>(y1 + dy[j], x1 + dx[j]) - imgray.at<uchar>(y2 + dy[j], x2 + dx[j]));//这里有部分的一致性检测，一个邻域范围内的检测
if (sum_check > limit_of_check) state[i] = 0;
if (state[i])
{
F_prepoint.push_back(prepoint[i]);
F_nextpoint.push_back(nextpoint[i]);
}
}
[点击并拖拽以移动]
- 方法二
Dyna slam geometry.cpp line362
关于邻域检测，也有先记录相对中心点周围数据的序列号，然后用中心点的序例号加上相对序列号获得周围的点坐标。
int x = (int)matCurrentFrame.at<float>(i,0) + (int)u1.at<float>(j,0);
int y = (int)matCurrentFrame.at<float>(i,1) + (int)u1.at<float>(j,1);
float _d = currentFrame.mImDepth.at<float>(y,x);
// 看点周围一个patch的深度是否>0并且比中心点的深度小
if ((_d > 0) && (_d < matProjDepth.at<float>(i,0)))
{
// 符合条件就存储这个深度并且计算深度
_matDepth.at<float>(s,0) = _d;
_matDiffDepth.at<float>(s,0) = matProjDepth.at<float>(i,0) - _d; // 存储这个点的深度-这个patch点的深度
s++;
}
而u1中存放的就是
for (int i(-mDmax); i <= mDmax; i++){
for (int j(-mDmax); j <= mDmax; j++){
u1.at<float>(m,0) = i;
u1.at<float>(m,1) = j;
m++; // 存储一个patch的x,y坐标
}
}
求取图像patch块的方差
可以把要比较的点放在一个方阵内，然后根据序列号求取出一个patch，利用std标准库里的东西进行方差的求解。
int xIni = (int)matCurrentFrame.at<float>(i,0) - mDmax;
int yIni = (int)matCurrentFrame.at<float>(i,1) - mDmax;
int xEnd = (int)matCurrentFrame.at<float>(i,0) + mDmax + 1;
int yEnd = (int)matCurrentFrame.at<float>(i,1) + mDmax + 1;
cv::Mat patch = currentFrame.mImDepth.rowRange(yIni,yEnd).colRange(xIni,xEnd);
cv::Mat mean, stddev;
// 计算patch内深度的均值和方差
// 对应论文中【如果关键点被设置为动态，但是它周围深度地图具有较大方差，
// 我们改变编号为静态。】的部分
cv::meanStdDev(patch,mean,stddev);
光流跟踪并进行一致性检验
cv::goodFeaturesToTrack(imGrayPre, prepoint, 1000, 0.01, 8, cv::Mat(), 3, true, 0.04);
cv::cornerSubPix(imGrayPre, prepoint, cv::Size(10, 10), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
cv::calcOpticalFlowPyrLK(imGrayPre, imgray, prepoint, nextpoint, state, err, cv::Size(22, 22), 5, cv::TermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.01));
for (int i = 0; i < state.size(); i++)
{
if(state[i] != 0)
{
int dx[10] = { -1, 0, 1, -1, 0, 1, -1, 0, 1 };
int dy[10] = { -1, -1, -1, 0, 0, 0, 1, 1, 1 };
int x1 = prepoint[i].x, y1 = prepoint[i].y;
int x2 = nextpoint[i].x, y2 = nextpoint[i].y;
if ((x1 < limit_edge_corner || x1 >= imgray.cols - limit_edge_corner || x2 < limit_edge_corner || x2 >= imgray.cols - limit_edge_corner
|| y1 < limit_edge_corner || y1 >= imgray.rows - limit_edge_corner || y2 < limit_edge_corner || y2 >= imgray.rows - limit_edge_corner))
{
state[i] = 0;
continue;
}
double sum_check = 0;
for (int j = 0; j < 9; j++)
sum_check += abs(imGrayPre.at<uchar>(y1 + dy[j], x1 + dx[j]) - imgray.at<uchar>(y2 + dy[j], x2 + dx[j]));//这里有部分的一致性检测，一个邻域范围内的检测
if (sum_check > limit_of_check) state[i] = 0;
if (state[i])
{
F_prepoint.push_back(prepoint[i]);
F_nextpoint.push_back(nextpoint[i]);
}
}
}
// F-Matrix
cv::Mat mask = cv::Mat(cv::Size(1, 300), CV_8UC1);
cv::Mat F = cv::findFundamentalMat(F_prepoint, F_nextpoint, mask, cv::FM_RANSAC, 0.1, 0.99);//这里的F_prepoint是2维的，也就是像素坐标系的坐标，求出来的是F
for (int i = 0; i < mask.rows; i++)
{
if (mask.at<uchar>(i, 0) == 0);
else //经过ransac是内点，但是没有经过F阵进行检验
{
// Circle(pre_frame, F_prepoint[i], 6, Scalar(255, 255, 0), 3);
double A = F.at<double>(0, 0)*F_prepoint[i].x + F.at<double>(0, 1)*F_prepoint[i].y + F.at<double>(0, 2);
double B = F.at<double>(1, 0)*F_prepoint[i].x + F.at<double>(1, 1)*F_prepoint[i].y + F.at<double>(1, 2);
double C = F.at<double>(2, 0)*F_prepoint[i].x + F.at<double>(2, 1)*F_prepoint[i].y + F.at<double>(2, 2);
double dd = fabs(A*F_nextpoint[i].x + B*F_nextpoint[i].y + C) / sqrt(A*A + B*B); //Epipolar constraints
if (dd <= 0.1)// 这里的dd也不过是一个经验值而已
{
F2_prepoint.push_back(F_prepoint[i]);
F2_nextpoint.push_back(F_nextpoint[i]);
}
}
}
F_prepoint = F2_prepoint;
F_nextpoint = F2_nextpoint;
for (int i = 0; i < prepoint.size(); i++)
{
if (state[i] != 0)//对光流正常跟踪上的点进行F检验
{
double A = F.at<double>(0, 0)*prepoint[i].x + F.at<double>(0, 1)*prepoint[i].y + F.at<double>(0, 2);
double B = F.at<double>(1, 0)*prepoint[i].x + F.at<double>(1, 1)*prepoint[i].y + F.at<double>(1, 2);
double C = F.at<double>(2, 0)*prepoint[i].x + F.at<double>(2, 1)*prepoint[i].y + F.at<double>(2, 2);
double dd = fabs(A*nextpoint[i].x + B*nextpoint[i].y + C) / sqrt(A*A + B*B);
// Judge outliers
if (dd <= limit_dis_epi) continue;
T_M.push_back(nextpoint[i]);
}
}
三角化
注意推导对极几何、平面几何、三角测量等，需要知道哪些等式来源于哪里。
P2=RP1+t 空间刚体变换 ，所以是P1、P2是三维坐标；
sx=KP 这是由相机内参构成的，x是像素点坐标，s是空间三维点z向的值
s2x2=Rs1x1+t
推导对极几何的时候用归一化相机坐标比较容易，再转换到像素点中；
三角测量也是从相机归一化坐标系入手进行解决。
x1,x2是像素点的归一化平面坐标，也就是用三维齐次坐标表示图像上的像素坐标， 同时除以第三维的话也就是归一化坐标（这个第三项也是空间点的z向值）；同时K的逆乘上x1也就是归一化相机平面上（z值为1）的坐标，
for ( DMatch m:matches )
{
// 将像素坐标转换至相机坐标
pts_1.push_back ( pixel2cam( keypoint_1[m.queryIdx].pt, K) );
pts_2.push_back ( pixel2cam( keypoint_2[m.trainIdx].pt, K) );//先转换到相机系
}
Mat pts_4d;
cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d );//三角化pts_4d是四维的，要将这四维同时除以第四维，得到空间三维点坐标
三角化操作中涉及的AX=0
SVD求最小二乘解
T是l帧到各个帧的转换(因为所有位姿和特征点目前都是在l帧表示的)，仅仅是关系变换矩阵而不是位姿矩阵
所以下面求解出来的点的坐标也是在l帧参考下的坐标
特别要注意opencv求解出来的位姿矩阵、特征点都是谁相对于谁的
这里的r1、r2、r3是代表的行向量的意思
x向量是四维了，解出来的值最后一个数需要确保它是1
triangulatePoint()
void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
Vector2d &point0, Vector2d &point1, Vector3d &point_3d)
{//https://blog.csdn.net/jsf921942722/article/details/95511167
Matrix4d design_matrix = Matrix4d::Zero();
design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);
design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);
design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);
design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);
Vector4d triangulated_point;
triangulated_point =
design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
point_3d(0) = triangulated_point(0) / triangulated_point(3);
point_3d(1) = triangulated_point(1) / triangulated_point(3);
point_3d(2) = triangulated_point(2) / triangulated_point(3);
}
一般来讲，求位姿，2D-2D对极几何只是在第一次使用，也就是没有3D特征点坐标的时候使用，一旦有了特征点，之后都会用3D-2D的方式求位姿。然后会进入PnP求新位姿，然后三角化求新3D坐标的循环中。
pnp求解
cv::Mat r, rvec, t, D, tmp_r;
cv::eigen2cv(R_initial, tmp_r);//转换成solvePnP能处理的格式
cv::Rodrigues(tmp_r, rvec);
cv::eigen2cv(P_initial, t);
cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
//注意这里K取单位阵 写代码的时候要特别注意
bool pnp_succ;
pnp_succ = cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1);//得到了第i帧到第l帧的旋转平移（以l帧为参考） pts_3_vector是l帧下的坐标，pts_2_vector是i帧下归一化相机坐标的值
if(!pnp_succ)
{
return false;
}
cv::Rodrigues(rvec, r);//罗德里格斯公式将旋转矩阵转换成旋转向量
//cout << "r " << endl << r << endl;
MatrixXd R_pnp;
cv::cv2eigen(r, R_pnp);//转换成原有格式
MatrixXd T_pnp;
cv::cv2eigen(t, T_pnp);
R_initial = R_pnp;//覆盖原先的旋转平移
P_initial = T_pnp;
return true;
特别要注意opencv求解出来的位姿矩阵、特征点都是谁相对于谁的
pnp_succ = cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1);//得到了第i帧到第l帧的旋转平移（以l帧为参考） pts_3_vector是l帧下的坐标，pts_2_vector是i帧下归一化相机坐标的值
关于sfm的一些处理技巧
以10帧大小滑窗，找到一个第l帧，使得第l帧和最新帧间的视差角大的那个（越靠近最新帧的话，视差会变小 if(average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T)) { //solveRelativeRT()通过基础矩阵计算当前帧与第l帧之间的R和T,并判断内点数目是否足够），利用匹配上的特征点的范数（vector.norm）进行判断，先以第l帧为参考，通过2D-2D特征点找到本质矩阵，解算出相对位姿，并进行三角化。对l+1、l+2、l+3到最新帧，和前一帧进行3D-2D的匹配解算相对矩阵。对于在sliding window里在第l帧之后的每一帧，分别都和前一帧用PnP求它的位姿，得到位姿后再和最新一帧三角化得到它们共视点的3D坐标 。从第l+1帧到滑窗的最后的每一帧再与第l帧进行三角化补充3D坐标 。第一1、2、到l-1帧，后一帧进行3D-2D的pnp矩阵。得到位姿后再和第l帧三角化得到它们共视点的3D坐标 。三角化其他未恢复的特征点 至此得到了滑动窗口中所有图像帧的位姿以及特征点的3D坐标。
对于滑窗内的帧，把它们设为关键帧，并获得它们对应的IMU坐标系到l系的旋转平移
对滑窗外的所有帧，求出它们对应的IMU坐标系到l帧的旋转平移
2359
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
