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