std::vector<cv::DMatch> GraphManager::processNodePair(Node& new_node, Node& old_node, bool& edge_added,Eigen::Matrix4f& ransac_transform, Eigen::Matrix4f& final_trafo) {
std::clock_t starttime=std::clock();
const unsigned int hit_threshold = 50; // minimal number of hits to be a valid candidate for a link
std::vector<cv::DMatch> initial_matches;
Eigen::Matrix4f icp_trafo;
Eigen::Matrix4f icp_trafo2;
AIS::LoadedEdge3D edge;
float rmse;
new_node.findPairsFlann(old_node, &initial_matches);
ROS_DEBUG("found %i inital matches",(int) initial_matches.size());
if (initial_matches.size() >= hit_threshold) {
bool valid_trafo = new_node.getRelativeTransformationTo(old_node,&initial_matches, ransac_transform, rmse, matches_);
if (!valid_trafo) {
ROS_INFO("GraphManager: Found no valid trafo, but had initially %d hits",(int) initial_matches.size());
edge_added = false;
}
else {
// improve transformation by using the generalized ICP
// std::clock_t starttime_gicp = std::clock();
// new_node.getRelativeTransformationTo_ICP(old_node,icp_trafo, &ransac_transform);
// cout << "complete time for ICP: " << ((std::clock()-starttime_gicp*1.0) / (double)CLOCKS_PER_SEC) << endl;
// cout << "old icp " << icp_trafo << endl;
// starttime_gicp = std::clock();
//new_node.getRelativeTransformationTo_ICP2(old_node,icp_trafo2, &ransac_transform);
// cout << "complete time for ICP2: " << ((std::clock()-starttime_gicp*1.0) / (double)CLOCKS_PER_SEC) << endl;
//cout << "new icp " << icp_trafo2 << endl;
// ROS_INFO_STREAM("zzz icp trafo: " << endl << icp_trafo << endl);
// ROS_INFO_STREAM("zzz icp trafo2: " << endl << icp_trafo2 << endl);
final_trafo = ransac_transform ; //*icp_trafo2;
edge.id1 = old_node.id_;//and we have a valid transformation
edge.id2 = new_node.id_; //since there are enough matching features,
edge.mean = eigen2Hogman(final_trafo);//we insert an edge between the frames
edge.informationMatrix = Matrix6::eye(1.0); //TODO: What do we do about the information matrix? Scale with rmse? inlier_count)
edge_added = addEdgeToHogman(edge, isBigTrafo(final_trafo));
if (edge_added) {
ROS_DEBUG("Adding Edge between %i and %i. Inlier: %i, error: %f",edge.id1,edge.id2,(int) matches_.size(),rmse);
}
}
}
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 initial_matches;
}
void PoseGraph::addConstraint( const envire::FrameNode* fn1, const envire::FrameNode* fn2, const envire::TransformWithUncertainty& transform )
{
AISNavigation::PoseGraph3D::Vertex
*v1 = optimizer->hogman->vertex( fn1->getUniqueIdNumericalSuffix() ),
*v2 = optimizer->hogman->vertex( fn2->getUniqueIdNumericalSuffix() );
assert( v1 && v2 );
optimizer->hogman->addEdge(
v1,
v2,
eigen2Hogman( transform.getTransform() ),
envireCov2HogmanInf( transform.getCovariance() )
);
}
void PoseGraph::addNode( envire::FrameNode* fn, const envire::TransformWithUncertainty& transform )
{
// get the vertex id
long id = fn->getUniqueIdNumericalSuffix();
// make sure the node is not already registered
AISNavigation::PoseGraph3D::Vertex
*v = optimizer->hogman->vertex( id );
assert( !v );
// create a sensor maps object
getSensorMaps( fn );
// create a new hogman vertex for this node
optimizer->hogman->addVertex(
id,
eigen2Hogman( transform.getTransform() ),
envireCov2HogmanInf( transform.getCovariance() ) );
}
/**
* associate two framenodes, if they are within a feasable distance
* between each other and have overlapping bounding boxes.
*
* @return true if an association has been added
*/
bool PoseGraph::associateNodes( envire::FrameNode* a, envire::FrameNode* b )
{
// discard if distance between is too high
// TODO: this is potentially dangerous as it doesn't take the
// uncertainty into account... see how to make this safer, but still
// fast.
if( (a->getTransform().translation() - b->getTransform().translation()).norm() > max_node_radius )
return false;
// get the sensor map objects for both frameNodes
SensorMaps
*sma = getSensorMaps( a ),
*smb = getSensorMaps( b );
// check if the bounding boxes have a common intersection
// and return false if not
if( sma->bounds.intersection( smb->bounds ).isEmpty() )
return false;
std::vector<envire::TransformWithUncertainty> constraints;
sma->associate( smb, constraints );
for( size_t i = 0; i < constraints.size(); i++ )
{
// add the egde to the optimization framework
// this will update an existing edge
optimizer->hogman->addEdge(
optimizer->hogman->vertex( a->getUniqueIdNumericalSuffix() ),
optimizer->hogman->vertex( b->getUniqueIdNumericalSuffix() ),
eigen2Hogman( constraints[i].getTransform() ),
envireCov2HogmanInf( constraints[i].getCovariance() )
);
}
// TODO store both successful and unsuccessful associations
// since it doesn't make sense to try to associate twice
return !constraints.empty();
}
//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;
}
