//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;
}
