bool GraphManager::addEdgeToHogman(AIS::LoadedEdge3D edge, bool largeEdge) {
std::clock_t starttime=std::clock();
freshlyOptimized_ = false;
AIS::PoseGraph3D::Vertex* v1 = optimizer_->vertex(edge.id1);
AIS::PoseGraph3D::Vertex* v2 = optimizer_->vertex(edge.id2);
// at least one vertex has to be created, assert that the transformation
// is large enough to avoid to many vertices on the same spot
if (!v1 || !v2) {
if (!largeEdge) {
ROS_INFO("edge to new vertex is to short, vertex will not be inserted");
//return false;
}
}
if (!v1) {
v1 = optimizer_->addVertex(edge.id1, Transformation3(), Matrix6::eye(1.0));
assert(v1);
}
if (!v2) {
v2 = optimizer_->addVertex(edge.id2, Transformation3(), Matrix6::eye(1.0));
assert(v2);
}
optimizer_->addEdge(v1, v2, edge.mean, edge.informationMatrix);
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 true;
}
void alignMesh( Polyhedron & poly )
{
int num = poly.size_of_vertices();
// initialization here is very important!!
Point3 ave( 0.0, 0.0, 0.0 );
for ( Vertex_iterator vi = poly.vertices_begin(); vi != poly.vertices_end(); ++vi ) {
ave = ave + ( vi->point() - CGAL::ORIGIN );
}
ave = CGAL::ORIGIN + ( ave - CGAL::ORIGIN )/( double )num;
unsigned int dim = 3;
double * data = new double [ num*dim ];
int nPoints = 0;
for ( Vertex_iterator vi = poly.vertices_begin(); vi != poly.vertices_end(); ++vi ) {
data[ nPoints * dim + 0 ] = vi->point().x() - ave.x();
data[ nPoints * dim + 1 ] = vi->point().y() - ave.y();
data[ nPoints * dim + 2 ] = vi->point().z() - ave.z();
nPoints++;
}
assert( nPoints == ( int )num );
/*****************************************
analyze the engenstructure of X^T X
*****************************************/
/* define the matrix X */
gsl_matrix_view X = gsl_matrix_view_array( data, num, dim );
/* memory reallocation */
// proj = ( double * )realloc( proj, sizeof( double ) * PDIM * num );
/* calculate the covariance matrix B */
gsl_matrix * B = gsl_matrix_alloc( dim, dim );
gsl_blas_dgemm( CblasTrans, CblasNoTrans, 1.0,
&(X.matrix),
&(X.matrix), 0.0,
B );
/* divided by the number of samples */
gsl_matrix_scale( B, 1.0/(double)num );
gsl_vector * eVal = gsl_vector_alloc( dim );
gsl_matrix * eVec = gsl_matrix_alloc( dim, dim );
gsl_eigen_symmv_workspace * w
= gsl_eigen_symmv_alloc( dim );
// eigenanalysis of the matrix B
gsl_eigen_symmv( B, eVal, eVec, w );
// release the memory of w
gsl_eigen_symmv_free( w );
// sort eigenvalues in a descending order
gsl_eigen_symmv_sort( eVal, eVec, GSL_EIGEN_SORT_VAL_DESC );
// #ifdef MYDEBUG
for ( unsigned int i = 0; i < dim; ++i ) {
cerr << "Eigenvalue No. " << i << " = " << gsl_vector_get( eVal, i ) << endl;
cerr << "Eigenvector No. " << i << endl;
double length = 0.0;
for ( unsigned int j = 0; j < dim; ++j ) {
cerr << gsl_matrix_get( eVec, i, j ) << " ";
length += gsl_matrix_get( eVec, i, j )*gsl_matrix_get( eVec, i, j );
}
cerr << " length = " << length << endl;
}
// #endif // MYDEBUG
// 0 1 2, 0 2 1,
Transformation3 map;
map =
Transformation3( gsl_matrix_get(eVec,1,0), gsl_matrix_get(eVec,0,0), gsl_matrix_get(eVec,2,0),
gsl_matrix_get(eVec,1,1), gsl_matrix_get(eVec,0,1), gsl_matrix_get(eVec,2,1),
gsl_matrix_get(eVec,1,2), gsl_matrix_get(eVec,0,2), gsl_matrix_get(eVec,2,2) );
if ( map.is_odd() ) {
cerr << " Transformation matrix reflected" << endl;
map =
Transformation3( gsl_matrix_get(eVec,1,0), gsl_matrix_get(eVec,0,0), -gsl_matrix_get(eVec,2,0),
gsl_matrix_get(eVec,1,1), gsl_matrix_get(eVec,0,1), -gsl_matrix_get(eVec,2,1),
gsl_matrix_get(eVec,1,2), gsl_matrix_get(eVec,0,2), -gsl_matrix_get(eVec,2,2) );
}
for ( unsigned int i = 0; i < dim; ++i ) {
cerr << "| ";
for ( unsigned int j = 0; j < dim; ++j ) {
cerr << map.cartesian( i, j ) << " ";
}
cerr << "|" << endl;
}
transformMesh( poly, map );
return;
}
// returns true, iff node could be added to the cloud
bool GraphManager::addNode(Node new_node) {
std::clock_t starttime=std::clock();
if(reset_request_) {
int numLevels = 3;
int nodeDistance = 2;
marker_id =0;
time_of_last_transform_= ros::Time();
last_batch_update_=std::clock();
delete optimizer_;
optimizer_ = new AIS::HCholOptimizer3D(numLevels, nodeDistance);
graph_.clear();
matches_.clear();
freshlyOptimized_= false;
reset_request_ = false;
}
if (new_node.feature_locations_2d_.size() <= 5) {
ROS_DEBUG("found only %i features on image, node is not included",(int)new_node.feature_locations_2d_.size());
return false;
}
//set the node id only if the node is actually added to the graph
//needs to be done here as the graph size can change inside this function
new_node.id_ = graph_.size();
//First Node, so only build its index, insert into storage and add a
//vertex at the origin, of which the position is very certain
if (graph_.size()==0) {
new_node.buildFlannIndex(); // create index so that next nodes can use it
// graph_.insert(make_pair(new_node.id_, new_node)); //store
graph_[new_node.id_] = new_node;
optimizer_->addVertex(0, Transformation3(), 1e9*Matrix6::eye(1.0)); //fix at origin
return true;
}
unsigned int num_edges_before = optimizer_->edges().size();
std::vector<cv::DMatch> initial_matches;
ROS_DEBUG("Graphsize: %d", (int) graph_.size());
marker_id = 0; //overdraw old markers
Eigen::Matrix4f ransac_trafo, final_trafo;
bool edge_added_to_base;
std::vector<int> vertices_to_comp = getPotentialEdgeTargets(new_node, -1); //vernetzungsgrad
int id_of_id = vertices_to_comp.size() -1;
for (; id_of_id >=0; id_of_id--) { //go from the back, so the last comparison is with the first node. The last comparison is visualized.
initial_matches = processNodePair(new_node, graph_[vertices_to_comp[id_of_id]],edge_added_to_base,ransac_trafo, final_trafo);
//initial_matches = processNodePair(new_node, graph_rit->second);
//What if the node has not been added? visualizeFeatureFlow3D(graph_rit->second, new_node, initial_matches, matches_, marker_id++);
}
id_of_id++;//set back to last valid id
if (optimizer_->edges().size() > num_edges_before) {
new_node.buildFlannIndex();
graph_[new_node.id_] = new_node;
ROS_DEBUG("Added Node, new Graphsize: %i", (int) graph_.size());
optimizeGraph();
//make the transform of the last node known
ros::TimerEvent unused;
broadcastTransform(unused);
visualizeGraphEdges();
visualizeGraphNodes();
visualizeFeatureFlow3D(graph_[vertices_to_comp[id_of_id]], new_node, initial_matches, matches_, marker_id++);
} else {
ROS_WARN("##### could not find link for PointCloud!");
matches_.clear();
}
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 (optimizer_->edges().size() > num_edges_before);
}
