LOAM Velodyne terminates abnormally on LOAM running due to TF_NAN_INPUT and TF_DENORMALIZED errors.

[HrkSt1234]

I got LiDAR data (PCAP file) using Velodyne's VLP16. The following error occurs on LOAM running. Why do the following errors occur ? Is there a solution ?

roslaunch terminal

[pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow. ....... [ERROR] [1536296492.318661477]: Ignoring transform for child_frame_id "aft_mapped" from authority "unknown_publisher" because of an invalid quaternion in the transform (nan nan nan nan) [ERROR] [1536296492.396192411]: Ignoring transform for child_frame_id "camera" from authority "unknown_publisher" because of a nan value in the transform (-nan -nan -nan) (nan nan nan nan) [ERROR] [1536296492.396217507]: Ignoring transform for child_frame_id "camera" from authority "unknown_publisher" because of an invalid quaternion in the transform (nan nan nan nan) .......

rosrun tf tf_echo terminal

Error: TF_NAN_INPUT: Ignoring transform for child_frame_id "aft_mapped" from authority "unknown_publisher" because of a nan value in the transform (-nan -nan -nan) (nan nan nan nan) at line 244 in /tmp/binarydeb/ros-kinetic-tf2-0.5.18/src/buffer_core.cpp Error: TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "aft_mapped" from authority "unknown_publisher" because of an invalid quaternion in the transform (nan nan nan nan) at line 257 in /tmp/binarydeb/ros-kinetic-tf2-0.5.18/src/buffer_core.cpp Error: TF_NAN_INPUT: Ignoring transform for child_frame_id "camera" from authority "unknown_publisher" because of a nan value in the transform (-nan -nan -nan) (nan nan nan nan) at line 244 in /tmp/binarydeb/ros-kinetic-tf2-0.5.18/src/buffer_core.cpp Error: TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "camera" from authority "unknown_publisher" because of an invalid quaternion in the transform (nan nan nan nan) at line 257 in /tmp/binarydeb/ros-kinetic-tf2-0.5.18/src/buffer_core.cpp .......

[Allen0]

Hi, I met the same error and found the reason. In MultiScanRegistration::process(), before computing startOri, you must remove NAN point in the input point cloud, since the first point might be NAN. Use pcl::removeNaNFromPointCloud(), the problem is fixed. Hope will help you

[HrkSt1234]

Thanks for your comment. Can you publish your source code about MultiScanRegistration::process() ? I would like to try it out, though I don't know whether to solve the problem because the timing at which an error occurs is random.

[Allen0]

void MultiScanRegistration::process(const pcl::PointCloud& laserCloudIn, const Time& scanTime) { pcl::PointCloud laserCloudOut; std::vector indices; pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudOut, indices); size_t cloudSize = laserCloudOut.size(); // determine scan start and end orientations float startOri = -std::atan2(laserCloudOut[0].y, laserCloudOut[0].x); float endOri = -std::atan2(laserCloudOut[cloudSize - 1].y, laserCloudOut[cloudSize - 1].x) + 2 float(M_PI); if (endOri - startOri > 3 M_PI) { endOri -= 2 M_PI; } else if (endOri - startOri < M_PI) { endOri += 2 M_PI; }

bool halfPassed = false; pcl::PointXYZI point; _laserCloudScans.resize(_scanMapper.getNumberOfScanRings()); // clear all scanline points std::for_each(_laserCloudScans.begin(), _laserCloudScans.end(), {v.clear(); });

// extract valid points from input cloud for (int i = 0; i < cloudSize; i++) { point.x = laserCloudOut[i].y; point.y = laserCloudOut[i].z; point.z = laserCloudOut[i].x;

// skip NaN and INF valued points
if (!pcl_isfinite(point.x) ||
    !pcl_isfinite(point.y) ||
    !pcl_isfinite(point.z)) {
  continue;
}

// skip zero valued points
if (point.x * point.x + point.y * point.y + point.z * point.z < 0.0001) {
  continue;
}

// calculate vertical point angle and scan ID
float angle = std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z));
int scanID = _scanMapper.getRingForAngle(angle);
if (scanID >= _scanMapper.getNumberOfScanRings() || scanID < 0 ){
  continue;
}

// calculate horizontal point angle
float ori = -std::atan2(point.x, point.z);
if (!halfPassed) {
  if (ori < startOri - M_PI / 2) {
    ori += 2 * M_PI;
  } else if (ori > startOri + M_PI * 3 / 2) {
    ori -= 2 * M_PI;
  }

  if (ori - startOri > M_PI) {
    halfPassed = true;
  }
} else {
  ori += 2 * M_PI;

  if (ori < endOri - M_PI * 3 / 2) {
    ori += 2 * M_PI;
  } else if (ori > endOri + M_PI / 2) {
    ori -= 2 * M_PI;
  }
}

// calculate relative scan time based on point orientation
float relTime = config().scanPeriod * (ori - startOri) / (endOri - startOri);
point.intensity = scanID + relTime;

projectPointToStartOfSweep(point, relTime);

_laserCloudScans[scanID].push_back(point);

}

processScanlines(scanTime, _laserCloudScans); publishResult(); }

[HrkSt1234]

I ran LOAM using above source code, but the same error occurred. I continue debugging.

[AlexKaravaev]

@HrkSt1234 Hi! Have you made any progress on that error? I am struggling with it really hard.