Autoware: ndt_mapping doesn't compute transformation matrix

edit

I tried Autoware v1.13 instead of the latest master, then it works.

This comment is right if others want to use ndt_mapping

I found the solution: replace these codes in original ndt_mapping.cpp between line 1016 and 1046: Since I don't know why the ROS Wiki's page cannot preview the markdown code well... Here is CSDN view link Roll down to the bottom of the page

double tf_x, tf_y, tf_z, tf_roll, tf_pitch, tf_yaw; // 3. Try getting base_link -> lidar TF from tf_* params if (!received_tf) { if (nh.getParam("tf_x", tf_x) && nh.getParam("tf_y", tf_y) && nh.getParam("tf_z", tf_z) && nh.getParam("tf_roll", tf_roll) && nh.getParam("tf_pitch", tf_pitch) && nh.getParam("tf_yaw", tf_yaw)) { tf::Vector3 trans(tf_x, tf_y, tf_z); tf::Quaternion quat; quat.setRPY(tf_roll, tf_pitch, tf_yaw); tf_baselink2primarylidar.setOrigin(trans); tf_baselink2primarylidar.setRotation(quat); received_tf = true; } else { ROS_WARN("Query base_link to primary lidar frame through tf_* params failed"); }

} if (received_tf) { ROS_INFO("base_link to primary lidar transform queried successfully");

tf_baselink2primarylidar.getOrigin();
tf_x = tf_baselink2primarylidar.getOrigin().getX();
tf_y = tf_baselink2primarylidar.getOrigin().getY();
tf_z = tf_baselink2primarylidar.getOrigin().getZ();
Eigen::Translation3f tl_btol(tf_x, tf_y, tf_z);// tl: translation

tf::Matrix3x3(tf_baselink2primarylidar.getRotation()).getRPY( tf_roll, tf_pitch,tf_yaw);
Eigen::AngleAxisf rot_x_btol(tf_roll, Eigen::Vector3f::UnitX());  // rot: rotation
Eigen::AngleAxisf rot_y_btol(tf_pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf rot_z_btol(tf_yaw, Eigen::Vector3f::UnitZ());
tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
tf_ltob = tf_btol.inverse();

}

And the reason is 1.14 didn't update tf_ltob that used in computing transformation matrix.