APRIL-ZJU / lidar_IMU_calib

[IROS 2020] Targetless Calibration of LiDAR-IMU System Based on Continuous-time Batch Estimation
GNU General Public License v3.0
927 stars 224 forks source link

a bug need to fix #38

Open chinahuangyong opened 2 years ago

chinahuangyong commented 2 years ago
bool read(const std::string path,
            const std::string imu_topic,
            const std::string lidar_topic,
            const double bag_start = -1.0,
            const double bag_durr = -1.0) {

    data_.reset(new LioDataset(lidar_model_));
    data_->bag_.reset(new rosbag::Bag);
    data_->bag_->open(path, rosbag::bagmode::Read);

    init();

    rosbag::View view;
    {
      std::vector<std::string> topics;
      topics.push_back(imu_topic);
      topics.push_back(lidar_topic);

      rosbag::View view_full;
      view_full.addQuery(*data_->bag_);
      ros::Time time_init = view_full.getBeginTime();
      time_init += ros::Duration(bag_start);
      ros::Time time_finish = (bag_durr < 0)?
                              view_full.getEndTime() : time_init + ros::Duration(bag_durr);
      view.addQuery(*data_->bag_, rosbag::TopicQuery(topics), time_init, time_finish);
    }
    for (rosbag::MessageInstance const m : view) {
      const std::string &topic = m.getTopic();

      if (lidar_topic == topic) {
        TPointCloud pointCloudPtr;
        double timestamp = 0;
        std::cout << "find lidar topic data type " << m.getDataType() << std::endl;
        if (m.getDataType() == std::string("velodyne_msgs/VelodyneScan")) {
          velodyne_msgs::VelodyneScan::ConstPtr vlp_msg =
                  m.instantiate<velodyne_msgs::VelodyneScan>();
          timestamp = vlp_msg->header.stamp.toSec();
          p_VelodyneLidarConvert_->unpack_scan(vlp_msg, pointCloudPtr);
          std::cout << "velodyne_msgs/VelodyneScan" << std::endl;
        }
        if (m.getDataType() == std::string("sensor_msgs/PointCloud2")) {
          sensor_msgs::PointCloud2::ConstPtr scan_msg =
                  m.instantiate<sensor_msgs::PointCloud2>();
          timestamp = scan_msg->header.stamp.toSec();
          p_VelodyneLidarConvert_->unpack_scan(scan_msg, pointCloudPtr);
          std::cout << "add sensor_msgs/PointCloud2" << std::endl;
        }
        if(m.getDataType() == std::string("livox_ros_driver/CustomMsg")) {
          li_calib::LivoxPointCloud ::ConstPtr scan_msg =
                  m.instantiate<li_calib::LivoxPointCloud>();
          timestamp = scan_msg->header.stamp.toSec();
          p_LivoxLidarConvert_->unpack_scan(scan_msg, pointCloudPtr);

          std::cout << "scan_msg header stamp: "<< scan_msg->header.stamp << std::endl;
          std::cout << "scan msg header frame id: " << scan_msg->header.frame_id << std::endl;
          std::cout << "scan msg point num: " << scan_msg->point_num << std::endl;
          std::cout << "scan msg point size: " << scan_msg->points.size() << std::endl;
          std::cout << "add livox_ros_driver/CustomMsg" << std::endl;
        }

        data_->scan_data_.emplace_back(pointCloudPtr);
        data_->scan_timestamps_.emplace_back(timestamp);
      }

      if (imu_topic == topic) {
        sensor_msgs::ImuConstPtr imu_msg = m.instantiate<sensor_msgs::Imu>();
        double time = imu_msg->header.stamp.toSec();
        data_->imu_data_.emplace_back();
        data_->imu_data_.back().timestamp = time;
        data_->imu_data_.back().accel = Eigen::Vector3d(
                imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y,
                imu_msg->linear_acceleration.z);
        data_->imu_data_.back().gyro = Eigen::Vector3d(
                imu_msg->angular_velocity.x, imu_msg->angular_velocity.y,
                imu_msg->angular_velocity.z);
      }
    }

    std::cout << lidar_topic << ": " << data_->scan_data_.size() << std::endl;
    std::cout << imu_topic << ": " << data_->imu_data_.size() << std::endl;

    return true;
  }

this function must return.

q87956256 commented 2 years ago
bool read(const std::string path,
            const std::string imu_topic,
            const std::string lidar_topic,
            const double bag_start = -1.0,
            const double bag_durr = -1.0) {

    data_.reset(new LioDataset(lidar_model_));
    data_->bag_.reset(new rosbag::Bag);
    data_->bag_->open(path, rosbag::bagmode::Read);

    init();

    rosbag::View view;
    {
      std::vector<std::string> topics;
      topics.push_back(imu_topic);
      topics.push_back(lidar_topic);

      rosbag::View view_full;
      view_full.addQuery(*data_->bag_);
      ros::Time time_init = view_full.getBeginTime();
      time_init += ros::Duration(bag_start);
      ros::Time time_finish = (bag_durr < 0)?
                              view_full.getEndTime() : time_init + ros::Duration(bag_durr);
      view.addQuery(*data_->bag_, rosbag::TopicQuery(topics), time_init, time_finish);
    }
    for (rosbag::MessageInstance const m : view) {
      const std::string &topic = m.getTopic();

      if (lidar_topic == topic) {
        TPointCloud pointCloudPtr;
        double timestamp = 0;
        std::cout << "find lidar topic data type " << m.getDataType() << std::endl;
        if (m.getDataType() == std::string("velodyne_msgs/VelodyneScan")) {
          velodyne_msgs::VelodyneScan::ConstPtr vlp_msg =
                  m.instantiate<velodyne_msgs::VelodyneScan>();
          timestamp = vlp_msg->header.stamp.toSec();
          p_VelodyneLidarConvert_->unpack_scan(vlp_msg, pointCloudPtr);
          std::cout << "velodyne_msgs/VelodyneScan" << std::endl;
        }
        if (m.getDataType() == std::string("sensor_msgs/PointCloud2")) {
          sensor_msgs::PointCloud2::ConstPtr scan_msg =
                  m.instantiate<sensor_msgs::PointCloud2>();
          timestamp = scan_msg->header.stamp.toSec();
          p_VelodyneLidarConvert_->unpack_scan(scan_msg, pointCloudPtr);
          std::cout << "add sensor_msgs/PointCloud2" << std::endl;
        }
        if(m.getDataType() == std::string("livox_ros_driver/CustomMsg")) {
          li_calib::LivoxPointCloud ::ConstPtr scan_msg =
                  m.instantiate<li_calib::LivoxPointCloud>();
          timestamp = scan_msg->header.stamp.toSec();
          p_LivoxLidarConvert_->unpack_scan(scan_msg, pointCloudPtr);

          std::cout << "scan_msg header stamp: "<< scan_msg->header.stamp << std::endl;
          std::cout << "scan msg header frame id: " << scan_msg->header.frame_id << std::endl;
          std::cout << "scan msg point num: " << scan_msg->point_num << std::endl;
          std::cout << "scan msg point size: " << scan_msg->points.size() << std::endl;
          std::cout << "add livox_ros_driver/CustomMsg" << std::endl;
        }

        data_->scan_data_.emplace_back(pointCloudPtr);
        data_->scan_timestamps_.emplace_back(timestamp);
      }

      if (imu_topic == topic) {
        sensor_msgs::ImuConstPtr imu_msg = m.instantiate<sensor_msgs::Imu>();
        double time = imu_msg->header.stamp.toSec();
        data_->imu_data_.emplace_back();
        data_->imu_data_.back().timestamp = time;
        data_->imu_data_.back().accel = Eigen::Vector3d(
                imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y,
                imu_msg->linear_acceleration.z);
        data_->imu_data_.back().gyro = Eigen::Vector3d(
                imu_msg->angular_velocity.x, imu_msg->angular_velocity.y,
                imu_msg->angular_velocity.z);
      }
    }

    std::cout << lidar_topic << ": " << data_->scan_data_.size() << std::endl;
    std::cout << imu_topic << ": " << data_->imu_data_.size() << std::endl;

    return true;
  }

this function must return.

请问下,我用ubuntu16.04编译运行没有问题,用ubuntu20.04编译运行会报错,加了return true之后就好了,请问这是什么原因呢?

chinahuangyong commented 2 years ago

我这边也是,开始的时候会出现内存报错的问题,后面是定位到这个函数,然后加return就好了

q87956256 commented 2 years ago

我这边也是,开始的时候会出现内存报错的问题,后面是定位到这个函数,然后加return就好了

好的,谢谢老哥,我用debug模式运行的时候提示的是boost指针没有初始化,可是我看里面的boost指针明明都初始化了,很想再问下这个加return 跟boost指针有啥关系呢? 你当时是怎么定位到用return就可以解决的呢?

chinahuangyong commented 2 years ago

我用的clion的内存调试工具叫Valgrind,会显示这个函数有问题

chinahuangyong commented 2 years ago

我这边也是,开始的时候会出现内存报错的问题,后面是定位到这个函数,然后加return就好了

好的,谢谢老哥,我用debug模式运行的时候提示的是boost指针没有初始化,可是我看里面的boost指针明明都初始化了,很想再问下这个加return 跟boost指针有啥关系呢? 你当时是怎么定位到用return就可以解决的呢?

不知道你这个目前使用的怎么样啦,貌似作者没有维护了,目前提了这么多issue没人维护了

nhk035 commented 2 years ago

我用的clion的内存调试工具叫Valgrind,会显示这个函数有问题

有个题外的问题想请教一下。clion里面的valgrind怎么调试roslaunch呢