雷达和相机标定

雷达标定

里程计标定和雷达标定

 

看笔记记录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博客

 

(PDF) Full-DOF Calibration of a Rotating 2-D LIDAR with a Simple Plane Measurement (researchgate.net)

已下载上述标定工具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(&params, &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)

{

 

gloggflags可以在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

 

 

posted @ 2023-02-08 18:29  tmjDD  阅读(90)  评论(0编辑  收藏  举报