雷达和相机标定
雷达标定
里程计标定和雷达标定
看笔记记录slam
安装ceres，用于运行上述标定相关函数
找时间按照这个教程安装
基于Win10+VS2019的ceres-solver-2.0.0配置流程 - Pyrokine - 博客园 (cnblogs.com)
Ceres Solver 在Windows下安装配置笔记 - 简书 (jianshu.com)
最好看第一个
代码已经下载
E:\[openFromGit]\CamLaserCalibraTool-master
ceres最小二成拟合直线
void LineFittingCeres(const std::vector<Eigen::Vector3d> Points, Eigen::Vector2d & Line)
{
double line[3] = {Line(0),Line(1)};
ceres::Problem problem;
for(size_t i = 0; i< Points.size(); ++i)
{
Eigen::Vector3d obi = Points[i];
ceres::CostFunction * costfunction =
new ceres::AutoDiffCostFunction<LineFittingResidfual,1,2>(
new LineFittingResidfual(obi.x(),obi.y()));
#ifdef LOSSFUNCTION
//ceres::LossFunctionWrapper* loss_function(new ceres::HuberLoss(1.0), ceres::TAKE_OWNERSHIP);
ceres::LossFunction * loss_function = new ceres::CauchyLoss(0.05);
problem.AddResidualBlock(costfunction, loss_function, line);
#else
problem.AddResidualBlock(costfunction, NULL, line);
#endif
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 10;
ceres::Solver::Summary summary;
ceres::Solve (options, &problem, & summary);
Line(0) = line[0];
Line(1) = line[1];
}
拟合圆
E:\[openFromGit]\CamLaserCalibraTool-master\camera_models\src\Utils.cc
(36条消息) 白巧克力亦唯心的博客_CSDN博客-算法推导系列,用ROS开发自己的机器人,slam领域博主
上面的博客提到了本论文；
上面博客还提到了轮子半径和雷达的标定方法
(22条消息) 2d Laser 和 Odomter 内外参数标定工具原理及使用方法_白巧克力亦唯心的博客-CSDN博客
https://github.com/MegviiRobot/OdomLaserCalibraTool.git
博客中提到了论文
(22条消息) 2d Laser 和 Odomter 内外参数标定工具原理及使用方法_白巧克力亦唯心的博客-CSDN博客
已下载上述标定工具repo
E:\[openFromGit]\OdomLaserCalibraTool-master
【great】从rosbag中对应topic读取odo数据和lidar数据
之前遇到过的，人腿检测里面也有读取rosBag数据；PlotJuggler也有读取rosBag里面topic对应的数据；
void messageIO::readDataFromBag(const std::string &bag_name, const std::string &laser_topic_name, const std::string &odom_topic_name,
std::vector<odometerData> &odom_data, std::vector<laserScanData> &laser_data)
{
rosbag::Bag bag;
格式转换
cScan.scanToLDP(laser_data, ldp); // 格式转换
使用迭代最近点匹配两帧lidar数据
sm_icp(¶ms, &results); // 使用迭代最近点匹配两帧lidar数据
复现了论文中的公式，值得阅读和复现
标定板aprilTag打印文件
E:\[openFromGit]\CamLaserCalibraTool-master\doc\april_6x6_80x80cm_A0.pdf
检测标定板位姿，去畸变和计算位姿，使用eigen计算;去图像畸变，得到归一化图像平面的坐标
E:\[openFromGit]\CamLaserCalibraTool-master\src\calcCamPose.cpp
std::vector<cv::Point2f> un_pts;
for (int i = 0, iend = (int)p2ds.size(); i < iend; ++i)
{
Eigen::Vector2d a(p2ds[i].x, p2ds[i].y);
Eigen::Vector3d b;
cam->liftProjective(a, b); // 去图像畸变， 得到归一化图像平面的坐标
un_pts.push_back(
cv::Point2f(b.x() / b.z(), b.y() / b.z() ));
}
if (EstimatePose(p3ds, un_pts, cam, Twc, img_raw))
{
cv::imshow("apriltag_detection & camPose_calculation", img_raw);
cv::waitKey(1);
return true;
}
bool CamPoseEst::EstimatePose(const std::vector<cv::Point3f> &p3ds,
const std::vector<cv::Point2f> &p2ds,
const CameraPtr &cam,
Eigen::Matrix4d &Twc, cv::Mat &img_raw)
{
glog和gflags可以在windows运行
库文件在：
E:\QtCode\cleanPackTest\main\lib_ceres\lib
测试文件在：
E:\QtCode\cleanPackTest\main\test_lib_ceres
自行取出加密
cvFileStorage 读写相机参数文件
cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
if(!fsSettings.isOpened())
{
std::cerr << "ERROR: Wrong path to settings" << std::endl;
return 0;
} else
{
std::cout << "READ CONFIG_FILE...\n";
}
std::string scan_topic_name;
std::string img_topic_name;
std::string bag_path;
std::string save_path;
fsSettings["scan_topic_name"] >> scan_topic_name;
fsSettings["img_topic_name"] >> img_topic_name;
fsSettings["savePath"] >> save_path;
fsSettings["bag_path"] >> bag_path;
配置文件
%YAML:1.0
#common parameters
savePath: "/home/hyj/all_ros_ws/LaserCamCal_ws/src/log/"
bag_path: "/home/hyj/all_ros_ws/LaserCamCal_ws/src/2019-07-12-11-36-17.bag"
scan_topic_name: "/scan"
img_topic_name: "/mynteye/left/image_raw"
# tag info
tag_type: 2 # 1: kalibr_tag, 2: apriltag
tag_size: 0.165 # tag size, unit meter
tag_spacing: 0.3 # tag spacing, only used in kalibr_tag. For details, please see kalibr tag description.
black_border: 1 # if you use kalibr_tag black_boarder = 2; if you use apriltag black_boarder = 1
#camera calibration
model_type: PINHOLE
camera_name: camera
image_width: 752
image_height: 480
distortion_parameters:
k1: 0.0
k2: 0.0
p1: 0.0
p2: 0.0
projection_parameters:
fx: 367.049931000148
fy: 366.94446918887405
cx: 368.7202381120387
cy: 241.13814795878562
相机类多个派生，三维坐标投影到相机平面等功能；
// Lift points from the image plane to the sphere
void liftSphere(const Eigen::Vector2d &p, Eigen::Vector3d &P) const;
//%output P
// Lift points from the image plane to the projective space
void liftProjective(const Eigen::Vector2d &p, Eigen::Vector3d &P) const;
//%output P
// Projects 3D points to the image plane (Pi function)
void spaceToPlane(const Eigen::Vector3d &P, Eigen::Vector2d &p) const;
//%output p
// Projects 3D points to the image plane (Pi function)
// and calculates jacobian
void spaceToPlane(const Eigen::Vector3d &P, Eigen::Vector2d &p,
Eigen::Matrix<double, 2, 3> &J) const;
//%output p
//%output J
void undistToPlane(const Eigen::Vector2d &p_u, Eigen::Vector2d &p) const;
//%output p
