示例#1
0
    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();
    }
示例#2
0
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;
}
示例#3
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;
}
示例#4
0
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 );
			}
		}
	}
示例#8
0
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);

  }



}
示例#9
0
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);
  }
}
示例#10
0
	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;
	}
示例#11
0
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 = &current->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);


}
示例#12
0
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);
  }
}
示例#13
0
    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();
    }