imu_lidar_calibration
imu_lidar_calibration copied to clipboard
About timestamp synchronization
In ros_calib_init.cpp,
/// Handle Lidar measurement
sensor_msgs::PointCloud2::ConstPtr s_lidar = m.instantiate<sensor_msgs::PointCloud2>();
if (s_lidar != nullptr && m.getTopic() == topic_lidar) {
lin_core::VPointCloud::Ptr cloud_pcl(new lin_core::VPointCloud);
pcl::fromROSMsg(*s_lidar, *cloud_pcl);
LOdom->feedScan((*s_lidar).header.stamp.toSec(), cloud_pcl);
LOdom->append_and_update(true);
Eigen::Matrix3d deltaR_L = LOdom->get_latest_relativePose().odometry_ij.block(0, 0, 3, 3);
Eigen::Quaterniond delta_qL(deltaR_L);
geometry_msgs::PoseStamped pose;
pose.header.stamp = s_lidar->header.stamp;
pose.pose.orientation.x = delta_qL.x();
pose.pose.orientation.y = delta_qL.y();
pose.pose.orientation.z = delta_qL.z();
pose.pose.orientation.w = delta_qL.w();
lodom_publisher.publish(pose);
**imupacket.header.stamp = s_lidar->header.stamp;**
imu_packet_publisher.publish(imupacket);
imupacket.stamps.clear();
imupacket.accelreadings.clear();
imupacket.gyroreadings.clear();
}
I don't understand why "imupacket.header.stamp = s_lidar->header.stamp". Why not use the timestamp of IMU?
topic: /imu_packet
header: seq: 38 stamp: secs: 1618587129 nsecs: 856979200 frame_id: '' stamps:
secs: 1618587129 // start
nsecs: 894484179
......
secs: 1618587129 // end
nsecs: 976984179
header: seq: 39 stamp: secs: 1618587129 nsecs: 956735488 frame_id: ''
why the last timestamp of 38 is later than the timestamp of 39 ?
@codehory
Hi, did you run it successfully? What's your version of Ubuntu and pcl?
I met an error:
(gdb) Program received signal SIGSEGV, Segmentation fault.
Undefined command: "Program". Try "help".
(gdb) 0x00007ffff2bdddc4 in long double boost::math::lanczos::lanczos17m64::lanczos_sum
I don't get the question about time sync but I discovered that https://github.com/koide3/ndt_omp works ok on newer ubuntu and pcl. I have added a slightly modified version in the repository itself.