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