lidar-imucalibration---lidar_align+运⾏liosam算法。。。
本⽂lidar-imu标定⽅法为瑞⼠苏黎世理⼯⼤学-⾃动驾驶实验室开源的⼀种校准 3D 激光雷达和 6 ⾃由度位姿传感器外参的⽅法。
该⽅法需要⼤量⾮平⾯的运动，因此不适合校准安装在汽车上的传感器标定
lidar_align⼯程代码实现了以下功能：
1、读取lidar和位姿传感器的数据
2、通过时间戳匹配lidar每帧⾥⾯的每个点和位姿传感器的坐标变换
2、通过上⾯的变换矩阵将利⽤位姿信息将lidar的每帧拼接成点云
3、每个点和它最近邻点的距离总和求出，在优化中就是不断的迭代找到坐标变换使这个距离最⼩
算法的整体思想：
1、为每帧lidar的每个点通过时间戳去匹配⼀个位姿数据，并通过插值的⽅式得到更准确的位姿值，每个点的时间偏移也是优化因素之⼀
2、利⽤NLopt的库的⾮线性优化⽅法
3、⽬标函数：让拼接后的点云的每个点的最近邻点最⼩
4、优化向量：x、y 、z、roll、pitch、yaw、time_offt
instead的用法总体来说就是不断的迭代，找到⼀个合适的优化向量（也就是lidar到⾥程计的坐标变换）使得拼在⼀起的点云每个点的最近邻点距离最⼩。
可以离线计算，录⼀个rosbag，然后跑⼀下就可以求的外参
它只会读取⼀个bag ，所以 lidar和位姿都要在⾥⾯
最终的结果：
在运⾏的时候，功能包会输出当前的估计变化
如何提高英语口语优化结束的时候 坐标变换的参数会打印在终端上
也会在路径下⾯保存⼀个txt⽂件和ply⽂件。
可以查看ply⽂件，就是拼接后的点云和场景是否⼀致
编译：
mkdir -p lidar_align_ws/src
cd lidar_align_ws/src
git /ethz-sl/lidar_align
cd..
catkin_make
编译过程中，报错如下：
问题⼀：编译时出现Could not ake
解决⽅法：
下载 安装⾮线性优化库nlopt
sudo apt-get install libnlopt-dev
若编译还是会报错：
Could not find a package configuration file provided by “NLOPT”
解决办法：
将 lidar_align ⽂件夹下的 ake 复制到 ROS⼯作空间lidar_align_ws/src路径下⾯
问题⼆：源码中没有接收IMU数据的接⼝，需要⾃⼰改写代码，加⼊imu数据的接⼝，只要改写loader.cpp即可，改写好的代码如下，可直接替换源码中loader.cpp的代码
#include<geometry_msgs/TransformStamped.h>
#include<rosbag/bag.h>
#include<rosbag/view.h>
#include<nsor_msgs/Imu.h>
#include"lidar_align/loader.h"
#include"lidar_align/transform.h"
namespace lidar_align {
Loader::Loader(const Config& config):config_(config){}
Loader::Config Loader::getConfig(ros::NodeHandle* nh){
Loader::Config config;
nh->param("u_n_scans", config.u_n_scans, config.u_n_scans); return config;
}
void Loader::parPointcloudMsg(const nsor_msgs::PointCloud2 msg, LoaderPointcloud* pointcloud){
bool has_timing =fal;
bool has_intensity =fal;
for(const nsor_msgs::PointField& field : msg.fields){
if(field.name =="time_offt_us"){
has_timing =true;
}el if(field.name =="intensity"){
has_intensity =true;
}
}
if(has_timing){
pcl::fromROSMsg(msg,*pointcloud);
return;
}el if(has_intensity){
Pointcloud raw_pointcloud;
pcl::fromROSMsg(msg, raw_pointcloud);
globecomfor(const Point& raw_point : raw_pointcloud){
PointAllFields point;
point.x = raw_point.x;
point.y = raw_point.y;
point.z = raw_point.z;
point.intensity = raw_point.intensity;
if(!std::isfinite(point.x)||!std::isfinite(point.y)||
!std::isfinite(point.z)||!std::isfinite(point.intensity)){
continue;
}
pointcloud->push_back(point);
}
pointcloud->header = raw_pointcloud.header;
}el{
pcl::PointCloud<pcl::PointXYZ> raw_pointcloud;
pcl::fromROSMsg(msg, raw_pointcloud);
for(const pcl::PointXYZ& raw_point : raw_pointcloud){
PointAllFields point;
point.x = raw_point.x;
point.y = raw_point.y;
point.z = raw_point.z;
if(!std::isfinite(point.x)||!std::isfinite(point.y)||
!
std::isfinite(point.z)){
continue;
}
pointcloud->push_back(point);
}
pointcloud->header = raw_pointcloud.header;
}
}
bool Loader::loadPointcloudFromROSBag(const std::string& bag_path,
const Scan::Config& scan_config,
Lidar* lidar){
rosbag::Bag bag;
cinatry{
bag.open(bag_path, rosbag::bagmode::Read);
}catch(rosbag::BagException e){
ROS_ERROR_STREAM("LOADING BAG FAILED: "<< e.what());
return fal;
}
std::vector<std::string> types;
不给糖就捣蛋 英文
types.push_back(std::string("nsor_msgs/PointCloud2"));
rosbag::View view(bag, rosbag::TypeQuery(types));
debbie rowesize_t scan_num =0;
for(const rosbag::MessageInstance& m : view){
std::cout <<" Loading scan: \e[1m"<< scan_num++<<"\e[0m from ros bag"
<<'\r'<< std::flush;
LoaderPointcloud pointcloud;
parPointcloudMsg(*(m.instantiate<nsor_msgs::PointCloud2>()),
&pointcloud);
lidar->addPointcloud(pointcloud, scan_config);
if(lidar->getNumberOfScans()>= config_.u_n_scans){
break;
}
}
if(lidar->getTotalPoints()==0){
ROS_ERROR_STREAM(
"No points were loaded, verify that the bag contains populated "
"messages of type nsor_msgs/PointCloud2");
return fal;
}
return true;
}
bool Loader::loadTformFromROSBag(const std::string& bag_path, Odom* odom){
结婚大作战 电影rosbag::Bag bag;
try{
bag.open(bag_path, rosbag::bagmode::Read);
}catch(rosbag::BagException e){
ROS_ERROR_STREAM("LOADING BAG FAILED: "<< e.what());
return fal;
}
std::vector<std::string> types;
types.push_back(std::string("nsor_msgs/Imu"));
rosbag::View view(bag, rosbag::TypeQuery(types));
size_t imu_num =0;
double shiftX=0,shiftY=0,shiftZ=0,velX=0,velY=0,velZ=0;
ros::Time time;
double timeDiff,lastShiftX,lastShiftY,lastShiftZ;
for(const rosbag::MessageInstance& m : view){
std::cout <<"Loading imu: \e[1m"<< imu_num++<<"\e[0m from ros bag"<<'\r'<< std::flush; nsor_msgs::Imu imu=*(m.instantiate<nsor_msgs::Imu>());
Timestamp stamp = imu.header.stamp.c *1000000ll+imu. /1000ll; if(imu_num==1){
time=imu.header.stamp;
Transform T(Transform::Translation(0,0,0),Transform::Rotation(1,0,0,0));
odom->addTransformData(stamp, T);
}
el{
timeDiff=(imu.header.stamp-time).toSec();
time=imu.header.stamp;
velX=velX+imu.linear_acceleration.x*timeDiff;
bec考试内容velY=velX+imu.linear_acceleration.y*timeDiff;
velZ=velZ+(imu.linear_acceleration.z-9.801)*timeDiff;
lastShiftX=shiftX;
lastShiftY=shiftY;
lastShiftZ=shiftZ;
shiftX=lastShiftX+velX*timeDiff+imu.linear_acceleration.x*timeDiff*timeDiff/2;
shiftY=lastShiftY+velY*timeDiff+imu.linear_acceleration.y*timeDiff*timeDiff/2;
shiftZ=lastShiftZ+velZ*timeDiff+(imu.linear_acceleration.z-9.801)*timeDiff*timeDiff/2;
Transform T(Transform::Translation(shiftX,shiftY,shiftZ),
Transform::ientation.w,
odom->addTransformData(stamp, T);
}
}
愿意的英文// std::vector<std::string> types;
wallet怎么用
// types.push_back(std::string("geometry_msgs/TransformStamped"));
// rosbag::View view(bag, rosbag::TypeQuery(types));
// size_t tform_num = 0;
/
/ for (const rosbag::MessageInstance& m : view) {
// std::cout << " Loading transform: \e[1m" << tform_num++
// << "\e[0m from ros bag" << '\r' << std::flush;
// geometry_msgs::TransformStamped transform_msg =
// *(m.instantiate<geometry_msgs::TransformStamped>());
// Timestamp stamp = transform_msg.header.stamp.c * 1000000ll +
// transform_msg. / 1000ll;
// Transform T(Transform::Translation(anslation.x,
// anslation.y,
// anslation.z),
// Transform::Rotation(ation.w,
/
/ ation.x,
// ation.y,
// ation.z));
// odom->addTransformData(stamp, T);
// }
if(odom->empty()){
ROS_ERROR_STREAM("No odom messages found!");
return fal;
}
return true;
}
bool Loader::loadTformFromMaplabCSV(const std::string& csv_path, Odom* odom){
std::ifstream file(csv_path, std::ifstream::in);
size_t tform_num =0;
while(file.peek()!=EOF){
std::cout <<" Loading transform: \e[1m"<< tform_num++
<<"\e[0m from csv file"<<'\r'<< std::flush;
Timestamp stamp;
Transform T;
if(getNextCSVTransform(file,&stamp,&T)){
odom->addTransformData(stamp, T);
}
}
return true;
}
// lots of potential failure cas not checked
bool Loader::getNextCSVTransform(std::istream& str, Timestamp* stamp,
Transform* T){
std::string line;
std::getline(str, line);
// ignore comment lines
if(line[0]=='#'){
return fal;
}
std::stringstream line_stream(line);
std::string cell;
std::vector<std::string> data;
while(std::getline(line_stream, cell,',')){
data.push_back(cell);
}
if(data.size()<9){
return fal;
}
constexpr size_t TIME =0;
constexpr size_t X =2;
constexpr size_t Y =3;
constexpr size_t Z =4;
constexpr size_t RW =5;
constexpr size_t RX =6;
constexpr size_t RY =7;
constexpr size_t RZ =8;
*stamp = std::stoll(data[TIME])/1000ll;
*T =Transform(Transform::Translation(std::stod(data[X]), std::stod(data[Y]), std::stod(data[Z])),
Transform::Rotation(std::stod(data[RW]), std::stod(data[RX]),
std::stod(data[RY]), std::stod(data[RZ])));
return true;
}
}// namespace lidar_align
注意：改写后记得包含头⽂件#include <nsor_msgs/Imu.h>
否则会报如下错误：
