LVI-SAM激光点云辅助视觉特征深度提取
这段代码的核心任务是将激光点云中的点与图像上的特征点进行对应，并计算图像特征点的深度。具体来说，它通过几个步骤将3D点云数据和2D图像特征点联系起来，最终估计出每个图像特征点的深度。下面是详细解释：
1. 坐标系转换
首先，代码会将激光点云从世界坐标系转换到相机坐标系。
- 步骤 0.3：使用TF库查找并获取当前时刻的相机位姿（
vins_world到vins_body_ros的变换）。通过这些数据，代码将世界坐标系下的点云转换到相机坐标系中。 - 步骤 0.4：调用
pcl::transformPointCloud函数，将激光点云从世界坐标系转换到相机坐标系下。这一步是将激光点云数据与相机的视角对齐，为后续操作做准备。
2. 构建单位球面坐标系下的图像特征点和激光点云
这一步的目的是将图像上的2D特征点和3D激光点投影到单位球面上，使得它们能够在相同的空间内进行比较。
- 步骤 0.5：将归一化后的图像特征点（z=1）投影到单位球面上。通过归一化处理，每个2D特征点被转换为一个3D方向向量。
- 步骤 5：同样的，将转换到相机坐标系后的激光点云也投影到单位球面上。每个激光点的深度信息被保存在
intensity字段中。
3. 构建深度直方图并过滤激光点云
这一部分的目的是通过构建一个深度直方图，过滤掉冗余的激光点，只保留距离相机最近的激光点。
- 步骤 3：将相机坐标系下的激光点投影到一个二维平面（深度直方图）上。通过这种方式，同一个bin内的多个激光点将被简化为距离最近的那个点。
- 步骤 4：根据深度直方图，从激光点云中提取出最接近的点，并构建一个过滤后的点云。这些点云点是与相机视角最近的点，将被用于后续的深度估计。
4. 最近邻搜索与深度估计
通过将图像特征点和激光点云在单位球面上的位置进行匹配，代码使用KD树查找激光点云中与每个图像特征点最近的三个激光点，并计算其深度。
- 步骤 6：为过滤后的激光点云构建一个KD树，以加速最近邻搜索。
- 步骤 7：对于每个图像特征点，在单位球面上查找与之最接近的三个激光点。通过这三个点的深度信息，以及它们在单位球面上的位置，利用几何方法估计图像特征点的深度。
5. 深度投影与可视化
最后，代码将深度信息投影回到图像平面，并可视化结果。
- 步骤 7：如果找到足够多的匹配点，将估计的深度值赋给图像特征点，并发布这些深度信息用于可视化。
- 步骤 8：将激光点云投影回图像平面，并对比显示。图像上显示的点根据其深度显示不同的颜色，方便用户直观地看到激光点云与图像特征的对应关系。
总结
这个流程目的是为了将激光雷达的3D信息与相机的2D图像特征点相结合。通过这种结合，可以为每个2D特征点估计一个深度值，从而帮助更准确地构建场景的3D结构。这对于视觉SLAM、3D重建等应用非常重要，因为它利用了激光雷达精确的深度测量来增强仅依赖视觉特征的深度估计。
sensor_msgs::ChannelFloat32 get_depth(const ros::Time& stamp_cur, const cv::Mat& imageCur,
const pcl::PointCloud<PointType>::Ptr& depthCloud,
const camodocal::CameraPtr& camera_model ,
const vector<geometry_msgs::Point32>& features_2d) // 去畸变后的归一化坐标 xy1
{
// 0.1 initialize depth for return 深度通道, 用于作为结果返回
sensor_msgs::ChannelFloat32 depth_of_point;
depth_of_point.name = "depth";
depth_of_point.values.resize(features_2d.size(), -1);
// 0.2 check if depthCloud available
if (depthCloud->size() == 0)
return depth_of_point;
// 0.3 look up transform at current image time
try{
listener.waitForTransform("vins_world", "vins_body_ros", stamp_cur, ros::Duration(0.01));
listener.lookupTransform("vins_world", "vins_body_ros", stamp_cur, transform);
}
catch (tf::TransformException ex){
// ROS_ERROR("image no tf");
return depth_of_point;
}
double xCur, yCur, zCur, rollCur, pitchCur, yawCur;
xCur = transform.getOrigin().x();
yCur = transform.getOrigin().y();
zCur = transform.getOrigin().z();
tf::Matrix3x3 m(transform.getRotation());
m.getRPY(rollCur, pitchCur, yawCur);
Eigen::Affine3f transNow = pcl::getTransformation(xCur, yCur, zCur, rollCur, pitchCur, yawCur);
// 0.4 transform cloud from global frame to camera frame
// 激光点云: 世界坐标系 ---> 相机坐标系
pcl::PointCloud<PointType>::Ptr depth_cloud_local(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*depthCloud, *depth_cloud_local, transNow.inverse());
// 0.5 project undistorted normalized (z) 2d features onto a unit sphere
// 将视觉特征点的归一化坐标投影到单位球上
pcl::PointCloud<PointType>::Ptr features_3d_sphere(new pcl::PointCloud<PointType>());
for (int i = 0; i < (int)features_2d.size(); ++i)
{
// normalize 2d feature to a unit sphere
Eigen::Vector3f feature_cur(features_2d[i].x, features_2d[i].y, features_2d[i].z); // z always equal to 1
feature_cur.normalize();
// convert to ROS standard
PointType p;
p.x = feature_cur(2);
p.y = -feature_cur(0);
p.z = -feature_cur(1);
p.intensity = -1; // intensity will be used to save depth
features_3d_sphere->push_back(p);
}
// 3. project depth cloud on a range image, filter points satcked in the same region
// 构造激光点云的深度直方图, 对激光点云进行采样 (类似于 LOAM 和 VoxelGrid)
float bin_res = 180.0 / (float)num_bins; // currently only cover the space in front of lidar (-90 ~ 90)
cv::Mat rangeImage = cv::Mat(num_bins, num_bins, CV_32F, cv::Scalar::all(FLT_MAX)); // 深度直方图
for (int i = 0; i < (int)depth_cloud_local->size(); ++i)
{
PointType p = depth_cloud_local->points[i];
// filter points not in camera view
if (p.x < 0 || abs(p.y / p.x) > 10 || abs(p.z / p.x) > 10)
continue;
// 当前激光点在深度直方图上的索引
// find row id in range image
float row_angle = atan2(p.z, sqrt(p.x * p.x + p.y * p.y)) * 180.0 / M_PI + 90.0; // degrees, bottom -> up, 0 -> 360
int row_id = round(row_angle / bin_res);
// find column id in range image
float col_angle = atan2(p.x, p.y) * 180.0 / M_PI; // degrees, left -> right, 0 -> 360
int col_id = round(col_angle / bin_res);
// id may be out of boundary
if (row_id < 0 || row_id >= num_bins || col_id < 0 || col_id >= num_bins)
continue; // 超出索引范围
// only keep points that's closer 仅仅保留最近的激光点深度
float dist = pointDistance(p);
if (dist < rangeImage.at<float>(row_id, col_id))
{
rangeImage.at<float>(row_id, col_id) = dist; // 深度
pointsArray[row_id][col_id] = p; // 激光点
}
}
// 4. extract downsampled depth cloud from range image
// 在深度直方图中, 每个bin只保留最近的一个激光点
pcl::PointCloud<PointType>::Ptr depth_cloud_local_filter2(new pcl::PointCloud<PointType>());
for (int i = 0; i < num_bins; ++i)
{
for (int j = 0; j < num_bins; ++j)
{
if (rangeImage.at<float>(i, j) != FLT_MAX)
depth_cloud_local_filter2->push_back(pointsArray[i][j]);
}
}
*depth_cloud_local = *depth_cloud_local_filter2;
publishCloud(&pub_depth_cloud, depth_cloud_local, stamp_cur, "vins_body_ros");
// 5. project depth cloud onto a unit sphere
// 将保留下来的激光点投影到单位球上
pcl::PointCloud<PointType>::Ptr depth_cloud_unit_sphere(new pcl::PointCloud<PointType>());
for (int i = 0; i < (int)depth_cloud_local->size(); ++i)
{
PointType p = depth_cloud_local->points[i];
float range = pointDistance(p);
p.x /= range;
p.y /= range;
p.z /= range;
p.intensity = range;
depth_cloud_unit_sphere->push_back(p);
}
if (depth_cloud_unit_sphere->size() < 10)
return depth_of_point;
// 6. create a kd-tree using the spherical depth cloud
pcl::KdTreeFLANN<PointType>::Ptr kdtree(new pcl::KdTreeFLANN<PointType>());
kdtree->setInputCloud(depth_cloud_unit_sphere);
// 7. find the feature depth using kd-tree
// 在单位球上, 寻找与视觉特征点最近的三个激光点, 来估计视觉特征点的深度
vector<int> pointSearchInd;
vector<float> pointSearchSqDis;
float dist_sq_threshold = pow(sin(bin_res / 180.0 * M_PI) * 5.0, 2);
for (int i = 0; i < (int)features_3d_sphere->size(); ++i)
{
kdtree->nearestKSearch(features_3d_sphere->points[i], 3, pointSearchInd, pointSearchSqDis);
if (pointSearchInd.size() == 3 && pointSearchSqDis[2] < dist_sq_threshold)
{
// 单位球上最近的三个激光点 A B C (实际坐标)
float r1 = depth_cloud_unit_sphere->points[pointSearchInd[0]].intensity; // A的深度
Eigen::Vector3f A(depth_cloud_unit_sphere->points[pointSearchInd[0]].x * r1,
depth_cloud_unit_sphere->points[pointSearchInd[0]].y * r1,
depth_cloud_unit_sphere->points[pointSearchInd[0]].z * r1);
float r2 = depth_cloud_unit_sphere->points[pointSearchInd[1]].intensity;
Eigen::Vector3f B(depth_cloud_unit_sphere->points[pointSearchInd[1]].x * r2,
depth_cloud_unit_sphere->points[pointSearchInd[1]].y * r2,
depth_cloud_unit_sphere->points[pointSearchInd[1]].z * r2);
float r3 = depth_cloud_unit_sphere->points[pointSearchInd[2]].intensity;
Eigen::Vector3f C(depth_cloud_unit_sphere->points[pointSearchInd[2]].x * r3,
depth_cloud_unit_sphere->points[pointSearchInd[2]].y * r3,
depth_cloud_unit_sphere->points[pointSearchInd[2]].z * r3);
// 视觉特征点在单位球上的坐标 V
// https://math.stackexchange.com/questions/100439/determine-where-a-vector-will-intersect-a-plane
Eigen::Vector3f V(features_3d_sphere->points[i].x,
features_3d_sphere->points[i].y,
features_3d_sphere->points[i].z);
// 估计视觉特征点的深度 s
Eigen::Vector3f N = (A - B).cross(B - C);
float s = (N(0) * A(0) + N(1) * A(1) + N(2) * A(2)) // (BA X CB) * OA
/ (N(0) * V(0) + N(1) * V(1) + N(2) * V(2)); // (BA X CB) * OV
float min_depth = min(r1, min(r2, r3));
float max_depth = max(r1, max(r2, r3));
if (max_depth - min_depth > 2 || s <= 0.5)
{
continue;
} else if (s - max_depth > 0) {
s = max_depth;
} else if (s - min_depth < 0) {
s = min_depth;
}
// convert feature into cartesian space if depth is available
features_3d_sphere->points[i].x *= s;
features_3d_sphere->points[i].y *= s;
features_3d_sphere->points[i].z *= s;
// the obtained depth here is for unit sphere, VINS estimator needs depth for normalized feature (by value z), (lidar x = camera z)
features_3d_sphere->points[i].intensity = features_3d_sphere->points[i].x; // ???
}
}
// visualize features in cartesian 3d space (including the feature without depth (default 1))
publishCloud(&pub_depth_feature, features_3d_sphere, stamp_cur, "vins_body_ros");
// update depth value for return
for (int i = 0; i < (int)features_3d_sphere->size(); ++i)
{
if (features_3d_sphere->points[i].intensity > 3.0)
depth_of_point.values[i] = features_3d_sphere->points[i].intensity;
}
// visualization project points on image for visualization (it's slow!)
if (pub_depth_image.getNumSubscribers() != 0)
{
vector<cv::Point2f> points_2d;
vector<float> points_distance;
for (int i = 0; i < (int)depth_cloud_local->size(); ++i)
{
// convert points from 3D to 2D
Eigen::Vector3d p_3d(-depth_cloud_local->points[i].y,
-depth_cloud_local->points[i].z,
depth_cloud_local->points[i].x);
Eigen::Vector2d p_2d;
camera_model->spaceToPlane(p_3d, p_2d);
points_2d.push_back(cv::Point2f(p_2d(0), p_2d(1)));
points_distance.push_back(pointDistance(depth_cloud_local->points[i]));
}
cv::Mat showImage, circleImage;
cv::cvtColor(imageCur, showImage, cv::COLOR_GRAY2RGB);
circleImage = showImage.clone();
for (int i = 0; i < (int)points_2d.size(); ++i)
{
float r, g, b;
getColor(points_distance[i], 50.0, r, g, b);
cv::circle(circleImage, points_2d[i], 0, cv::Scalar(r, g, b), 5);
}
cv::addWeighted(showImage, 1.0, circleImage, 0.7, 0, showImage); // blend camera image and circle image
cv_bridge::CvImage bridge;
bridge.image = showImage;
bridge.encoding = "rgb8";
sensor_msgs::Image::Ptr imageShowPointer = bridge.toImageMsg();
imageShowPointer->header.stamp = stamp_cur;
pub_depth_image.publish(imageShowPointer);
}
return depth_of_point;
}