//TODO: Merge this with processNodePair MatchingResult Node::matchNodePair(const Node* older_node){ MatchingResult mr; const unsigned int min_matches = 16; // minimal number of feature correspondences to be a valid candidate for a link // std::clock_t starttime=std::clock(); this->findPairsFlann(older_node, &mr.all_matches); ROS_DEBUG("found %i inital matches",(int) mr.all_matches.size()); if (mr.all_matches.size() < min_matches){ ROS_INFO("Too few inliers: Adding no Edge between %i and %i. Only %i correspondences to begin with.", older_node->id_,this->id_,(int)mr.all_matches.size()); } else { if (!getRelativeTransformationTo(older_node,&mr.all_matches, mr.ransac_trafo, mr.rmse, mr.inlier_matches) ){ // mr.all_matches.size()/3 ROS_INFO("Found no valid trafo, but had initially %d feature matches",(int) mr.all_matches.size()); } else { mr.final_trafo = mr.ransac_trafo; #ifdef USE_ICP_CODE getRelativeTransformationTo_ICP_code(older_node,mr.icp_trafo, &mr.ransac_trafo); #endif #ifdef USE_ICP_BIN // improve transformation by using the generalized ICP // std::clock_t starttime_gicp1 = std::clock(); bool converged = getRelativeTransformationTo_ICP_bin(older_node,mr.icp_trafo, &mr.ransac_trafo); //ROS_INFO_STREAM("Paper: ICP1: " << ((std::clock()-starttime_gicp1*1.0) / (double)CLOCKS_PER_SEC)); ROS_INFO("icp: inliers: %i", mr.inlier_matches.size()); if (!converged) { ROS_INFO("ICP did not converge. No Edge added"); return mr; } mr.final_trafo = mr.ransac_trafo * mr.icp_trafo; MatchingResult mr_icp; vector<double> errors; double error; std::vector<cv::DMatch> inliers; // check if icp improves alignment: computeInliersAndError(mr.inlier_matches, mr.final_trafo, this->feature_locations_3d_, older_node->feature_locations_3d_, inliers, error, errors, 0.04*0.04); for (uint i=0; i<errors.size(); i++) { cout << "error: " << round(errors[i]*10000)/100 << endl; } cout << "error was: " << mr.rmse << " and is now: " << error << endl; double roll, pitch, yaw, dist; mat2components(mr.ransac_trafo, roll, pitch, yaw, dist); cout << "ransac: " << roll << " "<< pitch << " "<< yaw << " "<< dist << endl; mat2components(mr.icp_trafo, roll, pitch, yaw, dist); cout << "icp: " << roll << " "<< pitch << " "<< yaw << " "<< dist << endl; mat2components(mr.final_trafo, roll, pitch, yaw, dist); cout << "final: " << roll << " "<< pitch << " "<< yaw << " "<< dist << endl; cout << "ransac: " << endl << mr.ransac_trafo << endl; cout << "icp: " << endl << mr.icp_trafo << endl; cout << "total: " << endl << mr.final_trafo << endl; if (error > (mr.rmse+0.02)) { cout << "#### icp-error is too large, ignoring the connection" << endl; return mr; } #endif #ifndef USE_ICP_BIN #ifndef USE_ICP_CODE mr.final_trafo = mr.ransac_trafo; #endif #endif mr.edge.id1 = older_node->id_;//and we have a valid transformation mr.edge.id2 = this->id_; //since there are enough matching features, mr.edge.mean = eigen2Hogman(mr.final_trafo);//we insert an edge between the frames mr.edge.informationMatrix = Matrix6::eye(mr.inlier_matches.size()*mr.inlier_matches.size()); //TODO: What do we do about the information matrix? Scale with inlier_count. Should rmse be integrated?) } } // Paper // ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "processNodePair runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec"); return mr; }
///Find transformation with largest support, RANSAC style. ///Return false if no transformation can be found bool Node::getRelativeTransformationTo(const Node& earlier_node, std::vector<cv::DMatch>* initial_matches, Eigen::Matrix4f& resulting_transformation, float& rmse, std::vector<cv::DMatch>& matches, //for visualization? unsigned int min_inlier_threshold, unsigned int ransac_iterations) const{ std::clock_t starttime=std::clock(); assert(initial_matches != NULL); matches.clear(); std::vector<cv::DMatch> inlier; //holds those feature correspondences that support the transformation if(initial_matches->size() <= min_inlier_threshold){ ROS_DEBUG("Only %d feature matches between frames %d and %d",(int)initial_matches->size() , this->id_, earlier_node.id_); return false; } double inlier_error; //all squared errors srand((long)std::clock()); pcl::TransformationFromCorrespondences tfc; // a point is an inlier if it's no more than max_dist_m m from its partner apart float max_dist_m = 0.02; unsigned int n_iter = 0; vector<double> errors; vector<double> temp_errorsA; double best_error = 1e6; int valid_iterations = 0; unsigned int good_inlier_cnt = min(20,int(initial_matches->size()*0.2)); for (; n_iter < ransac_iterations; n_iter++) { tfc.reset(); // chose randomly from the correspondences: int sample_size = 3; // TODO: assert that no point is drawn twice for (int k = 0; k < sample_size; k++) { int id = rand() % initial_matches->size(); int this_id = initial_matches->at(id).queryIdx; int earlier_id = initial_matches->at(id).trainIdx; Eigen::Vector3f from(this->feature_locations_3d_[this_id][0], this->feature_locations_3d_[this_id][1], this->feature_locations_3d_[this_id][2]); Eigen::Vector3f to (earlier_node.feature_locations_3d_[earlier_id][0], earlier_node.feature_locations_3d_[earlier_id][1], earlier_node.feature_locations_3d_[earlier_id][2]); tfc.add(from, to); } // get relative movement from samples Eigen::Matrix4f transformation = tfc.getTransformation().matrix(); computeInliersAndError(*initial_matches, transformation, this->feature_locations_3d_, earlier_node.feature_locations_3d_, inlier, inlier_error, temp_errorsA, max_dist_m*max_dist_m); if (inlier.size() < good_inlier_cnt || inlier_error > max_dist_m) continue; ///Iterations with more than half of the initial_matches inlying, count twice if (inlier.size() > initial_matches->size()*0.5) n_iter++; ///Iterations with more than 80% of the initial_matches inlying, count threefold if (inlier.size() > initial_matches->size()*0.8) n_iter++; valid_iterations++; assert(inlier_error>0); if (inlier_error < best_error ) { resulting_transformation = transformation; matches = inlier; assert(matches.size()>= good_inlier_cnt); rmse = inlier_error; errors = temp_errorsA; best_error = inlier_error; } int max_ndx = min((int) good_inlier_cnt,30); double new_inlier_error; // compute new trafo from inliers: for (int k = 0; k < sample_size; k++) { int id = rand() % max_ndx; int this_id = inlier[id].queryIdx; int earlier_id = inlier[id].trainIdx; Eigen::Vector3f from(this->feature_locations_3d_[this_id][0], this->feature_locations_3d_[this_id][1], this->feature_locations_3d_[this_id][2]); Eigen::Vector3f to (earlier_node.feature_locations_3d_[earlier_id][0], earlier_node.feature_locations_3d_[earlier_id][1], earlier_node.feature_locations_3d_[earlier_id][2]); tfc.add(from, to); } // get relative movement from samples transformation = tfc.getTransformation().matrix(); computeInliersAndError(*initial_matches, transformation, this->feature_locations_3d_, earlier_node.feature_locations_3d_, inlier, new_inlier_error, temp_errorsA, max_dist_m*max_dist_m); if (inlier.size() < good_inlier_cnt || new_inlier_error > max_dist_m) continue; assert(new_inlier_error>0); if (new_inlier_error < best_error ) { resulting_transformation = transformation; matches = inlier; assert(matches.size()>= good_inlier_cnt); rmse = new_inlier_error; errors = temp_errorsA; best_error = new_inlier_error; } } //iterations ROS_DEBUG("asd %i good iterations, inlier percentage %i, inlier cnt: %i ",valid_iterations, (int) (matches.size()*1.0/initial_matches->size()*100),(int) matches.size()); ROS_DEBUG("asd time: %.2f, error: %.3f cm",( std::clock() - starttime ) / (double)CLOCKS_PER_SEC, rmse*100 ); for (unsigned int i=0; i < errors.size(); i++){ // ROS_INFO("asd error(%i) = %.4f cm",i,errors.at(i)*100); } ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "function runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec"); return (matches.size() > 0); }
///Find transformation with largest support, RANSAC style. ///Return false if no transformation can be found bool Node::getRelativeTransformationTo(const Node* earlier_node, std::vector<cv::DMatch>* initial_matches, Eigen::Matrix4f& resulting_transformation, float& rmse, std::vector<cv::DMatch>& matches, unsigned int ransac_iterations) const { std::clock_t starttime=std::clock(); assert(initial_matches != NULL); matches.clear(); if(initial_matches->size() <= global_min_feature_count){ ROS_INFO("Only %d feature matches between %d and %d (minimal: %i)",(int)initial_matches->size() , this->id_, earlier_node->id_, global_min_feature_count); return false; } unsigned int min_inlier_threshold = int(initial_matches->size()*global_min_inlier_pct); std::vector<cv::DMatch> inlier; //holds those feature correspondences that support the transformation double inlier_error; //all squared errors srand((long)std::clock()); // a point is an inlier if it's no more than max_dist_m m from its partner apart const float max_dist_m = global_max_dist_for_inliers; vector<double> dummy; // best values of all iterations (including invalids) double best_error = 1e6, best_error_invalid = 1e6; unsigned int best_inlier_invalid = 0, best_inlier_cnt = 0, valid_iterations = 0; Eigen::Matrix4f transformation; const unsigned int sample_size = 3;// chose this many randomly from the correspondences: for (uint n_iter = 0; n_iter < ransac_iterations; n_iter++) { //generate a map of samples. Using a map solves the problem of drawing a sample more than once std::set<cv::DMatch> sample_matches; std::vector<cv::DMatch> sample_matches_vector; while(sample_matches.size() < sample_size){ int id = rand() % initial_matches->size(); sample_matches.insert(initial_matches->at(id)); sample_matches_vector.push_back(initial_matches->at(id)); } bool valid; // valid is false iff the sampled points clearly aren't inliers themself transformation = getTransformFromMatches(earlier_node, sample_matches.begin(), sample_matches.end(),valid,max_dist_m); if (!valid) continue; // valid is false iff the sampled points aren't inliers themself if(transformation!=transformation) continue; //Contains NaN //test whether samples are inliers (more strict than before) computeInliersAndError(sample_matches_vector, transformation, this->feature_locations_3d_, earlier_node->feature_locations_3d_, inlier, inlier_error, /*output*/ dummy, max_dist_m*max_dist_m); if(inlier_error > 1000) continue; //most possibly a false match in the samples computeInliersAndError(*initial_matches, transformation, this->feature_locations_3d_, earlier_node->feature_locations_3d_, inlier, inlier_error, /*output*/ dummy, max_dist_m*max_dist_m); // check also invalid iterations if (inlier.size() > best_inlier_invalid) { best_inlier_invalid = inlier.size(); best_error_invalid = inlier_error; } // ROS_INFO("iteration %d cnt: %d, best: %d, error: %.2f",n_iter, inlier.size(), best_inlier_cnt, inlier_error*100); if(inlier.size() < min_inlier_threshold || inlier_error > max_dist_m){ //inlier.size() < ((float)initial_matches->size())*min_inlier_ratio || // ROS_INFO("Skipped iteration: inliers: %i (min %i), inlier_error: %.2f (max %.2f)", (int)inlier.size(), (int) min_inlier_threshold, inlier_error*100, max_dist_m*100); continue; } // ROS_INFO("Refining iteration from %i samples: all matches: %i, inliers: %i, inlier_error: %f", (int)sample_size, (int)initial_matches->size(), (int)inlier.size(), inlier_error); valid_iterations++; //if (inlier_error > 0) ROS_ERROR("size: %i", (int)dummy.size()); assert(inlier_error>0); //Performance hacks: ///Iterations with more than half of the initial_matches inlying, count twice if (inlier.size() > initial_matches->size()*0.5) n_iter++; ///Iterations with more than 80% of the initial_matches inlying, count threefold if (inlier.size() > initial_matches->size()*0.8) n_iter++; if (inlier_error < best_error) { //copy this to the result resulting_transformation = transformation; matches = inlier; assert(matches.size()>= min_inlier_threshold); best_inlier_cnt = inlier.size(); //assert(matches.size()>= ((float)initial_matches->size())*min_inlier_ratio); rmse = inlier_error; best_error = inlier_error; // ROS_INFO(" new best iteration %d cnt: %d, best_inlier: %d, error: %.4f, bestError: %.4f",n_iter, inlier.size(), best_inlier_cnt, inlier_error, best_error); }else { // ROS_INFO("NO new best iteration %d cnt: %d, best_inlier: %d, error: %.4f, bestError: %.4f",n_iter, inlier.size(), best_inlier_cnt, inlier_error, best_error); } //int max_ndx = min((int) min_inlier_threshold,30); //? What is this 30? double new_inlier_error; transformation = getTransformFromMatches(earlier_node, matches.begin(), matches.end(), valid); // compute new trafo from all inliers: if(transformation!=transformation) continue; //Contains NaN computeInliersAndError(*initial_matches, transformation, this->feature_locations_3d_, earlier_node->feature_locations_3d_, inlier, new_inlier_error, dummy, max_dist_m*max_dist_m); // ROS_INFO("asd recomputed: inliersize: %i, inlier error: %f", (int) inlier.size(),100*new_inlier_error); // check also invalid iterations if (inlier.size() > best_inlier_invalid) { best_inlier_invalid = inlier.size(); best_error_invalid = inlier_error; } if(inlier.size() < min_inlier_threshold || new_inlier_error > max_dist_m){ //inlier.size() < ((float)initial_matches->size())*min_inlier_ratio || // ROS_INFO("Skipped iteration: inliers: %i (min %i), inlier_error: %.2f (max %.2f)", (int)inlier.size(), (int) min_inlier_threshold, inlier_error*100, max_dist_m*100); continue; } // ROS_INFO("Refined iteration from %i samples: all matches %i, inliers: %i, new_inlier_error: %f", (int)sample_size, (int)initial_matches->size(), (int)inlier.size(), new_inlier_error); assert(new_inlier_error>0); if (new_inlier_error < best_error) { resulting_transformation = transformation; matches = inlier; assert(matches.size()>= min_inlier_threshold); //assert(matches.size()>= ((float)initial_matches->size())*min_inlier_ratio); rmse = new_inlier_error; best_error = new_inlier_error; // ROS_INFO(" improved: new best iteration %d cnt: %d, best_inlier: %d, error: %.2f, bestError: %.2f",n_iter, inlier.size(), best_inlier_cnt, inlier_error*100, best_error*100); }else { // ROS_INFO("improved: NO new best iteration %d cnt: %d, best_inlier: %d, error: %.2f, bestError: %.2f",n_iter, inlier.size(), best_inlier_cnt, inlier_error*100, best_error*100); } } //iterations ROS_INFO("%i good iterations (from %i), inlier pct %i, inlier cnt: %i, error: %.2f cm",valid_iterations, (int) ransac_iterations, (int) (matches.size()*1.0/initial_matches->size()*100),(int) matches.size(),rmse*100); // ROS_INFO("best overall: inlier: %i, error: %.2f",best_inlier_invalid, best_error_invalid*100); ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", "getRelativeTransformationTo runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec"); return matches.size() >= min_inlier_threshold; }