void initializeICP() { gicp.setMaximumIterations(50); gicp.setTransformationEpsilon(1e-5); gicp.setRotationEpsilon(1e-5); gicp.setMaxCorrespondenceDistance(0.1 /* 0.1 */); gicp.setEuclideanFitnessEpsilon(0.1); }
void initializeICP() { gicp.setMaximumIterations(2000); gicp.setTransformationEpsilon(1e-5); gicp.setRotationEpsilon(1e-5); gicp.setMaxCorrespondenceDistance(10.0); }
void initialize_icp() { // gicp.setMaximumIterations(2000); gicp.setMaximumIterations(100); gicp.setTransformationEpsilon(1e-8); gicp.setRotationEpsilon(1e-8); // gicp.setMaxCorrespondenceDistance(1.0); gicp.setMaxCorrespondenceDistance(2.0); }
void setup_gicp() { // Set the max correspondence distance gicp.setMaxCorrespondenceDistance (10); // Set the maximum number of iterations (criterion 1) gicp.setMaximumIterations (10); // Set the transformation epsilon (criterion 2) gicp.setTransformationEpsilon (1e-6); // Set the euclidean distance difference epsilon (criterion 3) gicp.setEuclideanFitnessEpsilon (1e-6); gicp.setRANSACIterations(0); }
int perform_icp(pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_pointcloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_pointcloud, Eigen::Matrix<double, 4, 4> *correction, pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_pointcloud2) { pcl::PointCloud<pcl::PointXYZRGB> out_pcl_pointcloud; pcl::PointCloud<pcl::PointXYZRGB> out_pcl_pointcloud_transformed; pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_filtered = GridFiltering(source_pointcloud, 0.2); pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_filtered = GridFiltering(target_pointcloud, 0.2); gicp.setInputCloud(source_filtered); gicp.setInputTarget(target_filtered); gicp.align(out_pcl_pointcloud); (*correction) = gicp.getFinalTransformation().cast<double>(); pcl::transformPointCloud(out_pcl_pointcloud, out_pcl_pointcloud_transformed, (*correction)); //save_clouds_for_debug(*source_pointcloud, *target_pointcloud, out_pcl_pointcloud_transformed, *source_pointcloud2); if(gicp.hasConverged()) { return 1; } return 0; }
void laser_callback(const global_matching::GlocCloud& glocCloud) { //TODO: Map needs to be centered around with starting point at (0,0) #ifdef ONLY_ONCE if(glocCloud.id > 1) { stop_matching = true; return; } #endif if(glocCloud.id <= node_index) { ROS_DEBUG_STREAM("Skipping node: " << glocCloud.id); return; } ROS_DEBUG_STREAM("Processing node: " << glocCloud.id); bool match_found = false; pcl::fromROSMsg(glocCloud.cloud, *cloud_in); #ifdef VOXEL_FILTER sor.setInputCloud (cloud_in); sor.filter(*cloud_in_filt); #else cloud_in_filt = cloud_in; cloud_map_filt = cloud_map; #endif float cur_x = glocCloud.pose.pose.position.x; float cur_y = glocCloud.pose.pose.position.y; float cur_yaw = tf::getYaw(glocCloud.pose.pose.orientation); //LOOP FOR RANDOM GUESSES for(int i = 0; i < ITERATIONS; i++) { //ROS_DEBUG_STREAM("Itertation " << i); //Give random initial conditions float dx,dy,dth; if(i == 0) { //try current pose first dx = cur_x; dy = cur_y; dth = cur_yaw; } else { dx = (float)rand()/RAND_MAX*2*GUESS_DIST_RNG-GUESS_DIST_RNG + cur_x; dy = (float)rand()/RAND_MAX*2*GUESS_DIST_RNG-GUESS_DIST_RNG + cur_y; dth = (float)rand()/RAND_MAX*GUESS_ANGLE_RNG; } trans_init << cos(dth) , -sin(dth), 0, dx, sin(dth), cos(dth), 0, dy, 0, 0, 1, 0, 0, 0, 0, 1; pcl::transformPointCloud(*cloud_in_filt, *cloud_in_trans, trans_init); //Calculate gicp gicp.setInputCloud(cloud_in_trans); gicp.setInputTarget(cloud_map_filt); gicp.align(*cloud_final); float score = gicp.getFitnessScore(); float normScore = score / cloud_in_trans->size(); ROS_DEBUG_STREAM(" Score: " << score << " : " << normScore); transformation = gicp.getFinalTransformation(); trans_init = transformation*trans_init; pcl::transformPointCloud(*cloud_in, *cloud_final, trans_init); pcl::toROSMsg(*cloud_map_filt,ros_map); //pcl::toROSMsg(*cloud_in_trans,ros_guess); pcl::toROSMsg(*cloud_final,ros_match); ros_map.header.frame_id = "/nasa"; ros_guess.header.frame_id = "/nasa"; ros_match.header.frame_id = "/nasa"; pub_map.publish(ros_map); //pub_guess.publish(ros_guess); pub_match.publish(ros_match); if(normScore < MAX_SCORE) { ROS_INFO_STREAM("Global Match successful"); ROS_DEBUG_STREAM("initial: " << trans_init); //refine match gicp.setInputCloud(cloud_final); gicp.setInputTarget(cloud_map); gicp.align(*cloud_refined); transformation = gicp.getFinalTransformation(); trans_init = transformation*trans_init; ROS_DEBUG_STREAM("refined: " << trans_init); pcl::transformPointCloud(*cloud_in, *cloud_refined, trans_init); pcl::toROSMsg(*cloud_refined,ros_guess); pub_guess.publish(ros_guess); match_found = true; break; } }//END RANDOM GUESSES if(match_found) { //publish edge try_count = 0; double yaw,pitch,roll; graph_slam::Edge gEdge; gEdge.delta.x = trans_init(0,3); gEdge.delta.y = trans_init(1,3); tf::Matrix3x3 mat; mat[0][0] = trans_init(0,0); mat[0][1] = trans_init(0,1); mat[0][2] = trans_init(0,2); mat[1][0] = trans_init(1,0); mat[1][1] = trans_init(0,1); mat[1][2] = trans_init(1,2); mat[2][0] = trans_init(2,0); mat[2][1] = trans_init(0,1); mat[2][2] = trans_init(2,2); mat.getEulerYPR(yaw, pitch, roll,1); gEdge.delta.theta = yaw; gEdge.to = glocCloud.id; gEdge.from = 0; gEdge.covariance[0] = COV_XY; gEdge.covariance[1] = 0; gEdge.covariance[2] = 0; gEdge.covariance[3] = 0; gEdge.covariance[4] = COV_XY; gEdge.covariance[5] = 0; gEdge.covariance[6] = 0; gEdge.covariance[7] = 0; gEdge.covariance[8] = COV_YAW; edgePub.publish(gEdge); node_index = glocCloud.id; } else { try_count++; ROS_DEBUG_STREAM("No match found :("); } if(try_count >= MAX_TRIES) { //matching failed return initial guess ROS_WARN_STREAM(""); graph_slam::Edge gEdge; gEdge.delta.x = glocCloud.pose.pose.position.x; gEdge.delta.y = glocCloud.pose.pose.position.y; gEdge.delta.theta = tf::getYaw(glocCloud.pose.pose.orientation); gEdge.to = glocCloud.id; gEdge.from = 0; gEdge.covariance[0] = COV_XY; gEdge.covariance[1] = 0; gEdge.covariance[2] = 0; gEdge.covariance[3] = 0; gEdge.covariance[4] = COV_XY; gEdge.covariance[5] = 0; gEdge.covariance[6] = 0; gEdge.covariance[7] = 0; gEdge.covariance[8] = COV_YAW; edgePub.publish(gEdge); node_index = glocCloud.id; } }
int runGeneralizedICP(pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_pointcloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_pointcloud, Eigen::Matrix<double, 4, 4> source_pointcloud_pose, Eigen::Matrix<double, 4, 4> target_pointcloud_pose, Eigen::Matrix<double, 4, 4> *out_pose, int current_index, int before_index) { static int is_first_icp_run = 1; Eigen::Matrix<double, 4, 4> out_transform; Eigen::Matrix<double, 4, 4> src_transform; Eigen::Matrix<double, 4, 4> final_transform; /**************************************/ /********** Initializations ***********/ /**************************************/ pcl::PointCloud<pcl::PointXYZRGB>::Ptr src_pcl_pointcloud_corrected = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> >( new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr tgt_pcl_pointcloud_corrected = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> >( new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud < pcl::PointXYZRGB > out_pcl_pointcloud; pcl::PointCloud < pcl::PointXYZRGB > out_pcl_pointcloud_transformed; /**************************************/ /**************** GICP ****************/ /**************************************/ // if (is_first_icp_run) { // accumulated_correction_transform = // Eigen::Matrix<double, 4, 4>().Identity(); // is_first_icp_run = 0; // } // source_pointcloud_pose = accumulated_correction_transform // * source_pointcloud_pose; // target_pointcloud_pose = accumulated_correction_transform // * target_pointcloud_pose; pcl::transformPointCloud(*source_pointcloud, *src_pcl_pointcloud_corrected, (target_pointcloud_pose.inverse() * source_pointcloud_pose).cast<float>()); pcl::transformPointCloud(*target_pointcloud, *tgt_pcl_pointcloud_corrected, Eigen::Matrix<float, 4, 4>().Identity()); gicp.setInputCloud(src_pcl_pointcloud_corrected); gicp.setInputTarget(tgt_pcl_pointcloud_corrected); gicp.align(out_pcl_pointcloud); out_transform = //target_pointcloud_pose * gicp.getFinalTransformation().cast<double>(); // * target_pointcloud_pose.inverse(); //*corrected_pose = final_transform = source_pointcloud_pose; src_pcl_pointcloud_corrected->clear(); tgt_pcl_pointcloud_corrected->clear(); if (gicp.hasConverged()) { *out_pose = out_transform; return 1; } return 0; }