RS_LiDAR rewrite problems and Initialization failed

[Richardson-Chong]

Based on authors suggets and rs_16 manual, I have rewritten vlp_common.h as follows:

`void unpack_scan_rs(const rslidar_msgs::rslidarScan::ConstPtr &lidarMsg, TPointCloud &outPointCloud) const{ outPointCloud.clear(); outPointCloud.header = pcl_conversions::toPCL(lidarMsg->header); if (m_modelType == ModelType::RS_16) { outPointCloud.height = 16; outPointCloud.width = 24(int)lidarMsg->packets.size(); outPointCloud.is_dense = false; outPointCloud.resize(outPointCloud.height outPointCloud.width); }

int block_counter = 0;

double scan_timestamp = lidarMsg->header.stamp.toSec();

for (size_t i = 0; i < lidarMsg->packets.size(); ++i) {
  float azimuth;
  float azimuth_diff;
  float last_azimuth_diff=0;
  float azimuth_corrected_f;
  int azimuth_corrected;
  float x, y, z;

  const raw_packet_t *raw = (const raw_packet_t *) &lidarMsg->packets[i].data[0];

  for (int block = 0; block < BLOCKS_PER_PACKET; block++, block_counter++) {
    // Calculate difference between current and next block's azimuth angle.
    azimuth = (float)(raw->blocks[block].rotation);

    if (block < (BLOCKS_PER_PACKET-1)){
      azimuth_diff = (float)((36000 + raw->blocks[block+1].rotation - raw->blocks[block].rotation)%36000);
      last_azimuth_diff = azimuth_diff;
    }
    else {
      azimuth_diff = last_azimuth_diff; //11th and 0th
    }

    for (int firing=0, k=0; firing < FIRINGS_PER_BLOCK; firing++) {
      for (int dsr=0; dsr < SCANS_PER_FIRING; dsr++, k += RAW_SCAN_SIZE) {

        /** Position Calculation */
        union two_bytes tmp;
        tmp.bytes[0] = raw->blocks[block].data[k];
        tmp.bytes[1] = raw->blocks[block].data[k+1];

        /** correct for the laser rotation as a function of timing during the firings **/
        azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr*DSR_TOFFSET) + (firing*FIRING_TOFFSET)) / BLOCK_TDURATION);
        azimuth_corrected = ((int)round(azimuth_corrected_f)) % 36000;

        /*condition added to avoid calculating points which are not
      in the interesting defined area (min_angle < area < max_angle)*/
        if ((azimuth_corrected >= m_config.min_angle
             && azimuth_corrected <= m_config.max_angle
             && m_config.min_angle < m_config.max_angle)
            || (m_config.min_angle > m_config.max_angle
                && (azimuth_corrected <= m_config.max_angle
                    || azimuth_corrected >= m_config.min_angle))) {
          // convert polar coordinates to Euclidean XYZ
          float distance = tmp.uint * DISTANCE_RESOLUTION;

          float cos_vert_angle = cos_vert_angle_[dsr];
          float sin_vert_angle = sin_vert_angle_[dsr];

          float cos_rot_angle = cos_rot_table_[azimuth_corrected];
          float sin_rot_angle = sin_rot_table_[azimuth_corrected];

          x = distance * cos_vert_angle * sin_rot_angle;
          y = distance * cos_vert_angle * cos_rot_angle;
          z = distance * sin_vert_angle;

          /** Use standard ROS coordinate system (right-hand rule) */
          float x_coord = y;
          float y_coord = -x;
          float z_coord = z;

          float intensity = raw->blocks[block].data[k+2];  // 反射率
          double point_timestamp = scan_timestamp + getExactTime(scan_mapping_16[dsr], 2*block_counter+firing);

          TPoint point;
          point.timestamp = point_timestamp;
          if (pointInRange(distance)) {
            point.x = x_coord;
            point.y = y_coord;
            point.z = z_coord;
            point.intensity = intensity;
          } else {
            point.x = NAN;
            point.y = NAN;
            point.z = NAN;
            point.intensity = 0;
          }
          if(m_modelType == ModelType::RS_16)
            outPointCloud.at(2*block_counter+firing, scan_mapping_16[dsr]) = point;
        }
      }
    }
  }
}

}`

`private: void setParameters(ModelType modelType) { m_modelType = modelType; m_config.max_range = 200; m_config.min_range = 0.2; m_config.min_angle = 0; m_config.max_angle = 36000; // Set up cached values for sin and cos of all the possible headings for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index); cos_rottable[rot_index] = cosf(rotation); sin_rottable[rot_index] = sinf(rotation); }

//finished
//MSOP packet has 12 blocks, each block has 2 groups of 16-line LiDAR data
if (modelType == RS_16) {
  FIRINGS_PER_BLOCK =   2;
  SCANS_PER_FIRING  =  16;
  BLOCK_TDURATION   = 100.0f;   // [µs]
  DSR_TOFFSET       =   3.0f;   // [µs]
  FIRING_TOFFSET    =  50.0f;   // [µs]
  PACKET_TIME = (BLOCKS_PER_PACKET*2*FIRING_TOFFSET);

  //Not_finished
  float vert_correction[16] = {
         -0.2617993877991494,//1
         -0.22689280275926285,//2
         -0.19198621771937624,//3
         -0.15707963267948966,
         -0.12217304763960307,
         -0.08726646259971647,
         -0.05235987755982989,
         -0.017453292519943295,
          0.2617993877991494,//9
         0.22689280275926285,
         0.19198621771937624,
         0.15707963267948966,
         0.12217304763960307,
         0.08726646259971647,
         0.05235987755982989,
         0.017453292519943295,
  };
  for(int i = 0; i < 16; i++) {
    cos_vert_angle_[i] = std::cos(vert_correction[i]);
    sin_vert_angle_[i] = std::sin(vert_correction[i]);
  }
  scan_mapping_16[0]=0;
  scan_mapping_16[1]=1;
  scan_mapping_16[2]=2;
  scan_mapping_16[3]=3;
  scan_mapping_16[4]=4;
  scan_mapping_16[5]=5;
  scan_mapping_16[6]=6;
  scan_mapping_16[7]=7;
  scan_mapping_16[8]=8;
  scan_mapping_16[9]=9;
  scan_mapping_16[10]=10;
  scan_mapping_16[11]=11;
  scan_mapping_16[12]=12;
  scan_mapping_16[13]=13;
  scan_mapping_16[14]=14;
  scan_mapping_16[15]=15;

  for(unsigned int w = 0; w < 1824; w++) {
    for(unsigned int h = 0; h < 16; h++) {
      mVLP16TimeBlock[w][h] = h * 3 * 1e-6 + w * 50 * 1e-6; 
    }
  }
}

}`

And the code runs with some problems. I don't know whether caused by my wrong code or other kind of things. In Initialization, after optimization with data collected from a car platform, the terminal shows [pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow. and Initialization failed. I show my rewritten code and desirely hope someone will check it. And sincerely thank for autorths' sharing!

[Richardson-Chong]

And I really want to know that how to get 1824 in width.

[Richardson-Chong]

And after push the bottom "Initialization",the node died as follows: Ceres Solver Report: Iterations: 12, Initial cost: 1.179723e+07, Final cost: 1.305846e+02, Termination: CONVERGENCE [li_calib_gui-2] process has died [pid 15957, exit code -11, cmd /home/swayrich/ROS_WS/catkin_li_calib/devel/lib/li_calib/li_calib_gui name:=li_calib_gui log:=/home/swayrich/.ros/log/1b3ee1c0-f12a-11eb-bc20-d46a6ac4229d/li_calib_gui-2.log]. log file: /home/swayrich/.ros/log/1b3ee1c0-f12a-11eb-bc20-d46a6ac4229d/li_calib_gui-2*.log

[FishInWave]

And after push the bottom "Initialization",the node died as follows: Ceres Solver Report: Iterations: 12, Initial cost: 1.179723e+07, Final cost: 1.305846e+02, Termination: CONVERGENCE [li_calib_gui-2] process has died [pid 15957, exit code -11, cmd /home/swayrich/ROS_WS/catkin_li_calib/devel/lib/li_calib/li_calib_gui name:=li_calib_gui log:=/home/swayrich/.ros/log/1b3ee1c0-f12a-11eb-bc20-d46a6ac4229d/li_calib_gui-2.log]. log file: /home/swayrich/.ros/log/1b3ee1c0-f12a-11eb-bc20-d46a6ac4229d/li_calib_gui-2*.log

Hi, Have you solved this problem?

[Richardson-Chong]

Unsolved... And the project dies again and again mightly caused by pcl or ndt? I've reinstall the system and nothing changed. Have you figured out with your own lidar?

[FishInWave]

Unsolved... And the project dies again and again mightly caused by pcl or ndt? I've reinstall the system and nothing changed. Have you figured out with your own lidar?

Unluckily, not yet. I'll let you know if there's any progress.

[Richardson-Chong]

Thx for your help!

[msh003]

Could you solve the proble? I face the error

Ceres Solver Report: Iterations: 2, Initial cost: 3.920000e-06, Final cost: 1.010307e-14, Termination: CONVERGENCE
[ WARN] [1662137064.943109275]: [Initialization] fails.

[narayanlongani]

Same error, After pressing the initialization .... tried many times

Load dataset from /home/a/Downloads/bfiles/Garage-01.bag /velodyne_packets: 341 /imu1/data: 14033 Ceres Solver Report: Iterations: 12, Initial cost: 1.179887e+07, Final cost: 3.787628e+02, Termination: CONVERGENCE

[li_calib_gui-2] process has died [pid 6984, exit code -11, cmd /home/a/april_lidar_imu_calib/devel/lib/li_calib/li_calib_gui name:=li_calib_gui log:=/home/a/.ros/log/2dc4a8cc-a694-11ed-8568-88aedd1681ad/li_calib_gui-2.log]. log file: /home/a/.ros/log/2dc4a8cc-a694-11ed-8568-88aedd1681ad/li_calib_gui-2*.log

Can anyone guide on this issue?