int ElmerReader::readNodes(node_map& nodes){
string line;
double coords[3]; // store node coords
int id, buffer, tokens;
char test;
nodes.clear();
// printf("reading nodes from \"%s\"\n",nodeFile.c_str());
ifstream in(nodeFile.c_str());
if(!in.is_open()) return -1;
// printf("file opened successfully\n");
while(in.good()){
getline(in, line); test=0;
// format for nodes is "node-id unused x y z"
tokens=sscanf(line.c_str(),"%d %d %lf %lf %lf%c",
&id, &buffer, coords, coords+1, coords+2, &test);
//printf("read %d tokens: %d %d %lf %lf %lf, test is %d\n",
// tokens, id, buffer, coords[0], coords[1], coords[2], test);
if(tokens==5 && test==0){ //line format is correct
nodes[id]=vector<double>(coords, coords+3);
}
}
in.close();
// printf("read %d nodes from %s\n", nodes.size(),nodeFile.c_str());
return nodes.size();
}
int assignRegions(id_map& regions, node_map& regionDefs, node_map& nodes, elem_map& elems){
regions.clear();
Eigen::Vector3d n; // tmp storage for a region-constraint
double d; // such that the constraint is evaluated on p as n.dot(p)<=d
bool found, match;
unsigned int i=0;
for(elem_map::const_iterator it=elems.begin(); it!=elems.end(); ++it,++i){
// check which region fits this tri
Eigen::Vector3d // a,b,c are node coordinates
a (nodes[it->second[0]][0], nodes[it->second[0]][1], nodes[it->second[0]][2]),
b (nodes[it->second[1]][0], nodes[it->second[1]][1], nodes[it->second[1]][2]),
c (nodes[it->second[2]][0], nodes[it->second[2]][1], nodes[it->second[2]][2]);
found=false;
for(node_map::iterator rd=regionDefs.begin(); rd!=regionDefs.end() && !found; ++rd){ //iterate region defs
match=true;
for(int j=0; j<rd->second.size() && match; j+=4){
n[0]=rd->second[j ]; n[1]=rd->second[j+1]; n[2]=rd->second[j+2];
d =rd->second[j+3];
if( n.dot(a) > d && // || // using && means tri is added to region if at least 1 vertex matches
n.dot(b) > d && // || // using || means tri is added to region if all of its vertices match
n.dot(c) > d ) match=false;
}
if(match){
found=true;
regions[it->first]=rd->first;
}
}
}
return 0;
}
int readRegionDefinitions(node_map& regionDefs, string filename){ //node_map maps id to vector<double>
int nRegions=0; // number of regionDefs read
bool regKwdFound=false; // look for the keyword "regions" followed by the number of regions to read
string line, token;
int ret, done=0;
char check;
double a,b,c,d;
unsigned int nextId;
std::istringstream strstream;
ifstream in(filename.c_str());
if(!in.is_open()) return -1;
while(in.good()){
getline(in, line);
if(line.empty() || line.at(0)=='#') continue; // comments have # in the first column
strstream.clear();
strstream.str(line);
getline(strstream, token, ' ');
if(!regKwdFound){
if(token.compare("regions")==0){
regKwdFound=true;
getline(strstream, token, ' '); //next token is number of regions to read
ret=sscanf(token.c_str(), "%u", &nRegions);
if(ret!=1){
in.close();
//printf("invalid ret=%d should be 1 on token %s\n",ret, token.c_str());
return -1;
}
}
}else if(done<nRegions){ //reading next region until we have plenty
//printf("processing '%s' done %d\n",line.c_str(),done);
ret=sscanf(token.c_str(), "%u", &nextId);
if(ret==1) while(getline(strstream, token, ' ')){ //token is now one condition of the region definition
//printf("parsing token '%s'",token.c_str());
ret=sscanf(token.c_str(), "(%lf,%lf,%lf,%lf%c",&a,&b,&c,&d,&check);
if(ret==5 && check==')'){ // correct format
regionDefs[nextId].push_back(a);
regionDefs[nextId].push_back(b);
regionDefs[nextId].push_back(c);
regionDefs[nextId].push_back(d);
//printf(" ... ok\n");
}
//else printf(" ... reject ret=%d, check='%c'\n",ret,check);
}
++done;
}
}
//printf("\nregionDefs:\n");
//printMap(regionDefs);
// finally add an empty region def which will be the default
if(regionDefs.empty()) nextId=ELEM_BASE_INDEX;
else nextId = regionDefs.rbegin()->first +1;
regionDefs[nextId].assign(0,0.0);
return nRegions;
}
arma::vec3 normalFromElnodes(const gind* elnodes, const node_map& nodes){
arma::vec3 n0 = nodes.at(elnodes[0])->xyzvec3();
arma::vec3 n1 = nodes.at(elnodes[1])->xyzvec3() - n0;
arma::vec3 n2 = nodes.at(elnodes[2])->xyzvec3() - n0;
arma::vec3 n = cross(n1,n2);
n/=arma::norm(n,2.0);
return n;
}
void CModuleManager::remove_in_use_module_ids_from_set( const node_map &search_nodes, guid_set &set ) const
{
for( node_map::const_iterator i = search_nodes.begin(); i != search_nodes.end(); i++ )
{
const INode *node = i->second;
set.erase( node->get_interface_definition().get_module_guid() );
remove_in_use_module_ids_from_set( node->get_children(), set );
}
}
bool CModuleManager::is_module_in_use( const node_map &search_nodes, const GUID &module_id ) const
{
for( node_map::const_iterator i = search_nodes.begin(); i != search_nodes.end(); i++ )
{
const INode *node = i->second;
if ( CGuidHelper::guids_are_equal( node->get_interface_definition().get_module_guid(), module_id ) )
{
return true;
}
if( is_module_in_use( node->get_children(), module_id ) )
{
return true;
}
}
return false;
}
void CServer::dump_state( const node_map &nodes, int indentation ) const
{
for( node_map::const_iterator i = nodes.begin(); i != nodes.end(); i++ )
{
const INode *node = i->second;
for( int i = 0; i < indentation; i++ )
{
std::cout << " |";
}
const IInterfaceDefinition &interface_definition = node->get_interface_definition();
string module_id_string = CGuidHelper::guid_to_string( interface_definition.get_module_guid() );
std::cout << " Node: \"" << node->get_name() << "\".\t module name: " << interface_definition.get_interface_info().get_name() << ".\t module id: " << module_id_string << ".\t Path: " << node->get_path().get_string() << std::endl;
bool has_children = !node->get_children().empty();
const node_endpoint_map &node_endpoints = node->get_node_endpoints();
for( node_endpoint_map::const_iterator node_endpoint_iterator = node_endpoints.begin(); node_endpoint_iterator != node_endpoints.end(); node_endpoint_iterator++ )
{
const INodeEndpoint *node_endpoint = node_endpoint_iterator->second;
const CValue *value = node_endpoint->get_value();
if( !value ) continue;
for( int i = 0; i < indentation; i++ )
{
std::cout << " |";
}
std::cout << ( has_children ? " |" : " " );
string value_string = value->get_as_string();
std::cout << " -Attribute: " << node_endpoint->get_endpoint_definition().get_name() << " = " << value_string << std::endl;
}
if( has_children )
{
dump_state( node->get_children(), indentation + 1 );
}
}
}
void sendClouds(ros::NodeHandle& nh, node_map& nodes, map<int,ros::Publisher>& publishers)
{
sensor_msgs::PointCloud2 msg;
for (node_map::iterator node_it = nodes.begin(); node_it != nodes.end(); ++node_it){
Node* node = &node_it->second;
if (publishers.find(node->cluster_id) == publishers.end())
{
// create new publisher:
ros::Publisher pub;
char buff[20]; sprintf(buff, "/cluster_%i", node->cluster_id);
pub = nh.advertise<sensor_msgs::PointCloud2>(string(buff),5);
publishers[node->cluster_id] = pub;
}
if (publishers.find(node->cluster_id)->second.getNumSubscribers() == 0)
continue;
pcl::PointCloud<pcl::PointXYZRGB> pc_global;
pcl::transformPointCloud(node->frame.dense_pointcloud,pc_global,node->pose.cast<float>());
pcl::toROSMsg (pc_global, msg);
msg.header.frame_id= "/openni_rgb_optical_frame"; // /fixed_frame";
msg.header.stamp = ros::Time::now();
msg.header.seq = node->node_id;
publishers.find(node->cluster_id)->second.publish(msg);
}
}
void sendClouds_simple(ros::Publisher pub, node_map& nodes)
{
if (pub.getNumSubscribers() == 0)
return;
sensor_msgs::PointCloud2 msg;
for (node_map::iterator node_it = nodes.begin(); node_it != nodes.end(); ++node_it){
Node* node = &node_it->second;
pcl::PointCloud<pcl::PointXYZRGB> pc_global;
pcl::transformPointCloud(node->frame.dense_pointcloud,pc_global,node->pose.cast<float>());
pcl::toROSMsg (pc_global, msg);
msg.header.frame_id= "/openni_rgb_optical_frame"; // /fixed_frame";
msg.header.stamp = ros::Time::now();
msg.header.seq = node->node_id;
// ROS_INFO("sending cloud %i", node->node_id);
pub.publish(msg);
}
}
int readVCG(std::string filename, node_map& nodes, elem_map& elems){
localReaderVCG::MyMesh mesh;
nodes.clear(); elems.clear();
int r = io::Importer<localReaderVCG::MyMesh>::Open(mesh,filename.c_str());
id_map vertexId; unsigned int k=NODE_BASE_INDEX;
for(localReaderVCG::MyMesh::VertexIterator vi=mesh.vert.begin(); vi!=mesh.vert.end(); ++vi) if(!vi->IsD()){
nodes[k].assign(3,0.0);
nodes[k][0]= vi->P()[0];
nodes[k][1]= vi->P()[1];
nodes[k][2]= vi->P()[2];
vertexId[vi-mesh.vert.begin()]=k;
++k;
}
k=ELEM_BASE_INDEX;
for(localReaderVCG::MyMesh::FaceIterator fi=mesh.face.begin(); fi!=mesh.face.end(); ++fi) if(!fi->IsD()){
elems[k].assign(3,0);
elems[k][0]=vertexId[tri::Index(mesh, fi->V(0))];
elems[k][1]=vertexId[tri::Index(mesh, fi->V(1))];
elems[k][2]=vertexId[tri::Index(mesh, fi->V(2))];
++k;
}
return r;
}
void visualizeOdometry_GT(node_map& nodes, FrameGenerator& frame_gen, bool estimate)
{
int size_px = 800;
float border_m = 0.25; // size of border around bounding box
cv::Mat odo_img(size_px, size_px,CV_32FC3);
odo_img.setTo(cv::Scalar::all(0));
// compute the track's bounding box
cv::Point2f min_(1e5,1e5);
cv::Point2f max_(-1e5,-1e5);
for (node_it it = nodes.begin(); it != nodes.end(); ++it)
{
cv::Point2f p;
if (estimate){
p.x = (*it).second.opt_pose(0,3);
p.y = (*it).second.opt_pose(1,3);
}
else
p = (*it).second.gt_pos_xy;
min_.x = min(min_.x,p.x); max_.x = max(max_.x,p.x);
min_.y = min(min_.y,p.y); max_.y = max(max_.y,p.y);
}
// 0.5 Abstand um die Bounding Box
double m2px_x = (2*border_m+ (max_.x-min_.x))/size_px;
double m2px_y = (2*border_m+ (max_.y-min_.y))/size_px;
double m2px = max(m2px_x,m2px_y);
// ROS_INFO("m2px: %f", m2px);
// ROS_INFO("min: %f %f", min_.x, min_.y);
map<uint, cv::Point> px_pos;
for (node_it it = nodes.begin(); it != nodes.end(); ++it)
{
cv::Point2f p;
if (estimate){
p.x = (*it).second.opt_pose(0,3);
p.y = (*it).second.opt_pose(1,3);
}
else
p = (*it).second.gt_pos_xy;
// printf("gt_pos: %f %f \n", p.x,p.y);
px_pos[it->first] = cv::Point2i( (p.x-(min_.x-border_m))/m2px , (p.y-(min_.y-border_m))/m2px ) ;
// ROS_INFO("px: %i %i",px_pos[it->first].x, px_pos[it->first].y );
// px_pos[it->first] = cv::Point2i( (border_m+(p.x-min_.x))/m2px, (border_m+(p.y-min_.y))/m2px );
}
for (float x = floor(min_.x-border_m); x <= ceil(max_.x+border_m); x+=0.25) {
int px_x = (x-(min_.x-border_m))/m2px;
cv::line(odo_img,cv::Point2i(px_x,0),cv::Point2i(px_x,size_px), cv::Scalar(255,255,255),1);
}
for (float y = floor(min_.y-border_m); y <= ceil(max_.y+border_m); y+=0.25) {
int px_y = (y-(min_.y-border_m))/m2px;
cv::line(odo_img,cv::Point2i(0,px_y),cv::Point2i(size_px,px_y), cv::Scalar(255,255,255),1);
}
// show node positions
for (node_it it = nodes.begin(); it != nodes.end(); ++it)
{
Node* current = &(*it).second;
if (current->is_keyframe)
cv::circle(odo_img,px_pos[current->node_id],3,cv::Scalar(255,0,0),1 );
else
cv::circle(odo_img,px_pos[current->node_id],1,cv::Scalar(255,255,255),1 );
}
// if (nodes.size() > 0){
// Node* last = &nodes.rbegin()->second;
// for (uint i=0; i<last->tree_proposals.size(); ++i)
// cv::line(odo_img,px_pos[last->node_id],px_pos[nodes[last->tree_proposals[i]].node_id],cv::Scalar::all(255),1);
// }
// show edges:
for (node_it it = nodes.begin(); it != nodes.end(); ++it)
{
Node* current = &(*it).second;
for (uint j=0; j<current->matches.size(); j++)
{
Node_match* nm = ¤t->matches.at(j);
if (current->node_id < nm->other_id)
continue;
// ROS_INFO("edges: %i %i", current->node_id, nm->other_id);
Node* neighbour = &nodes[nm->other_id];
int r,g,b;
r=g=b=0;
// ROS_INFO("type: %i", nm->type);
switch (nm->type) {
case ET_Direct_neighbour:
r = b = 255; break; // yellow
case ET_Tree_Proposal:
r = 255; break; // red
case ET_Ring:
g = 255; break;
case ET_Last_match:
g = r = 255; break;
default:
r=g=b=255; break;
}
cv::Scalar col = cv::Scalar(b,g,r);
cv::line(odo_img,px_pos[current->node_id],px_pos[neighbour->node_id],col,1);
}
}
// if (frame_gen.node_id_running % 10 == 0){
// char buffer[300];
// sprintf(buffer, "/home/engelhar/Desktop/img/%i.png", (int)frame_gen.node_id_running);;
// cv::imwrite(buffer, odo_img);
// }
if (estimate){
cv::namedWindow("pos_Est",1);
cv::imshow("pos_Est",odo_img);
}
else
{
cv::namedWindow("odometry_GT",1);
cv::imshow("odometry_GT",odo_img);
}
cv::waitKey(10);
}
void sendMarkers(ros::Publisher pub_marker, ros::Publisher pub_marker_array , node_map& nodes)
{
visualization_msgs::MarkerArray m_array;
if (pub_marker_array.getNumSubscribers() > 0)
{
for (node_map::iterator nm_it = nodes.begin(); nm_it !=nodes.end(); ++nm_it)
{
visualization_msgs::Marker marker;
visualization_msgs::Marker marker_text;
Eigen::Vector4d P = Eigen::Vector4d::Zero();
P.array()[3] = 1;
Eigen::Vector4d start = nm_it->second.pose*P;
P.array()[2] = 0.2; // 20cm in z-direction
Eigen::Vector4d end = nm_it->second.pose*P;
// cerr << "node " << nm_it->second.node_id << " at: " << start << endl;
marker.header.frame_id = "/openni_rgb_optical_frame";
marker.header.stamp = ros::Time();
marker.ns = "cam_positions_ns";
marker.id = nm_it->first*2;
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::MODIFY; // same as add
geometry_msgs::Point p,p2;
p.x = start.array()[0]; p.y = start.array()[1]; p.z = start.array()[2];
marker.points.push_back(p);
p2.x = end.array()[0]; p2.y = end.array()[1]; p2.z = end.array()[2];
marker.points.push_back(p2);
marker.scale.x = 0.01; // radius in m
marker.scale.y = 0.03; // head radius in m
marker.color.a = 1;
// ROS_INFO("drawing node with cluster %i ( mod 3 = %i)",nm_it->second.cluster_id, nm_it->second.cluster_id%3 );
// cout << "at " << nm_it->second.pose << endl;
marker.color.r = marker.color.g = marker.color.b = 0;
switch (nm_it->second.cluster_id%3) {
case 0: marker.color.b = 1; break;
case 1: marker.color.g = 1; break;
case 2: marker.color.r = 1; break;
default:
break;
}
m_array.markers.push_back(marker);
marker_text.header.frame_id = "/openni_rgb_optical_frame";//"/fixed_frame";
//marker_text.header.frame_id = "/fixed_frame";
marker_text.header.stamp = ros::Time();
marker_text.ns = "cam_positions_ns";
marker_text.id = nm_it->first*2+1;
marker_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
marker_text.action = visualization_msgs::Marker::MODIFY; // same as add
char buff[20]; sprintf(buff, "%i", nm_it->first);
marker_text.text = string(buff);
marker_text.color = marker.color;
marker_text.scale.z = 0.02;
marker_text.pose.position.x = nm_it->second.pose(0,3);
marker_text.pose.position.y = nm_it->second.pose(1,3);
marker_text.pose.position.z = nm_it->second.pose(2,3)-0.1;
m_array.markers.push_back(marker_text);
}
// ROS_ERROR("published %i arrows", (int) m_array.markers.size());
pub_marker_array.publish(m_array);
}
if (pub_marker.getNumSubscribers() > 0)
{
// publish connections between cameras
visualization_msgs::Marker m_cam_edge_list;
m_cam_edge_list.header.frame_id = "/openni_rgb_optical_frame"; //"/fixed_frame";
m_cam_edge_list.header.stamp = ros::Time();
m_cam_edge_list.ns = "cam_matches_ns";
m_cam_edge_list.id++;
m_cam_edge_list.type = visualization_msgs::Marker::LINE_LIST;
m_cam_edge_list.action = visualization_msgs::Marker::MODIFY;
m_cam_edge_list.color.a =1;
m_cam_edge_list.color.g = 1;
m_cam_edge_list.color.r = m_cam_edge_list.color.b = 0;
m_cam_edge_list.scale.x = 0.005; // line width in m
for (node_map::iterator nm_it = nodes.begin(); nm_it !=nodes.end(); ++nm_it)
{
Node* current = &nm_it->second;
Eigen::Vector4d P = Eigen::Vector4d::Zero();
P.array()[3] = 1; // homogeneous coordinates
Eigen::Vector4d p1 = current->pose*P;
geometry_msgs::Point p; p.x = p1.array()[0]; p.y = p1.array()[1]; p.z = p1.array()[2];
for (uint i=0; i<current->matches.size(); ++i)
{
Node* other = &nodes[current->matches.at(i).other_id];
Eigen::Vector4d p2 = other->pose*P;
geometry_msgs::Point gp; gp.x = p2.array()[0]; gp.y = p2.array()[1]; gp.z = p2.array()[2];
// ROS_INFO("vis edge between %i and %i", current->node_id, other->node_id);
m_cam_edge_list.points.push_back(p);
m_cam_edge_list.points.push_back(gp);
}
}
pub_marker.publish(m_cam_edge_list);
}
}
int BEMReader::readModel(elem_map& elems, id_map& bodyIDs,
elem_map& bndrys, elem_map& bndryParents, node_map& nodes,
const ELEM_TYPE elemType, const std::set<unsigned int> elemBodies,
const ELEM_TYPE bndryType, const std::set<unsigned int> bndryBodies
){
//if meshFile ends with ".stl", ".obj", ".ply" etc. use VCG to read raw-mesh (nodes & elems, nothing more)
if( meshFile.find(".stl")!=meshFile.npos ||
meshFile.find(".obj")!=meshFile.npos ||
meshFile.find(".off")!=meshFile.npos ||
meshFile.find(".ply")!=meshFile.npos
){
printf("\n%% reading raw triangle mesh from %s (requires --remesh option)\n%%\t",meshFile.c_str());
return readVCG(meshFile,nodes,elems);
}
node_map nodes_in;
//elem_map elems_in;
int ret;
// read elements and nodes
ret=readElems(elems, elemType, elemBodies,ELEMENTS_FILE,&bodyIDs);
if(ret<0) return ret;
ret=readNodes(nodes_in);
if(ret<0) return ret;
// now switch to .boundary file
string tmp=elemFile;
elemFile=meshFile;
elemFile.append(".boundary");
ret=readElems(bndrys,bndryType,bndryBodies,BOUNDARY_FILE,NULL,&bndryParents,true);
// restore state of the BEMReader object
elemFile=tmp;
//if(ret<0) return ret; // it's ok to not have a boundary file, if there's no pre-defined cracks
// reading is complete, but for HyENA-BEM compatibility the nodes need to be numbered
// in the same order as they appear in the element map.
// run through the element map and decide new node numbers
id_map fwd;// bkwd; // fwd[old_id]=new_id, bkwd[new_id]=old_id
unsigned int new_id=NODE_BASE_INDEX;
for(elem_map::iterator i = elems.begin(); i!=elems.end(); ++i){
//run through all nodes of the element
for(elem_map::mapped_type::iterator j = i->second.begin(); j!=i->second.end(); ++j){
if(fwd.count(*j)==0){ // assing new number at first occurence of node
fwd[*j]=new_id; //bkwd[new_id]=*j;
new_id++;
}
(*j)=fwd[*j]; //update element
}
}
nodes.clear();
// copy from nodes_in to nodes while applying new numbering
for(node_map::iterator i = nodes_in.begin(); i!= nodes_in.end(); ++i){
nodes[fwd[i->first]] = i->second;
}
// apply new numbering to bndry elements
for(elem_map::iterator i = bndrys.begin(); i!=bndrys.end(); ++i){
for(elem_map::mapped_type::iterator j = i->second.begin(); j!=i->second.end(); ++j){
(*j)=fwd[*j]; //update element
}
}
return elems.size();
}