static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) { pcl::PointXYZI p; pcl::PointCloud<pcl::PointXYZI> scan; pcl::PointCloud<velodyne_pointcloud::PointXYZIR> tmp; sensor_msgs::PointCloud2 filtered_msg; pcl::fromROSMsg(*input, scan); pcl::fromROSMsg(*input, tmp); scan.points.clear(); for (pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::const_iterator item = tmp.begin(); item != tmp.end(); item++) { p.x = (double)item->x; p.y = (double)item->y; p.z = (double)item->z; p.intensity = (double)item->intensity; if (item->ring % ring_div == 0) { scan.points.push_back(p); } if (item->ring > ring_max) { ring_max = item->ring; } } pcl::PointCloud<pcl::PointXYZI>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZI>(scan)); pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>()); // if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL) if (voxel_leaf_size >= 0.1) { // Downsampling the velodyne scan using VoxelGrid filter pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter; voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size); voxel_grid_filter.setInputCloud(scan_ptr); voxel_grid_filter.filter(*filtered_scan_ptr); pcl::toROSMsg(*filtered_scan_ptr, filtered_msg); } else { pcl::toROSMsg(*scan_ptr, filtered_msg); } filtered_points_pub.publish(filtered_msg); points_filter_info_msg.header = input->header; points_filter_info_msg.filter_name = "ring_filter"; points_filter_info_msg.original_points_size = scan.size(); if (voxel_leaf_size >= 0.1) { points_filter_info_msg.filtered_points_size = filtered_scan_ptr->size(); } else { points_filter_info_msg.filtered_points_size = scan_ptr->size(); } points_filter_info_msg.original_ring_size = ring_max; points_filter_info_msg.filtered_ring_size = ring_max / ring_div; points_filter_info_pub.publish(points_filter_info_msg); }
static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) { pcl::PointCloud<pcl::PointXYZI> scan; pcl::fromROSMsg(*input, scan); if(measurement_range != MAX_MEASUREMENT_RANGE){ scan = removePointsByRange(scan, 0, measurement_range); } pcl::PointCloud<pcl::PointXYZI>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZI>(scan)); pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>()); sensor_msgs::PointCloud2 filtered_msg; filter_start = std::chrono::system_clock::now(); // if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL) if (voxel_leaf_size >= 0.1) { // Downsampling the velodyne scan using VoxelGrid filter pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter; voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size); voxel_grid_filter.setInputCloud(scan_ptr); voxel_grid_filter.filter(*filtered_scan_ptr); pcl::toROSMsg(*filtered_scan_ptr, filtered_msg); } else { pcl::toROSMsg(*scan_ptr, filtered_msg); } filter_end = std::chrono::system_clock::now(); filtered_msg.header = input->header; filtered_points_pub.publish(filtered_msg); points_downsampler_info_msg.header = input->header; points_downsampler_info_msg.filter_name = "voxel_grid_filter"; points_downsampler_info_msg.measurement_range = measurement_range; points_downsampler_info_msg.original_points_size = scan.size(); if (voxel_leaf_size >= 0.1) { points_downsampler_info_msg.filtered_points_size = filtered_scan_ptr->size(); } else { points_downsampler_info_msg.filtered_points_size = scan_ptr->size(); } points_downsampler_info_msg.original_ring_size = 0; points_downsampler_info_msg.filtered_ring_size = 0; points_downsampler_info_msg.exe_time = std::chrono::duration_cast<std::chrono::microseconds>(filter_end - filter_start).count() / 1000.0; points_downsampler_info_pub.publish(points_downsampler_info_msg); if(_output_log == true){ if(!ofs){ std::cerr << "Could not open " << filename << "." << std::endl; exit(1); } ofs << points_downsampler_info_msg.header.seq << "," << points_downsampler_info_msg.header.stamp << "," << points_downsampler_info_msg.header.frame_id << "," << points_downsampler_info_msg.filter_name << "," << points_downsampler_info_msg.original_points_size << "," << points_downsampler_info_msg.filtered_points_size << "," << points_downsampler_info_msg.original_ring_size << "," << points_downsampler_info_msg.filtered_ring_size << "," << points_downsampler_info_msg.exe_time << "," << std::endl; } }
static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) { pcl::PointCloud<pcl::PointXYZI> scan; pcl::PointCloud<velodyne_pointcloud::PointXYZIR> tmp; sensor_msgs::PointCloud2 filtered_msg; pcl::fromROSMsg(*input, scan); pcl::fromROSMsg(*input, tmp); filter_start = std::chrono::system_clock::now(); scan.points.clear(); double square_measurement_range = measurement_range*measurement_range; for (pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::const_iterator item = tmp.begin(); item != tmp.end(); item++) { pcl::PointXYZI p; p.x = item->x; p.y = item->y; p.z = item->z; p.intensity = item->intensity; double square_distance = p.x * p.x + p.y * p.y; if (item->ring % ring_div == 0 && square_distance <= square_measurement_range) { scan.points.push_back(p); } if (item->ring > ring_max) { ring_max = item->ring; } } pcl::PointCloud<pcl::PointXYZI>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZI>(scan)); pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>()); // if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL) if (voxel_leaf_size >= 0.1) { // Downsampling the velodyne scan using VoxelGrid filter pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter; voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size); voxel_grid_filter.setInputCloud(scan_ptr); voxel_grid_filter.filter(*filtered_scan_ptr); pcl::toROSMsg(*filtered_scan_ptr, filtered_msg); } else { pcl::toROSMsg(*scan_ptr, filtered_msg); } filter_end = std::chrono::system_clock::now(); filtered_msg.header = input->header; filtered_points_pub.publish(filtered_msg); points_downsampler_info_msg.header = input->header; points_downsampler_info_msg.filter_name = "ring_filter"; points_downsampler_info_msg.measurement_range = measurement_range; points_downsampler_info_msg.original_points_size = scan.size(); if (voxel_leaf_size >= 0.1) { points_downsampler_info_msg.filtered_points_size = filtered_scan_ptr->size(); } else { points_downsampler_info_msg.filtered_points_size = scan_ptr->size(); } points_downsampler_info_msg.original_ring_size = ring_max; points_downsampler_info_msg.filtered_ring_size = ring_max / ring_div; points_downsampler_info_msg.exe_time = std::chrono::duration_cast<std::chrono::microseconds>(filter_end - filter_start).count() / 1000.0; points_downsampler_info_pub.publish(points_downsampler_info_msg); if(_output_log == true){ if(!ofs){ std::cerr << "Could not open " << filename << "." << std::endl; exit(1); } ofs << points_downsampler_info_msg.header.seq << "," << points_downsampler_info_msg.header.stamp << "," << points_downsampler_info_msg.header.frame_id << "," << points_downsampler_info_msg.filter_name << "," << points_downsampler_info_msg.original_points_size << "," << points_downsampler_info_msg.filtered_points_size << "," << points_downsampler_info_msg.original_ring_size << "," << points_downsampler_info_msg.filtered_ring_size << "," << points_downsampler_info_msg.exe_time << "," << std::endl; } }
static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) { if (map_loaded == 1 && init_pos_set == 1) { matching_start = std::chrono::system_clock::now(); static tf::TransformBroadcaster br; tf::Transform transform; tf::Quaternion predict_q, ndt_q, current_q, control_q, localizer_q; pcl::PointXYZ p; pcl::PointCloud<pcl::PointXYZ> scan; current_scan_time = input->header.stamp; pcl::fromROSMsg(*input, scan); if(_localizer == "velodyne"){ pcl::PointCloud<velodyne_pointcloud::PointXYZIR> tmp; pcl::fromROSMsg(*input, tmp); scan.points.clear(); for (pcl::PointCloud<velodyne_pointcloud::PointXYZIR>::const_iterator item = tmp.begin(); item != tmp.end(); item++) { p.x = (double) item->x; p.y = (double) item->y; p.z = (double) item->z; if(item->ring >= min && item->ring <= max && item->ring % layer == 0 ){ scan.points.push_back(p); } } } pcl::PointCloud<pcl::PointXYZ>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(scan)); pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>()); Eigen::Matrix4f t(Eigen::Matrix4f::Identity()); // base_link Eigen::Matrix4f t2(Eigen::Matrix4f::Identity()); // localizer // Downsampling the velodyne scan using VoxelGrid filter pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter; voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size); voxel_grid_filter.setInputCloud(scan_ptr); voxel_grid_filter.filter(*filtered_scan_ptr); // Setting point cloud to be aligned. ndt.setInputSource(filtered_scan_ptr); // Guess the initial gross estimation of the transformation predict_pose.x = previous_pose.x + offset_x; predict_pose.y = previous_pose.y + offset_y; predict_pose.z = previous_pose.z + offset_z; predict_pose.roll = previous_pose.roll; predict_pose.pitch = previous_pose.pitch; predict_pose.yaw = previous_pose.yaw + offset_yaw; Eigen::Translation3f init_translation(predict_pose.x, predict_pose.y, predict_pose.z); Eigen::AngleAxisf init_rotation_x(predict_pose.roll, Eigen::Vector3f::UnitX()); Eigen::AngleAxisf init_rotation_y(predict_pose.pitch, Eigen::Vector3f::UnitY()); Eigen::AngleAxisf init_rotation_z(predict_pose.yaw, Eigen::Vector3f::UnitZ()); Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x) * tf_btol; pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>); ndt.align(*output_cloud, init_guess); t = ndt.getFinalTransformation(); // localizer t2 = t * tf_ltob; // base_link iteration = ndt.getFinalNumIteration(); score = ndt.getFitnessScore(); trans_probability = ndt.getTransformationProbability(); tf::Matrix3x3 mat_l; // localizer mat_l.setValue(static_cast<double>(t(0, 0)), static_cast<double>(t(0, 1)), static_cast<double>(t(0, 2)), static_cast<double>(t(1, 0)), static_cast<double>(t(1, 1)), static_cast<double>(t(1, 2)), static_cast<double>(t(2, 0)), static_cast<double>(t(2, 1)), static_cast<double>(t(2, 2))); // Update ndt_pose localizer_pose.x = t(0, 3); localizer_pose.y = t(1, 3); localizer_pose.z = t(2, 3); mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1); tf::Matrix3x3 mat_b; // base_link mat_b.setValue(static_cast<double>(t2(0, 0)), static_cast<double>(t2(0, 1)), static_cast<double>(t2(0, 2)), static_cast<double>(t2(1, 0)), static_cast<double>(t2(1, 1)), static_cast<double>(t2(1, 2)), static_cast<double>(t2(2, 0)), static_cast<double>(t2(2, 1)), static_cast<double>(t2(2, 2))); // Update ndt_pose ndt_pose.x = t2(0, 3); ndt_pose.y = t2(1, 3); ndt_pose.z = t2(2, 3); mat_b.getRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw, 1); // Compute the velocity scan_duration = current_scan_time - previous_scan_time; double secs = scan_duration.toSec(); double distance = sqrt((ndt_pose.x - previous_pose.x) * (ndt_pose.x - previous_pose.x) + (ndt_pose.y - previous_pose.y) * (ndt_pose.y - previous_pose.y) + (ndt_pose.z - previous_pose.z) * (ndt_pose.z - previous_pose.z)); predict_pose_error = sqrt((ndt_pose.x - predict_pose.x) * (ndt_pose.x - predict_pose.x) + (ndt_pose.y - predict_pose.y) * (ndt_pose.y - predict_pose.y) + (ndt_pose.z - predict_pose.z) * (ndt_pose.z - predict_pose.z)); current_velocity = distance / secs; current_velocity_smooth = (current_velocity + previous_velocity + second_previous_velocity) / 3.0; if(current_velocity_smooth < 0.2){ current_velocity_smooth = 0.0; } current_acceleration = (current_velocity - previous_velocity) / secs; estimated_vel_mps.data = current_velocity; estimated_vel_kmph.data = current_velocity * 3.6; estimated_vel_mps_pub.publish(estimated_vel_mps); estimated_vel_kmph_pub.publish(estimated_vel_kmph); if(predict_pose_error <= PREDICT_POSE_THRESHOLD){ use_predict_pose = 0; }else{ use_predict_pose = 1; } use_predict_pose = 0; if(use_predict_pose == 0){ current_pose.x = ndt_pose.x; current_pose.y = ndt_pose.y; current_pose.z = ndt_pose.z; current_pose.roll = ndt_pose.roll; current_pose.pitch = ndt_pose.pitch; current_pose.yaw = ndt_pose.yaw; }else{ current_pose.x = predict_pose.x; current_pose.y = predict_pose.y; current_pose.z = predict_pose.z; current_pose.roll = predict_pose.roll; current_pose.pitch = predict_pose.pitch; current_pose.yaw = predict_pose.yaw; } control_pose.roll = current_pose.roll; control_pose.pitch = current_pose.pitch; control_pose.yaw = current_pose.yaw - angle / 180.0 * M_PI; double theta = control_pose.yaw; control_pose.x = cos(theta) * (-control_shift_x) + sin(theta) * (-control_shift_y) + current_pose.x; control_pose.y = -sin(theta) * (-control_shift_x) + cos(theta) * (-control_shift_y) + current_pose.y; control_pose.z = current_pose.z - control_shift_z; // Set values for publishing pose predict_q.setRPY(predict_pose.roll, predict_pose.pitch, predict_pose.yaw); predict_pose_msg.header.frame_id = "/map"; predict_pose_msg.header.stamp = current_scan_time; predict_pose_msg.pose.position.x = predict_pose.x; predict_pose_msg.pose.position.y = predict_pose.y; predict_pose_msg.pose.position.z = predict_pose.z; predict_pose_msg.pose.orientation.x = predict_q.x(); predict_pose_msg.pose.orientation.y = predict_q.y(); predict_pose_msg.pose.orientation.z = predict_q.z(); predict_pose_msg.pose.orientation.w = predict_q.w(); ndt_q.setRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw); ndt_pose_msg.header.frame_id = "/map"; ndt_pose_msg.header.stamp = current_scan_time; ndt_pose_msg.pose.position.x = ndt_pose.x; ndt_pose_msg.pose.position.y = ndt_pose.y; ndt_pose_msg.pose.position.z = ndt_pose.z; ndt_pose_msg.pose.orientation.x = ndt_q.x(); ndt_pose_msg.pose.orientation.y = ndt_q.y(); ndt_pose_msg.pose.orientation.z = ndt_q.z(); ndt_pose_msg.pose.orientation.w = ndt_q.w(); current_q.setRPY(current_pose.roll, current_pose.pitch, current_pose.yaw); current_pose_msg.header.frame_id = "/map"; current_pose_msg.header.stamp = current_scan_time; current_pose_msg.pose.position.x = current_pose.x; current_pose_msg.pose.position.y = current_pose.y; current_pose_msg.pose.position.z = current_pose.z; current_pose_msg.pose.orientation.x = current_q.x(); current_pose_msg.pose.orientation.y = current_q.y(); current_pose_msg.pose.orientation.z = current_q.z(); current_pose_msg.pose.orientation.w = current_q.w(); control_q.setRPY(control_pose.roll, control_pose.pitch, control_pose.yaw); control_pose_msg.header.frame_id = "/map"; control_pose_msg.header.stamp = current_scan_time; control_pose_msg.pose.position.x = control_pose.x; control_pose_msg.pose.position.y = control_pose.y; control_pose_msg.pose.position.z = control_pose.z; control_pose_msg.pose.orientation.x = control_q.x(); control_pose_msg.pose.orientation.y = control_q.y(); control_pose_msg.pose.orientation.z = control_q.z(); control_pose_msg.pose.orientation.w = control_q.w(); localizer_q.setRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw); localizer_pose_msg.header.frame_id = "/map"; localizer_pose_msg.header.stamp = current_scan_time; localizer_pose_msg.pose.position.x = localizer_pose.x; localizer_pose_msg.pose.position.y = localizer_pose.y; localizer_pose_msg.pose.position.z = localizer_pose.z; localizer_pose_msg.pose.orientation.x = localizer_q.x(); localizer_pose_msg.pose.orientation.y = localizer_q.y(); localizer_pose_msg.pose.orientation.z = localizer_q.z(); localizer_pose_msg.pose.orientation.w = localizer_q.w(); predict_pose_pub.publish(predict_pose_msg); ndt_pose_pub.publish(ndt_pose_msg); current_pose_pub.publish(current_pose_msg); control_pose_pub.publish(control_pose_msg); localizer_pose_pub.publish(localizer_pose_msg); // Send TF "/base_link" to "/map" transform.setOrigin(tf::Vector3(current_pose.x, current_pose.y, current_pose.z)); transform.setRotation(current_q); br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link")); matching_end = std::chrono::system_clock::now(); exe_time = std::chrono::duration_cast<std::chrono::microseconds>(matching_end-matching_start).count()/1000.0; time_ndt_matching.data = exe_time; time_ndt_matching_pub.publish(time_ndt_matching); // Set values for /estimate_twist angular_velocity = (current_pose.yaw - previous_pose.yaw) / secs; estimate_twist_msg.header.stamp = current_scan_time; estimate_twist_msg.twist.linear.x = current_velocity; estimate_twist_msg.twist.linear.y = 0.0; estimate_twist_msg.twist.linear.z = 0.0; estimate_twist_msg.twist.angular.x = 0.0; estimate_twist_msg.twist.angular.y = 0.0; estimate_twist_msg.twist.angular.z = angular_velocity; estimate_twist_pub.publish(estimate_twist_msg); geometry_msgs::Vector3Stamped estimate_vel_msg; estimate_vel_msg.header.stamp = current_scan_time; estimate_vel_msg.vector.x = current_velocity; estimated_vel_pub.publish(estimate_vel_msg); // Set values for /ndt_stat ndt_stat_msg.header.stamp = current_scan_time; ndt_stat_msg.exe_time = time_ndt_matching.data; ndt_stat_msg.iteration = iteration; ndt_stat_msg.score = score; ndt_stat_msg.velocity = current_velocity; ndt_stat_msg.acceleration = current_acceleration; ndt_stat_msg.use_predict_pose = 0; ndt_stat_pub.publish(ndt_stat_msg); /* Compute NDT_Reliability */ ndt_reliability.data = Wa * (exe_time/100.0) * 100.0 + Wb * (iteration/10.0) * 100.0 + Wc * ((2.0-trans_probability)/2.0) * 100.0; ndt_reliability_pub.publish(ndt_reliability); #ifdef OUTPUT // Output log.csv std::ofstream ofs_log("log.csv", std::ios::app); if (ofs_log == NULL) { std::cerr << "Could not open 'log.csv'." << std::endl; exit(1); } ofs_log << input->header.seq << "," << step_size << "," << trans_eps << "," << voxel_leaf_size << "," << current_pose.x << "," << current_pose.y << "," << current_pose.z << "," << current_pose.roll << "," << current_pose.pitch << "," << current_pose.yaw << "," << predict_pose.x << "," << predict_pose.y << "," << predict_pose.z << "," << predict_pose.roll << "," << predict_pose.pitch << "," << predict_pose.yaw << "," << current_pose.x - predict_pose.x << "," << current_pose.y - predict_pose.y << "," << current_pose.z - predict_pose.z << "," << current_pose.roll - predict_pose.roll << "," << current_pose.pitch - predict_pose.pitch << "," << current_pose.yaw - predict_pose.yaw << "," << predict_pose_error << "," << iteration << "," << score << "," << trans_probability << "," << ndt_reliability.data << "," << current_velocity << "," << current_velocity_smooth << "," << current_acceleration << "," << angular_velocity << "," << time_ndt_matching.data << "," << std::endl; #endif std::cout << "-----------------------------------------------------------------" << std::endl; std::cout << "Sequence: " << input->header.seq << std::endl; std::cout << "Timestamp: " << input->header.stamp << std::endl; std::cout << "Frame ID: " << input->header.frame_id << std::endl; std::cout << "Number of Scan Points: " << scan_ptr->size() << " points." << std::endl; std::cout << "Number of Filtered Scan Points: " << filtered_scan_ptr->size() << " points." << std::endl; std::cout << "Leaf Size: " << voxel_leaf_size << std::endl; std::cout << "NDT has converged: " << ndt.hasConverged() << std::endl; std::cout << "Fitness Score: " << ndt.getFitnessScore() << std::endl; std::cout << "Transformation Probability: " << ndt.getTransformationProbability() << std::endl; std::cout << "Execution Time: " << exe_time << " ms." << std::endl; std::cout << "Number of Iterations: " << ndt.getFinalNumIteration() << std::endl; std::cout << "NDT Reliability: " << ndt_reliability.data << std::endl; std::cout << "(x,y,z,roll,pitch,yaw): " << std::endl; std::cout << "(" << current_pose.x << ", " << current_pose.y << ", " << current_pose.z << ", " << current_pose.roll << ", " << current_pose.pitch << ", " << current_pose.yaw << ")" << std::endl; std::cout << "Transformation Matrix: " << std::endl; std::cout << t << std::endl; std::cout << "-----------------------------------------------------------------" << std::endl; // Update previous_*** offset_x = current_pose.x - previous_pose.x; offset_y = current_pose.y - previous_pose.y; offset_z = current_pose.z - previous_pose.z; offset_yaw = current_pose.yaw - previous_pose.yaw; previous_pose.x = current_pose.x; previous_pose.y = current_pose.y; previous_pose.z = current_pose.z; previous_pose.roll = current_pose.roll; previous_pose.pitch = current_pose.pitch; previous_pose.yaw = current_pose.yaw; previous_scan_time.sec = current_scan_time.sec; previous_scan_time.nsec = current_scan_time.nsec; second_previous_velocity = previous_velocity; previous_velocity = current_velocity; previous_acceleration = current_acceleration; previous_estimated_vel_kmph.data = estimated_vel_kmph.data; } }
static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input) { if (map_loaded == 1 && init_pos_set == 1) { matching_start = std::chrono::system_clock::now(); static tf::TransformBroadcaster br; tf::Transform transform; tf::Quaternion predict_q, icp_q, current_q, localizer_q; pcl::PointXYZ p; pcl::PointCloud<pcl::PointXYZ> filtered_scan; current_scan_time = input->header.stamp; pcl::fromROSMsg(*input, filtered_scan); pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(filtered_scan)); int scan_points_num = filtered_scan_ptr->size(); Eigen::Matrix4f t(Eigen::Matrix4f::Identity()); // base_link Eigen::Matrix4f t2(Eigen::Matrix4f::Identity()); // localizer std::chrono::time_point<std::chrono::system_clock> align_start, align_end, getFitnessScore_start, getFitnessScore_end; static double align_time, getFitnessScore_time = 0.0; // Setting point cloud to be aligned. // ndt.setInputSource(filtered_scan_ptr); icp.setInputSource(filtered_scan_ptr); // Guess the initial gross estimation of the transformation predict_pose.x = previous_pose.x + offset_x; predict_pose.y = previous_pose.y + offset_y; predict_pose.z = previous_pose.z + offset_z; predict_pose.roll = previous_pose.roll; predict_pose.pitch = previous_pose.pitch; predict_pose.yaw = previous_pose.yaw + offset_yaw; Eigen::Translation3f init_translation(predict_pose.x, predict_pose.y, predict_pose.z); Eigen::AngleAxisf init_rotation_x(predict_pose.roll, Eigen::Vector3f::UnitX()); Eigen::AngleAxisf init_rotation_y(predict_pose.pitch, Eigen::Vector3f::UnitY()); Eigen::AngleAxisf init_rotation_z(predict_pose.yaw, Eigen::Vector3f::UnitZ()); Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x) * tf_btol; pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>); // ndt.align(*output_cloud, init_guess); icp.setMaximumIterations(maximum_iterations); icp.setTransformationEpsilon(transformation_epsilon); icp.setMaxCorrespondenceDistance(max_correspondence_distance); icp.setEuclideanFitnessEpsilon(euclidean_fitness_epsilon); icp.setRANSACOutlierRejectionThreshold(ransac_outlier_rejection_threshold); align_start = std::chrono::system_clock::now(); icp.align(*output_cloud, init_guess); align_end = std::chrono::system_clock::now(); align_time = std::chrono::duration_cast<std::chrono::microseconds>(align_end - align_start).count() / 1000.0; // t = ndt.getFinalTransformation(); // localizer t = icp.getFinalTransformation(); // localizer t2 = t * tf_ltob; // base_link // iteration = ndt.getFinalNumIteration(); // score = ndt.getFitnessScore(); getFitnessScore_start = std::chrono::system_clock::now(); fitness_score = icp.getFitnessScore(); getFitnessScore_end = std::chrono::system_clock::now(); getFitnessScore_time = std::chrono::duration_cast<std::chrono::microseconds>(getFitnessScore_end - getFitnessScore_start).count() / 1000.0; // trans_probability = ndt.getTransformationProbability(); tf::Matrix3x3 mat_l; // localizer mat_l.setValue(static_cast<double>(t(0, 0)), static_cast<double>(t(0, 1)), static_cast<double>(t(0, 2)), static_cast<double>(t(1, 0)), static_cast<double>(t(1, 1)), static_cast<double>(t(1, 2)), static_cast<double>(t(2, 0)), static_cast<double>(t(2, 1)), static_cast<double>(t(2, 2))); // Update localizer_pose localizer_pose.x = t(0, 3); localizer_pose.y = t(1, 3); localizer_pose.z = t(2, 3); mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1); tf::Matrix3x3 mat_b; // base_link mat_b.setValue(static_cast<double>(t2(0, 0)), static_cast<double>(t2(0, 1)), static_cast<double>(t2(0, 2)), static_cast<double>(t2(1, 0)), static_cast<double>(t2(1, 1)), static_cast<double>(t2(1, 2)), static_cast<double>(t2(2, 0)), static_cast<double>(t2(2, 1)), static_cast<double>(t2(2, 2))); // Update ndt_pose icp_pose.x = t2(0, 3); icp_pose.y = t2(1, 3); icp_pose.z = t2(2, 3); mat_b.getRPY(icp_pose.roll, icp_pose.pitch, icp_pose.yaw, 1); // Calculate the difference between ndt_pose and predict_pose predict_pose_error = sqrt((icp_pose.x - predict_pose.x) * (icp_pose.x - predict_pose.x) + (icp_pose.y - predict_pose.y) * (icp_pose.y - predict_pose.y) + (icp_pose.z - predict_pose.z) * (icp_pose.z - predict_pose.z)); if (predict_pose_error <= PREDICT_POSE_THRESHOLD) { use_predict_pose = 0; } else { use_predict_pose = 1; } use_predict_pose = 0; if (use_predict_pose == 0) { current_pose.x = icp_pose.x; current_pose.y = icp_pose.y; current_pose.z = icp_pose.z; current_pose.roll = icp_pose.roll; current_pose.pitch = icp_pose.pitch; current_pose.yaw = icp_pose.yaw; } else { current_pose.x = predict_pose.x; current_pose.y = predict_pose.y; current_pose.z = predict_pose.z; current_pose.roll = predict_pose.roll; current_pose.pitch = predict_pose.pitch; current_pose.yaw = predict_pose.yaw; } // Compute the velocity and acceleration scan_duration = current_scan_time - previous_scan_time; double secs = scan_duration.toSec(); diff_x = current_pose.x - previous_pose.x; diff_y = current_pose.y - previous_pose.y; diff_z = current_pose.z - previous_pose.z; diff_yaw = current_pose.yaw - previous_pose.yaw; diff = sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z); current_velocity = diff / secs; current_velocity_x = diff_x / secs; current_velocity_y = diff_y / secs; current_velocity_z = diff_z / secs; angular_velocity = diff_yaw / secs; current_velocity_smooth = (current_velocity + previous_velocity + previous_previous_velocity) / 3.0; if (current_velocity_smooth < 0.2) { current_velocity_smooth = 0.0; } current_accel = (current_velocity - previous_velocity) / secs; current_accel_x = (current_velocity_x - previous_velocity_x) / secs; current_accel_y = (current_velocity_y - previous_velocity_y) / secs; current_accel_z = (current_velocity_z - previous_velocity_z) / secs; estimated_vel_mps.data = current_velocity; estimated_vel_kmph.data = current_velocity * 3.6; estimated_vel_mps_pub.publish(estimated_vel_mps); estimated_vel_kmph_pub.publish(estimated_vel_kmph); // Set values for publishing pose predict_q.setRPY(predict_pose.roll, predict_pose.pitch, predict_pose.yaw); predict_pose_msg.header.frame_id = "/map"; predict_pose_msg.header.stamp = current_scan_time; predict_pose_msg.pose.position.x = predict_pose.x; predict_pose_msg.pose.position.y = predict_pose.y; predict_pose_msg.pose.position.z = predict_pose.z; predict_pose_msg.pose.orientation.x = predict_q.x(); predict_pose_msg.pose.orientation.y = predict_q.y(); predict_pose_msg.pose.orientation.z = predict_q.z(); predict_pose_msg.pose.orientation.w = predict_q.w(); icp_q.setRPY(icp_pose.roll, icp_pose.pitch, icp_pose.yaw); icp_pose_msg.header.frame_id = "/map"; icp_pose_msg.header.stamp = current_scan_time; icp_pose_msg.pose.position.x = icp_pose.x; icp_pose_msg.pose.position.y = icp_pose.y; icp_pose_msg.pose.position.z = icp_pose.z; icp_pose_msg.pose.orientation.x = icp_q.x(); icp_pose_msg.pose.orientation.y = icp_q.y(); icp_pose_msg.pose.orientation.z = icp_q.z(); icp_pose_msg.pose.orientation.w = icp_q.w(); current_q.setRPY(current_pose.roll, current_pose.pitch, current_pose.yaw); current_pose_msg.header.frame_id = "/map"; current_pose_msg.header.stamp = current_scan_time; current_pose_msg.pose.position.x = current_pose.x; current_pose_msg.pose.position.y = current_pose.y; current_pose_msg.pose.position.z = current_pose.z; current_pose_msg.pose.orientation.x = current_q.x(); current_pose_msg.pose.orientation.y = current_q.y(); current_pose_msg.pose.orientation.z = current_q.z(); current_pose_msg.pose.orientation.w = current_q.w(); localizer_q.setRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw); localizer_pose_msg.header.frame_id = "/map"; localizer_pose_msg.header.stamp = current_scan_time; localizer_pose_msg.pose.position.x = localizer_pose.x; localizer_pose_msg.pose.position.y = localizer_pose.y; localizer_pose_msg.pose.position.z = localizer_pose.z; localizer_pose_msg.pose.orientation.x = localizer_q.x(); localizer_pose_msg.pose.orientation.y = localizer_q.y(); localizer_pose_msg.pose.orientation.z = localizer_q.z(); localizer_pose_msg.pose.orientation.w = localizer_q.w(); predict_pose_pub.publish(predict_pose_msg); icp_pose_pub.publish(icp_pose_msg); current_pose_pub.publish(current_pose_msg); localizer_pose_pub.publish(localizer_pose_msg); // Send TF "/base_link" to "/map" transform.setOrigin(tf::Vector3(current_pose.x, current_pose.y, current_pose.z)); transform.setRotation(current_q); br.sendTransform(tf::StampedTransform(transform, current_scan_time, "/map", "/base_link")); matching_end = std::chrono::system_clock::now(); exe_time = std::chrono::duration_cast<std::chrono::microseconds>(matching_end - matching_start).count() / 1000.0; time_icp_matching.data = exe_time; time_icp_matching_pub.publish(time_icp_matching); // Set values for /estimate_twist estimate_twist_msg.header.stamp = current_scan_time; estimate_twist_msg.twist.linear.x = current_velocity; estimate_twist_msg.twist.linear.y = 0.0; estimate_twist_msg.twist.linear.z = 0.0; estimate_twist_msg.twist.angular.x = 0.0; estimate_twist_msg.twist.angular.y = 0.0; estimate_twist_msg.twist.angular.z = angular_velocity; estimate_twist_pub.publish(estimate_twist_msg); geometry_msgs::Vector3Stamped estimate_vel_msg; estimate_vel_msg.header.stamp = current_scan_time; estimate_vel_msg.vector.x = current_velocity; estimated_vel_pub.publish(estimate_vel_msg); // Set values for /icp_stat icp_stat_msg.header.stamp = current_scan_time; icp_stat_msg.exe_time = time_icp_matching.data; // icp_stat_msg.iteration = iteration; icp_stat_msg.score = fitness_score; icp_stat_msg.velocity = current_velocity; icp_stat_msg.acceleration = current_accel; icp_stat_msg.use_predict_pose = 0; icp_stat_pub.publish(icp_stat_msg); // Write log if (!ofs) { std::cerr << "Could not open " << filename << "." << std::endl; exit(1); } ofs << input->header.seq << "," << scan_points_num << "," << current_pose.x << "," << current_pose.y << "," << current_pose.z << "," << current_pose.roll << "," << current_pose.pitch << "," << current_pose.yaw << "," << predict_pose.x << "," << predict_pose.y << "," << predict_pose.z << "," << predict_pose.roll << "," << predict_pose.pitch << "," << predict_pose.yaw << "," << current_pose.x - predict_pose.x << "," << current_pose.y - predict_pose.y << "," << current_pose.z - predict_pose.z << "," << current_pose.roll - predict_pose.roll << "," << current_pose.pitch - predict_pose.pitch << "," << current_pose.yaw - predict_pose.yaw << "," << predict_pose_error << "," << "," << fitness_score << "," << "," << current_velocity << "," << current_velocity_smooth << "," << current_accel << "," << angular_velocity << "," << exe_time << "," << align_time << "," << getFitnessScore_time << std::endl; std::cout << "-----------------------------------------------------------------" << std::endl; std::cout << "Sequence: " << input->header.seq << std::endl; std::cout << "Timestamp: " << input->header.stamp << std::endl; std::cout << "Frame ID: " << input->header.frame_id << std::endl; // std::cout << "Number of Scan Points: " << scan_ptr->size() << " points." << std::endl; std::cout << "Number of Filtered Scan Points: " << scan_points_num << " points." << std::endl; std::cout << "ICP has converged: " << icp.hasConverged() << std::endl; std::cout << "Fitness Score: " << fitness_score << std::endl; // std::cout << "Transformation Probability: " << ndt.getTransformationProbability() << std::endl; std::cout << "Execution Time: " << exe_time << " ms." << std::endl; // std::cout << "Number of Iterations: " << ndt.getFinalNumIteration() << std::endl; // std::cout << "NDT Reliability: " << ndt_reliability.data << std::endl; std::cout << "(x,y,z,roll,pitch,yaw): " << std::endl; std::cout << "(" << current_pose.x << ", " << current_pose.y << ", " << current_pose.z << ", " << current_pose.roll << ", " << current_pose.pitch << ", " << current_pose.yaw << ")" << std::endl; std::cout << "Transformation Matrix: " << std::endl; std::cout << t << std::endl; std::cout << "-----------------------------------------------------------------" << std::endl; // Update offset if (_offset == "linear") { offset_x = diff_x; offset_y = diff_y; offset_z = diff_z; offset_yaw = diff_yaw; } else if (_offset == "quadratic") { offset_x = (current_velocity_x + current_accel_x * secs) * secs; offset_y = (current_velocity_y + current_accel_y * secs) * secs; offset_z = diff_z; offset_yaw = diff_yaw; } else if (_offset == "zero") { offset_x = 0.0; offset_y = 0.0; offset_z = 0.0; offset_yaw = 0.0; } // Update previous_*** previous_pose.x = current_pose.x; previous_pose.y = current_pose.y; previous_pose.z = current_pose.z; previous_pose.roll = current_pose.roll; previous_pose.pitch = current_pose.pitch; previous_pose.yaw = current_pose.yaw; previous_scan_time.sec = current_scan_time.sec; previous_scan_time.nsec = current_scan_time.nsec; previous_previous_velocity = previous_velocity; previous_velocity = current_velocity; previous_velocity_x = current_velocity_x; previous_velocity_y = current_velocity_y; previous_velocity_z = current_velocity_z; previous_accel = current_accel; previous_estimated_vel_kmph.data = estimated_vel_kmph.data; } }
static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input) { pcl::PointXYZI sampled_p; pcl::PointCloud<pcl::PointXYZI> scan; pcl::fromROSMsg(*input, scan); if(measurement_range != MAX_MEASUREMENT_RANGE){ scan = removePointsByRange(scan, 0, measurement_range); } pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>()); filtered_scan_ptr->header = scan.header; int points_num = scan.size(); double w_total = 0.0; double w_step = 0.0; int m = 0; double c = 0.0; filter_start = std::chrono::system_clock::now(); for (pcl::PointCloud<pcl::PointXYZI>::const_iterator item = scan.begin(); item != scan.end(); item++) { w_total += item->x * item->x + item->y * item->y + item->z * item->z; } w_step = w_total / sample_num; pcl::PointCloud<pcl::PointXYZI>::const_iterator item = scan.begin(); for (m = 0; m < sample_num; m++) { while (m * w_step > c) { item++; c += item->x * item->x + item->y * item->y + item->z * item->z; } sampled_p.x = item->x; sampled_p.y = item->y; sampled_p.z = item->z; sampled_p.intensity = item->intensity; filtered_scan_ptr->points.push_back(sampled_p); } sensor_msgs::PointCloud2 filtered_msg; pcl::toROSMsg(*filtered_scan_ptr, filtered_msg); filter_end = std::chrono::system_clock::now(); filtered_msg.header = input->header; filtered_points_pub.publish(filtered_msg); points_downsampler_info_msg.header = input->header; points_downsampler_info_msg.filter_name = "distance_filter"; points_downsampler_info_msg.measurement_range = measurement_range; points_downsampler_info_msg.original_points_size = points_num; points_downsampler_info_msg.filtered_points_size = std::min((int)filtered_scan_ptr->size(), points_num); points_downsampler_info_msg.original_ring_size = 0; points_downsampler_info_msg.original_ring_size = 0; points_downsampler_info_msg.exe_time = std::chrono::duration_cast<std::chrono::microseconds>(filter_end - filter_start).count() / 1000.0; points_downsampler_info_pub.publish(points_downsampler_info_msg); if(_output_log == true){ if(!ofs){ std::cerr << "Could not open " << filename << "." << std::endl; exit(1); } ofs << points_downsampler_info_msg.header.seq << "," << points_downsampler_info_msg.header.stamp << "," << points_downsampler_info_msg.header.frame_id << "," << points_downsampler_info_msg.filter_name << "," << points_downsampler_info_msg.original_points_size << "," << points_downsampler_info_msg.filtered_points_size << "," << points_downsampler_info_msg.original_ring_size << "," << points_downsampler_info_msg.filtered_ring_size << "," << points_downsampler_info_msg.exe_time << "," << std::endl; } }