动态链接库和静态链接库的区别：(23条消息) 动态链接库和静态链接库的区别_wqfhenanxc的博客-CSDN博客_静态链接库和动态链接库的区别及优缺点
(23条消息) cmake中add_dependencies的基本作用_KingOfMyHeart的博客-CSDN博客_add_dependencies
静态链接库和动态链接库的区别 - 腾讯云开发者社区-腾讯云 (tencent.com)
launch文件详解
ROS 初级 - 解析 roslaunch 文件 - 知乎 (zhihu.com)
1)pkg
节点所在的包名
2)type
type=“file_name” 可执行文件的名称，如果是用Python文件就写xxx.py，如果是cpp就写编译生成的可执行文件名
3)name
name=“NODE_NAME” 为节点指派名称，这将会覆盖掉ros::init()定义的node_name
ROSbag数据集与opencv之间的相互转化：
ROS关于cv_brige的使用 - Being_young - 博客园 (cnblogs.com)
整体思路参考：
ROS S-MSCKF框架说明：
参考：(9条消息) S-MSCKF代码阅读_知也无涯12345的博客-CSDN博客
pluginlib：将类编译成为动态库，将库注册到ROS系统当中，更改时只需更换动态库，不用重新编译。zhongguo
前端总体框架：
调用ROSbag，记录的话题（cam0_image，cam1_image，imu）开始发送初始化完成的imu_sub和stereo_sub接收到，调用回调函数，分别对imu积分和双目图像进行处理
处理整体框架如下：notes (xinliang-zhong.vip)
处理完成后的发布，将最终追踪到的所有特征发布出去，（topic:features）
同时将追踪前后的特征数量也发布出去，（topic:tracking_info）
删除错误匹配点，twoPointRansac（）方法：
RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：
(24条消息) RANSAC算法理解_robinhjwy的博客-CSDN博客_ransac算法
- 有一个模型适用于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。
- 用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点。
- 如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。
- 然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。
- 最后，通过估计局内点与模型的错误率来评估模型。
void ImageProcessor::twoPointRansac(
// R_p_c通过IMU计算得到
const vector<Point2f>& pts1, const vector<Point2f>& pts2,
const cv::Matx33f& R_p_c, const cv::Vec4d& intrinsics,
const std::string& distortion_model,
const cv::Vec4d& distortion_coeffs,
const double& inlier_error,
const double& success_probability,
vector<int>& inlier_markers)1.检查数据，初始化所有数据点为内点，去畸变（一般较高质量摄像头没有畸变）
// Check the size of input point size.
if (pts1.size() != pts2.size())
ROS_ERROR("Sets of different size (%lu and %lu) are used...",
pts1.size(), pts2.size());
double norm_pixel_unit = 2.0 / (intrinsics[0]+intrinsics[1]);
int iter_num = static_cast<int>(
ceil(log(1-success_probability) / log(1-0.7*0.7)));//向上取整
// Initially, mark all points as inliers.
inlier_markers.clear();
inlier_markers.resize(pts1.size(), 1);
// Undistort all the points.
vector<Point2f> pts1_undistorted(pts1.size());
vector<Point2f> pts2_undistorted(pts2.size());
undistortPoints(
pts1, intrinsics, distortion_model,
distortion_coeffs, pts1_undistorted);
undistortPoints(
pts2, intrinsics, distortion_model,
distortion_coeffs, pts2_undistorted);2.通过IMU积分得到的旋转矩阵，更新上一帧关键点的位置，使两组关键点之间仅有平移t
// Compenstate the points in the previous image with
// the relative rotation.通过旋转矩阵更新位置
for (auto& pt : pts1_undistorted) {
Vec3f pt_h(pt.x, pt.y, 1.0f);
//Vec3f pt_hc = dR * pt_h;
Vec3f pt_hc = R_p_c * pt_h;
pt.x = pt_hc[0];
pt.y = pt_hc[1];
}3.归一化并记录缩放因子，保证计算时的数值稳定性
// Normalize the points to gain numerical stability.因为这里要开始ransac数值计算了
float scaling_factor = 0.0f;
rescalePoints(pts1_undistorted, pts2_undistorted, scaling_factor);
norm_pixel_unit *= scaling_factor;4.初筛，仅根据像素坐标之差来判断，当像素之差大于某一值时，直接判定为外点
// Compute the difference between previous and current points,
// which will be used frequently later.
vector<Point2d> pts_diff(pts1_undistorted.size());
for (int i = 0; i < pts1_undistorted.size(); ++i)
pts_diff[i] = pts1_undistorted[i] - pts2_undistorted[i];
// Mark the point pairs with large difference directly.大像素差距直接判断内外点
// BTW, the mean distance of the rest of the point pairs
// are computed.
double mean_pt_distance = 0.0;
int raw_inlier_cntr = 0;
for (int i = 0; i < pts_diff.size(); ++i) {
double distance = sqrt(pts_diff[i].dot(pts_diff[i]));
// 25 pixel distance is a pretty large tolerance for normal motion.
// However, to be used with aggressive motion, this tolerance should
// be increased significantly to match the usage.
if (distance > 50.0*norm_pixel_unit) {
inlier_markers[i] = 0;
} else {
mean_pt_distance += distance;
++raw_inlier_cntr;
}
}
mean_pt_distance /= raw_inlier_cntr;
// If the current number of inliers is less than 3, just mark
// all input as outliers. This case can happen with fast
// rotation where very few features are tracked.
if (raw_inlier_cntr < 3) {
for (auto& marker : inlier_markers) marker = 0;
return;
}5.检查是否为纯旋转，在纯旋转情况下，平移量t几乎为零，计算diff的平方，小于一定阈值，为纯旋转
// Before doing 2-point RANSAC, we have to check if the motion
// is degenerated, meaning that there is no translation between
// the frames, in which case, the model of the RANSAC does not
// work. If so, the distance between the matched points will
// be almost 0.
//if (mean_pt_distance < inlier_error*norm_pixel_unit) {
if (mean_pt_distance < norm_pixel_unit) {
//ROS_WARN_THROTTLE(1.0, "Degenerated motion...");
for (int i = 0; i < pts_diff.size(); ++i) {
if (inlier_markers[i] == 0) continue;
if (sqrt(pts_diff[i].dot(pts_diff[i])) >
inlier_error*norm_pixel_unit)
inlier_markers[i] = 0;
}
return;
}6.通常情况下的ransac,，建立拟合函数，通过优化目标（损失函数最小化），详细推导过程见notes (xinliang-zhong.vip)
这里主要提一下源代码作者采用的损失函数，因为只要求代表大小，不要求精确的值，尺度不确定，所以仅仅使Ax,AY.Az最小值对应的平移量为一，其他用上面其中一个式子求解即可，详细见代码
// Before doing 2-point RANSAC, we have to check if the motion
// is degenerated, meaning that there is no translation between
// the frames, in which case, the model of the RANSAC does not
// work. If so, the distance between the matched points will
// be almost 0.
//if (mean_pt_distance < inlier_error*norm_pixel_unit) {
if (mean_pt_distance < norm_pixel_unit) {
//ROS_WARN_THROTTLE(1.0, "Degenerated motion...");
for (int i = 0; i < pts_diff.size(); ++i) {
if (inlier_markers[i] == 0) continue;
if (sqrt(pts_diff[i].dot(pts_diff[i])) >
inlier_error*norm_pixel_unit)
inlier_markers[i] = 0;
}
return;
}
// In the case of general motion, the RANSAC model can be applied.
// The three column corresponds to tx, ty, and tz respectively.
// [y1-y2 -(x1-x2) x1y2-x2y1] 由对极几何约束得到
MatrixXd coeff_t(pts_diff.size(), 3);
for (int i = 0; i < pts_diff.size(); ++i) {
coeff_t(i, 0) = pts_diff[i].y;
coeff_t(i, 1) = -pts_diff[i].x;
coeff_t(i, 2) = pts1_undistorted[i].x*pts2_undistorted[i].y -
pts1_undistorted[i].y*pts2_undistorted[i].x;
}
//得到内点序号
vector<int> raw_inlier_idx;
for (int i = 0; i < inlier_markers.size(); ++i) {
if (inlier_markers[i] != 0)
raw_inlier_idx.push_back(i);
}
vector<int> best_inlier_set;
double best_error = 1e10;
random_numbers::RandomNumberGenerator random_gen;
for (int iter_idx = 0; iter_idx < iter_num; ++iter_idx) {
// Randomly select two point pairs.给出两个随机的
// Although this is a weird way of selecting two pairs, but it
// is able to efficiently avoid selecting repetitive pairs.
int select_idx1 = random_gen.uniformInteger(
0, raw_inlier_idx.size()-1);
int select_idx_diff = random_gen.uniformInteger(
1, raw_inlier_idx.size()-1);
int select_idx2 = select_idx1+select_idx_diff<raw_inlier_idx.size() ?
select_idx1+select_idx_diff :
select_idx1+select_idx_diff-raw_inlier_idx.size();
int pair_idx1 = raw_inlier_idx[select_idx1];
int pair_idx2 = raw_inlier_idx[select_idx2];
// Construct the model;两对点的t模型
Vector2d coeff_tx(coeff_t(pair_idx1, 0), coeff_t(pair_idx2, 0));
Vector2d coeff_ty(coeff_t(pair_idx1, 1), coeff_t(pair_idx2, 1));
Vector2d coeff_tz(coeff_t(pair_idx1, 2), coeff_t(pair_idx2, 2));
vector<double> coeff_l1_norm(3);
coeff_l1_norm[0] = coeff_tx.lpNorm<1>();//这里指一范数，简单求和
coeff_l1_norm[1] = coeff_ty.lpNorm<1>();
coeff_l1_norm[2] = coeff_tz.lpNorm<1>();
// 判断数据结构中最小的在哪里
int base_indicator = min_element(coeff_l1_norm.begin(),
coeff_l1_norm.end())-coeff_l1_norm.begin();
Vector3d model(0.0, 0.0, 0.0);//求解的是平移量
if (base_indicator == 0) {
Matrix2d A;
A << coeff_ty, coeff_tz;
Vector2d solution = A.inverse() * (-coeff_tx);
model(0) = 1.0;
model(1) = solution(0);
model(2) = solution(1);
} else if (base_indicator ==1) {
Matrix2d A;
A << coeff_tx, coeff_tz;
Vector2d solution = A.inverse() * (-coeff_ty);
model(0) = solution(0);
model(1) = 1.0;
model(2) = solution(1);
} else {
Matrix2d A;
A << coeff_tx, coeff_ty;
Vector2d solution = A.inverse() * (-coeff_tz);
model(0) = solution(0);
model(1) = solution(1);
model(2) = 1.0;
}
// Find all the inliers among point pairs.
VectorXd error = coeff_t * model;
vector<int> inlier_set;
for (int i = 0; i < error.rows(); ++i) {
if (inlier_markers[i] == 0) continue;
if (std::abs(error(i)) < inlier_error*norm_pixel_unit)
inlier_set.push_back(i);
}
// If the number of inliers is small, the current
// model is probably wrong.
// 直接进入下一个循环，随即选取新的点对
if (inlier_set.size() < 0.2*pts1_undistorted.size())
continue;
// Refit the model using all of the possible inliers.
VectorXd coeff_tx_better(inlier_set.size());
VectorXd coeff_ty_better(inlier_set.size());
VectorXd coeff_tz_better(inlier_set.size());
for (int i = 0; i < inlier_set.size(); ++i) {
coeff_tx_better(i) = coeff_t(inlier_set[i], 0);
coeff_ty_better(i) = coeff_t(inlier_set[i], 1);
coeff_tz_better(i) = coeff_t(inlier_set[i], 2);
}
Vector3d model_better(0.0, 0.0, 0.0);
if (base_indicator == 0) {
MatrixXd A(inlier_set.size(), 2);
A << coeff_ty_better, coeff_tz_better;
Vector2d solution =
(A.transpose() * A).inverse() * A.transpose() * (-coeff_tx_better);
//Vector2d solution = A.inverse() * (-coeff_tx);
model_better(0) = 1.0;
model_better(1) = solution(0);
model_better(2) = solution(1);
} else if (base_indicator ==1) {
MatrixXd A(inlier_set.size(), 2);
A << coeff_tx_better, coeff_tz_better;
Vector2d solution =
(A.transpose() * A).inverse() * A.transpose() * (-coeff_ty_better);
model_better(0) = solution(0);
model_better(1) = 1.0;
model_better(2) = solution(1);
} else {
MatrixXd A(inlier_set.size(), 2);
A << coeff_tx_better, coeff_ty_better;
Vector2d solution =
(A.transpose() * A).inverse() * A.transpose() * (-coeff_tz_better);
model_better(0) = solution(0);
model_better(1) = solution(1);
model_better(2) = 1.0;
}
// Compute the error and upate the best model if possible.
VectorXd new_error = coeff_t * model_better;
double this_error = 0.0;
for (const auto& inlier_idx : inlier_set)
this_error += std::abs(new_error(inlier_idx));
this_error /= inlier_set.size();
if (inlier_set.size() > best_inlier_set.size()) {
best_error = this_error;
best_inlier_set = inlier_set;
}
}
// Fill in the markers.
// inlier_set表示内点的序号，而inlier_markers是为所有点赋予零或一的标号
inlier_markers.clear();
inlier_markers.resize(pts1.size(), 0);
for (const auto& inlier_idx : best_inlier_set)
inlier_markers[inlier_idx] = 1;
//printf("inlier ratio: %lu/%lu\n",
// best_inlier_set.size(), inlier_markers.size());
return;
}
扫一扫
1026
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
