// 双目三角化
// 结果放入了feature的estimated_depth中
void FeatureManager::triangulate(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])
{
for (auto &it_per_id : feature)//对于所有的特征点
{
if (it_per_id.estimated_depth > 0)
continue;
if(STEREO && it_per_id.feature_per_frame[0].is_stereo)//如果使用了双目,并且特征点也是双目观测的
{
int imu_i = it_per_id.start_frame;
//利用imu的位姿计算左相机位姿
Eigen::Matrix<double, 3, 4> leftPose;
Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
leftPose.leftCols<3>() = R0.transpose();
leftPose.rightCols<1>() = -R0.transpose() * t0;
//cout << "left pose " << leftPose << endl;
//利用imu的位姿计算右相机位姿
Eigen::Matrix<double, 3, 4> rightPose;
Eigen::Vector3d t1 = Ps[imu_i] + Rs[imu_i] * tic[1];
Eigen::Matrix3d R1 = Rs[imu_i] * ric[1];
rightPose.leftCols<3>() = R1.transpose();
rightPose.rightCols<1>() = -R1.transpose() * t1;
//cout << "right pose " << rightPose << endl;
Eigen::Vector2d point0, point1;
Eigen::Vector3d point3d;
point0 = it_per_id.feature_per_frame[0].point.head(2);
point1 = it_per_id.feature_per_frame[0].pointRight.head(2);
//cout << "point0 " << point0.transpose() << endl;
//cout << "point1 " << point1.transpose() << endl;
triangulatePoint(leftPose, rightPose, point0, point1, point3d);//利用svd方法对双目进行三角化
//得到imu坐标系下的三维点坐标
Eigen::Vector3d localPoint;
localPoint = leftPose.leftCols<3>() * point3d + leftPose.rightCols<1>();//求得左相机坐标系下的坐标
double depth = localPoint.z();
if (depth > 0) {
it_per_id.estimated_depth = depth;
vins_depth_util_->feed_data(point0,depth);//@Hao
}
else
it_per_id.estimated_depth = INIT_DEPTH;
/*
Vector3d ptsGt = pts_gt[it_per_id.feature_id];
printf("stereo %d pts: %f %f %f gt: %f %f %f \n",it_per_id.feature_id, point3d.x(), point3d.y(), point3d.z(),
ptsGt.x(), ptsGt.y(), ptsGt.z());
*/
continue;
}
else if(it_per_id.feature_per_frame.size() > 1)
{
int imu_i = it_per_id.start_frame;
Eigen::Matrix<double, 3, 4> leftPose;
Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
leftPose.leftCols<3>() = R0.transpose();
leftPose.rightCols<1>() = -R0.transpose() * t0;
imu_i++;
Eigen::Matrix<double, 3, 4> rightPose;
Eigen::Vector3d t1 = Ps[imu_i] + Rs[imu_i] * tic[0];
Eigen::Matrix3d R1 = Rs[imu_i] * ric[0];
rightPose.leftCols<3>() = R1.transpose();
rightPose.rightCols<1>() = -R1.transpose() * t1;
Eigen::Vector2d point0, point1;
Eigen::Vector3d point3d;
point0 = it_per_id.feature_per_frame[0].point.head(2);
point1 = it_per_id.feature_per_frame[1].point.head(2);
triangulatePoint(leftPose, rightPose, point0, point1, point3d);
Eigen::Vector3d localPoint;
localPoint = leftPose.leftCols<3>() * point3d + leftPose.rightCols<1>();
double depth = localPoint.z();
if (depth > 0)
it_per_id.estimated_depth = depth;
else
it_per_id.estimated_depth = INIT_DEPTH;
/*
Vector3d ptsGt = pts_gt[it_per_id.feature_id];
printf("motion %d pts: %f %f %f gt: %f %f %f \n",it_per_id.feature_id, point3d.x(), point3d.y(), point3d.z(),
ptsGt.x(), ptsGt.y(), ptsGt.z());
*/
continue;
}
//Hao
double v = vins_depth_util_->calculate_average_depth_inside_boundry();
if(v>0) {
ros1StrPub_vins_depth_util_->publish("avg depth = "+ to_string(v));
}
it_per_id.used_num = it_per_id.feature_per_frame.size();
if (it_per_id.used_num < 4)
continue;
int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);
int svd_idx = 0;
Eigen::Matrix<double, 3, 4> P0;
Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
P0.leftCols<3>() = Eigen::Matrix3d::Identity();
P0.rightCols<1>() = Eigen::Vector3d::Zero();
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
imu_j++;
Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];
Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];
Eigen::Vector3d t = R0.transpose() * (t1 - t0);
Eigen::Matrix3d R = R0.transpose() * R1;
Eigen::Matrix<double, 3, 4> P;
P.leftCols<3>() = R.transpose();
P.rightCols<1>() = -R.transpose() * t;
Eigen::Vector3d f = it_per_frame.point.normalized();
svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0);
svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1);
if (imu_i == imu_j)
continue;
}
ROS_ASSERT(svd_idx == svd_A.rows());
Eigen::Vector4d svd_V = Eigen::JacobiSVD<Eigen::MatrixXd>(svd_A, Eigen::ComputeThinV).matrixV().rightCols<1>();
double svd_method = svd_V[2] / svd_V[3];
//it_per_id->estimated_depth = -b / A;
//it_per_id->estimated_depth = svd_V[2] / svd_V[3];
it_per_id.estimated_depth = svd_method;
//it_per_id->estimated_depth = INIT_DEPTH;
if (it_per_id.estimated_depth < 0.1)
{
it_per_id.estimated_depth = INIT_DEPTH;
}
}
}
“相关推荐”对你有帮助么?
-
非常没帮助
-
没帮助
-
一般
-
有帮助
-
非常有帮助
提交