ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back] 

vector<visualization_msgs::Marker> line_list, marker;
visualization_msgs::Marker text_view_facing;
line_list.resize(3); marker.resize(3);
for(int i=0;i<line_list.size();i++) {
    marker[i].header.frame_id = line_list[i].header.frame_id = text_view_facing.header.frame_id = "/camera_link";
    marker[i].header.stamp = line_list[i].header.stamp = text_view_facing.header.stamp =ros::Time();
    marker[i].ns = line_list[i].ns = text_view_facing.ns ="lines";
    marker[i].action = line_list[i].action = text_view_facing.action = visualization_msgs::Marker::ADD;
    marker[i].pose.orientation.w = line_list[i].pose.orientation.w = text_view_facing.pose.orientation.w = 1;
    marker[i].id = i+3;
    line_list[i].id = i;
    marker[i].type = visualization_msgs::Marker::POINTS;
    line_list[i].type = visualization_msgs::Marker::LINE_LIST;
    line_list[i].color.r = 0; line_list[i].color.g = 1; line_list[i].color. b = 1; line_list[i].color.a = 1;
    marker[i].lifetime = ros::Duration(0.01);
    marker[i].scale.x = 0.003;
    marker[i].scale.y = 0.003;
    marker[i].scale.z = 0.003;
}
text_view_facing.id = 6;
text_view_facing.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
text_view_facing.color.b = 1; text_view_facing.color.a = 1.0; text_view_facing.color.g = 1.0; text_view_facing.color.r = 1.0;
text_view_facing.scale.z = 0.015;
string obj1 = "first_segmented_image";
    bigarray size_arr, arr0;
    for(int p0=0;p0<bound.size() && ros::ok();p0++) {
        p[0] = output.at(bound[p0].x,bound[p0].y);
        if(!isnan(p[0].x) && !isnan(p[0].y) && !isnan(p[0].z)) { // point p0
            P.x = p[0].x; P.y = p[0].y; P.z = p[0].z;
            pc.r = 1; pc.g = 0; pc.b = 0; pc.a = 1;
            marker[0].points.clear(); marker[0].colors.clear();
            marker[0].points.push_back(P);
            marker[0].colors.push_back(pc);
            marker_pub.publish(marker[0]);
        }
        for(int p1=0;p1<bound.size() && ros::ok();p1++) {
            p[1] = output.at(bound[p1].x,bound[p1].y);
            if(!isnan(p[1].x) && !isnan(p[1].y) && !isnan(p[1].z)) { // point p1
                P.x = p[1].x; P.y = p[1].y; P.z = p[1].z;
                pc.r = 0; pc.g = 1; pc.b = 0; pc.a = 1;
                marker[1].points.clear(); marker[1].colors.clear();
                marker[1].points.push_back(P);
                marker[1].colors.push_back(pc);
                marker_pub.publish(marker[1]);
            }
            if(!isnan(p[1].x) && !isnan(p[1].y) && !isnan(p[1].z)) { // line p0p1
                line_list[0].points.clear(); line_list[0].colors.clear();
                line_list[0].scale.x = 0.00075;
                P.x = p[0].x; P.y = p[0].y; P.z = p[0].z;  line_list[0].points.push_back(P);
                P.x = p[1].x; P.y = p[1].y; P.z = p[1].z;  line_list[0].points.push_back(P);
                marker_pub.publish(line_list[0]);
            }
            int d0r = sqrt(pow(p[1].x-p[0].x,2)+pow(p[1].y-p[0].y,2)+pow(p[1].z-p[0].z,2) )*1000;
            if(d0r==10) {
                for(int p2=0;p2<bound.size() && ros::ok();p2++) {
                    p[2] = output.at(bound[p2].x,bound[p2].y);
                    if(!isnan(p[2].x) && !isnan(p[2].y) && !isnan(p[2].z)) { // point p2
                        P.x = p[2].x; P.y = p[2].y; P.z = p[2].z;
                        pc.r = 0; pc.g = 0; pc.b = 1; pc.a = 1;  marker[2].points.clear();
                        marker[2].colors.clear();
                        marker[2].points.push_back(P);
                        marker[2].colors.push_back(pc);
                        marker_pub.publish(marker[2]);
                    }
                    if(!isnan(p[2].x) && !isnan(p[2].y) && !isnan(p[2].z)) {  // line p1p2
                        line_list[1].points.clear(); line_list[1].colors.clear();
                        line_list[1].scale.x = 0.00075;
                        P.x = p[2].x; P.y = p[2].y; P.z = p[2].z;  line_list[1].points.push_back(P);
                        P.x = p[1].x; P.y = p[1].y; P.z = p[1].z;  line_list[1].points.push_back(P);
                        marker_pub.publish(line_list[1]);
                        line_list[2].points.clear(); line_list[2].colors.clear(); // line p0p2
                        line_list[2].scale.x = 0.00075;
                        P.x = p[2].x; P.y = p[2].y; P.z = p[2].z;  line_list[2].points.push_back(P);
                        P.x = p[0].x; P.y = p[0].y; P.z = p[0].z;  line_list[2].points.push_back(P);
                        marker_pub.publish(line_list[2]);
                    }
                    int d1r = sqrt(pow(p[2].x-p[1].x,2)+pow(p[2].y-p[1].y,2)+pow(p[2].z-p[1].z,2) )*1000;
                    int d4r = sqrt(pow(p[2].x-p[0].x,2)+pow(p[2].y-p[0].y,2)+pow(p[2].z-p[0].z,2) )*1000;
                    if(d4r>10 && d1r>10) { cout<<p0<<"\t"<<p1<<"\t"<<p2<<endl;
                        int key = ++size_arr(1,1,d0r,d1r,d4r);
                        arr0(1,key,d0r,d1r,d4r) = p0;
                        arr0(2,key,d0r,d1r,d4r) = p1;
                        arr0(3,key,d0r,d1r,d4r) = p2;
                    }
                }
            }
        }
    }

example: 

vector<visualization_msgs::Marker> 
vector<visualization_msgs::marker> line_list, marker;
 visualization_msgs::Marker text_view_facing;
 line_list.resize(3); marker.resize(3);
 for(int i=0;i<line_list.size();i++) {
    marker[i].header.frame_id = line_list[i].header.frame_id = text_view_facing.header.frame_id = "/camera_link";
    marker[i].header.stamp = line_list[i].header.stamp = text_view_facing.header.stamp =ros::Time();
    marker[i].ns = line_list[i].ns = text_view_facing.ns ="lines";
    marker[i].action = line_list[i].action = text_view_facing.action = visualization_msgs::Marker::ADD;
    marker[i].pose.orientation.w = line_list[i].pose.orientation.w = text_view_facing.pose.orientation.w = 1;
    marker[i].id = i+3;
    line_list[i].id = i;
    marker[i].type = visualization_msgs::Marker::POINTS;
    line_list[i].type = visualization_msgs::Marker::LINE_LIST;
    line_list[i].color.r = 0; line_list[i].color.g = 1; line_list[i].color. b = 1; line_list[i].color.a = 1;
    marker[i].lifetime = ros::Duration(0.01);
    marker[i].scale.x = 0.003;
    marker[i].scale.y = 0.003;
    marker[i].scale.z = 0.003;
}
text_view_facing.id = 6;
text_view_facing.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
text_view_facing.color.b = 1; text_view_facing.color.a = 1.0; text_view_facing.color.g = 1.0; text_view_facing.color.r = 1.0;
text_view_facing.scale.z = 0.015;
string obj1 = "first_segmented_image";
    bigarray size_arr, arr0;
    for(int p0=0;p0<bound.size() && ros::ok();p0++) {
        p[0] = output.at(bound[p0].x,bound[p0].y);
        if(!isnan(p[0].x) && !isnan(p[0].y) && !isnan(p[0].z)) { // point p0
            P.x = p[0].x; P.y = p[0].y; P.z = p[0].z;
            pc.r = 1; pc.g = 0; pc.b = 0; pc.a = 1;
            marker[0].points.clear(); marker[0].colors.clear();
            marker[0].points.push_back(P);
            marker[0].colors.push_back(pc);
            marker_pub.publish(marker[0]);
        }
        for(int p1=0;p1<bound.size() && ros::ok();p1++) {
            p[1] = output.at(bound[p1].x,bound[p1].y);
            if(!isnan(p[1].x) && !isnan(p[1].y) && !isnan(p[1].z)) { // point p1
                P.x = p[1].x; P.y = p[1].y; P.z = p[1].z;
                pc.r = 0; pc.g = 1; pc.b = 0; pc.a = 1;
                marker[1].points.clear(); marker[1].colors.clear();
                marker[1].points.push_back(P);
                marker[1].colors.push_back(pc);
                marker_pub.publish(marker[1]);
            }
            if(!isnan(p[1].x) && !isnan(p[1].y) && !isnan(p[1].z)) { // line p0p1
                line_list[0].points.clear(); line_list[0].colors.clear();
                line_list[0].scale.x = 0.00075;
                P.x = p[0].x; P.y = p[0].y; P.z = p[0].z;  line_list[0].points.push_back(P);
                P.x = p[1].x; P.y = p[1].y; P.z = p[1].z;  line_list[0].points.push_back(P);
                marker_pub.publish(line_list[0]);
            }
            int d0r = sqrt(pow(p[1].x-p[0].x,2)+pow(p[1].y-p[0].y,2)+pow(p[1].z-p[0].z,2) )*1000;
            if(d0r==10) {
                for(int p2=0;p2<bound.size() && ros::ok();p2++) {
                    p[2] = output.at(bound[p2].x,bound[p2].y);
                    if(!isnan(p[2].x) && !isnan(p[2].y) && !isnan(p[2].z)) { // point p2
                        P.x = p[2].x; P.y = p[2].y; P.z = p[2].z;
                        pc.r = 0; pc.g = 0; pc.b = 1; pc.a = 1;  marker[2].points.clear();
                        marker[2].colors.clear();
                        marker[2].points.push_back(P);
                        marker[2].colors.push_back(pc);
                        marker_pub.publish(marker[2]);
                    }
                    if(!isnan(p[2].x) && !isnan(p[2].y) && !isnan(p[2].z)) {  // line p1p2
                        line_list[1].points.clear(); line_list[1].colors.clear();
                        line_list[1].scale.x = 0.00075;
                        P.x = p[2].x; P.y = p[2].y; P.z = p[2].z;  line_list[1].points.push_back(P);
                        P.x = p[1].x; P.y = p[1].y; P.z = p[1].z;  line_list[1].points.push_back(P);
                        marker_pub.publish(line_list[1]);
                        line_list[2].points.clear(); line_list[2].colors.clear(); // line p0p2
                        line_list[2].scale.x = 0.00075;
                        P.x = p[2].x; P.y = p[2].y; P.z = p[2].z;  line_list[2].points.push_back(P);
                        P.x = p[0].x; P.y = p[0].y; P.z = p[0].z;  line_list[2].points.push_back(P);
                        marker_pub.publish(line_list[2]);
                    }
                    int d1r = sqrt(pow(p[2].x-p[1].x,2)+pow(p[2].y-p[1].y,2)+pow(p[2].z-p[1].z,2) )*1000;
                    int d4r = sqrt(pow(p[2].x-p[0].x,2)+pow(p[2].y-p[0].y,2)+pow(p[2].z-p[0].z,2) )*1000;
                    if(d4r>10 {="" marker[i].header.frame_id="line_list[i].header.frame_id" =="" text_view_facing.header.frame_id="/camera_link" ;="" marker[i].header.stamp="line_list[i].header.stamp" =="" text_view_facing.header.stamp="ros::Time();" marker[i].ns="line_list[i].ns" =="" text_view_facing.ns="lines" ;="" marker[i].action="line_list[i].action" =="" text_view_facing.action="visualization_msgs::Marker::ADD;" marker[i].pose.orientation.w="line_list[i].pose.orientation.w" =="" text_view_facing.pose.orientation.w="1;" marker[i].id="i+3;" line_list[i].id="i;" marker[i].type="visualization_msgs::Marker::POINTS;" line_list[i].type="visualization_msgs::Marker::LINE_LIST;" line_list[i].color.r="0;" line_list[i].color.g="1;" line_list[i].color.="" b="1;" line_list[i].color.a="1;" marker[i].lifetime="ros::Duration(0.01);" marker[i].scale.x="0.003;" marker[i].scale.y="0.003;" marker[i].scale.z="0.003;" }="" text_view_facing.id="6;" text_view_facing.type="visualization_msgs::Marker::TEXT_VIEW_FACING;" text_view_facing.color.b="1;" text_view_facing.color.a="1.0;" text_view_facing.color.g="1.0;" text_view_facing.color.r="1.0;" text_view_facing.scale.z="0.015;" string="" obj1="first_segmented_image" ;="" bigarray="" size_arr,="" arr0;="" for(int="" p0="0;p0&lt;bound.size()" &amp;&amp;="" ros::ok();p0++)="" {="" p[0]="output.at(bound[p0].x,bound[p0].y);" if(!isnan(p[0].x)="" &amp;&amp;="" !isnan(p[0].y)="" &amp;&amp;="" !isnan(p[0].z))="" {="" point="" p0="" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" pc.r="1;" pc.g="0;" pc.b="0;" pc.a="1;" marker[0].points.clear();="" marker[0].colors.clear();="" marker[0].points.push_back(p);="" marker[0].colors.push_back(pc);="" marker_pub.publish(marker[0]);="" }="" for(int="" p1="0;p1&lt;bound.size()" &amp;&amp;="" ros::ok();p1++)="" {="" p[1]="output.at(bound[p1].x,bound[p1].y);" if(!isnan(p[1].x)="" &amp;&amp;="" !isnan(p[1].y)="" &amp;&amp;="" !isnan(p[1].z))="" {="" point="" p1="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" pc.r="0;" pc.g="1;" pc.b="0;" pc.a="1;" marker[1].points.clear();="" marker[1].colors.clear();="" marker[1].points.push_back(p);="" marker[1].colors.push_back(pc);="" marker_pub.publish(marker[1]);="" }="" if(!isnan(p[1].x)="" &amp;&amp;="" !isnan(p[1].y)="" &amp;&amp;="" !isnan(p[1].z))="" {="" line="" p0p1="" line_list[0].points.clear();="" line_list[0].colors.clear();="" line_list[0].scale.x="0.00075;" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" line_list[0].points.push_back(p);="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" line_list[0].points.push_back(p);="" marker_pub.publish(line_list[0]);="" }="" int="" d0r="sqrt(pow(p[1].x-p[0].x,2)+pow(p[1].y-p[0].y,2)+pow(p[1].z-p[0].z,2)" )*1000;="" if(d0r="=10)" {="" for(int="" p2="0;p2&lt;bound.size()" &amp;&amp;="" ros::ok();p2++)="" {="" p[2]="output.at(bound[p2].x,bound[p2].y);" if(!isnan(p[2].x)="" &amp;&amp;="" !isnan(p[2].y)="" &amp;&amp;="" !isnan(p[2].z))="" {="" point="" p2="" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" pc.r="0;" pc.g="0;" pc.b="1;" pc.a="1;" marker[2].points.clear();="" marker[2].colors.clear();="" marker[2].points.push_back(p);="" marker[2].colors.push_back(pc);="" marker_pub.publish(marker[2]);="" }="" if(!isnan(p[2].x)="" &amp;&amp;="" !isnan(p[2].y)="" &amp;&amp;="" !isnan(p[2].z))="" {="" line="" p1p2="" line_list[1].points.clear();="" line_list[1].colors.clear();="" line_list[1].scale.x="0.00075;" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" line_list[1].points.push_back(p);="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" line_list[1].points.push_back(p);="" marker_pub.publish(line_list[1]);="" line_list[2].points.clear();="" line_list[2].colors.clear();="" line="" p0p2="" line_list[2].scale.x="0.00075;" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" line_list[2].points.push_back(p);="" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" line_list[2].points.push_back(p);="" marker_pub.publish(line_list[2]);="" }="" int="" d1r="sqrt(pow(p[2].x-p[1].x,2)+pow(p[2].y-p[1].y,2)+pow(p[2].z-p[1].z,2)" )*1000;="" int="" d4r="sqrt(pow(p[2].x-p[0].x,2)+pow(p[2].y-p[0].y,2)+pow(p[2].z-p[0].z,2)" )*1000;="" if(d4r="">10 && d1r>10) { cout<<p0<<"\t"<<p1<<"\t"<<p2<<endl;
                        int key = ++size_arr(1,1,d0r,d1r,d4r);
                        arr0(1,key,d0r,d1r,d4r) = p0;
                        arr0(2,key,d0r,d1r,d4r) = p1;
                        arr0(3,key,d0r,d1r,d4r) = p2;
                    }
                }
            }
        }
    }

example: cout<<p0&lt;&lt;"\t"&lt;<p1&lt;&lt;"\t"&lt;<p2&lt;<endl; int="" key="++size_arr(1,1,d0r,d1r,d4r);" arr0(1,key,d0r,d1r,d4r)="p0;" arr0(2,key,d0r,d1r,d4r)="p1;" arr0(3,key,d0r,d1r,d4r)="p2;" }="" }="" }="" }="" }="" example:="" <a="" href="https://www.youtube.com/watch?v=j0OzYY3aTdc&amp;feature=youtu.be">link text

vector<visualization_msgs::marker> line_list, marker; visualization_msgs::Marker text_view_facing; line_list.resize(3); marker.resize(3); for(int i=0;i<line_list.size();i++) {="" marker[i].header.frame_id="line_list[i].header.frame_id" =="" text_view_facing.header.frame_id="/camera_link" ;="" marker[i].header.stamp="line_list[i].header.stamp" =="" text_view_facing.header.stamp="ros::Time();" marker[i].ns="line_list[i].ns" =="" text_view_facing.ns="lines" ;="" marker[i].action="line_list[i].action" =="" text_view_facing.action="visualization_msgs::Marker::ADD;" marker[i].pose.orientation.w="line_list[i].pose.orientation.w" =="" text_view_facing.pose.orientation.w="1;" marker[i].id="i+3;" line_list[i].id="i;" marker[i].type="visualization_msgs::Marker::POINTS;" line_list[i].type="visualization_msgs::Marker::LINE_LIST;" line_list[i].color.r="0;" line_list[i].color.g="1;" line_list[i].color.="" b="1;" line_list[i].color.a="1;" marker[i].lifetime="ros::Duration(0.01);" marker[i].scale.x="0.003;" marker[i].scale.y="0.003;" marker[i].scale.z="0.003;" }="" text_view_facing.id="6;" text_view_facing.type="visualization_msgs::Marker::TEXT_VIEW_FACING;" text_view_facing.color.b="1;" text_view_facing.color.a="1.0;" text_view_facing.color.g="1.0;" text_view_facing.color.r="1.0;" text_view_facing.scale.z="0.015;" string="" obj1="first_segmented_image" ;="" bigarray="" size_arr,="" arr0;="" for(int="" p0="0;p0&lt;bound.size()" &amp;&amp;="" ros::ok();p0++)="" {="" p[0]="output.at(bound[p0].x,bound[p0].y);" if(!isnan(p[0].x)="" &amp;&amp;="" !isnan(p[0].y)="" &amp;&amp;="" !isnan(p[0].z))="" {="" point="" p0="" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" pc.r="1;" pc.g="0;" pc.b="0;" pc.a="1;" marker[0].points.clear();="" marker[0].colors.clear();="" marker[0].points.push_back(p);="" marker[0].colors.push_back(pc);="" marker_pub.publish(marker[0]);="" }="" for(int="" p1="0;p1&lt;bound.size()" &amp;&amp;="" ros::ok();p1++)="" {="" p[1]="output.at(bound[p1].x,bound[p1].y);" if(!isnan(p[1].x)="" &amp;&amp;="" !isnan(p[1].y)="" &amp;&amp;="" !isnan(p[1].z))="" {="" point="" p1="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" pc.r="0;" pc.g="1;" pc.b="0;" pc.a="1;" marker[1].points.clear();="" marker[1].colors.clear();="" marker[1].points.push_back(p);="" marker[1].colors.push_back(pc);="" marker_pub.publish(marker[1]);="" }="" if(!isnan(p[1].x)="" &amp;&amp;="" !isnan(p[1].y)="" &amp;&amp;="" !isnan(p[1].z))="" {="" line="" p0p1="" line_list[0].points.clear();="" line_list[0].colors.clear();="" line_list[0].scale.x="0.00075;" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" line_list[0].points.push_back(p);="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" line_list[0].points.push_back(p);="" marker_pub.publish(line_list[0]);="" }="" int="" d0r="sqrt(pow(p[1].x-p[0].x,2)+pow(p[1].y-p[0].y,2)+pow(p[1].z-p[0].z,2)" )*1000;="" if(d0r="=10)" {="" for(int="" p2="0;p2&lt;bound.size()" &amp;&amp;="" ros::ok();p2++)="" {="" p[2]="output.at(bound[p2].x,bound[p2].y);" if(!isnan(p[2].x)="" &amp;&amp;="" !isnan(p[2].y)="" &amp;&amp;="" !isnan(p[2].z))="" {="" point="" p2="" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" pc.r="0;" pc.g="0;" pc.b="1;" pc.a="1;" marker[2].points.clear();="" marker[2].colors.clear();="" marker[2].points.push_back(p);="" marker[2].colors.push_back(pc);="" marker_pub.publish(marker[2]);="" }="" if(!isnan(p[2].x)="" &amp;&amp;="" !isnan(p[2].y)="" &amp;&amp;="" !isnan(p[2].z))="" {="" line="" p1p2="" line_list[1].points.clear();="" line_list[1].colors.clear();="" line_list[1].scale.x="0.00075;" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" line_list[1].points.push_back(p);="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" line_list[1].points.push_back(p);="" marker_pub.publish(line_list[1]);="" line_list[2].points.clear();="" line_list[2].colors.clear();="" line="" p0p2="" line_list[2].scale.x="0.00075;" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" line_list[2].points.push_back(p);="" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" line_list[2].points.push_back(p);="" marker_pub.publish(line_list[2]);="" }="" int="" d1r="sqrt(pow(p[2].x-p[1].x,2)+pow(p[2].y-p[1].y,2)+pow(p[2].z-p[1].z,2)" )*1000;="" int="" d4r="sqrt(pow(p[2].x-p[0].x,2)+pow(p[2].y-p[0].y,2)+pow(p[2].z-p[0].z,2)" )*1000;="" if(d4r="">10 && d1r>10) { cout<<p0&lt;&lt;"\t"&lt;<p1&lt;&lt;"\t"&lt;<p2&lt;<endl; int="" key="++size_arr(1,1,d0r,d1r,d4r);" arr0(1,key,d0r,d1r,d4r)="p0;" arr0(2,key,d0r,d1r,d4r)="p1;" arr0(3,key,d0r,d1r,d4r)="p2;" }="" }="" }="" }="" }="" example:="" <a="" href="https://www.youtube.com/watch?v=j0OzYY3aTdc&amp;feature=youtu.be">link }<="" p="">

example: link text

vector<visualization_msgs::marker> line_list, marker; visualization_msgs::Marker text_view_facing; line_list.resize(3); marker.resize(3); for(int i=0;i<line_list.size();i++) {="" marker[i].header.frame_id="line_list[i].header.frame_id" =="" text_view_facing.header.frame_id="/camera_link" ;="" marker[i].header.stamp="line_list[i].header.stamp" =="" text_view_facing.header.stamp="ros::Time();" marker[i].ns="line_list[i].ns" =="" text_view_facing.ns="lines" ;="" marker[i].action="line_list[i].action" =="" text_view_facing.action="visualization_msgs::Marker::ADD;" marker[i].pose.orientation.w="line_list[i].pose.orientation.w" =="" text_view_facing.pose.orientation.w="1;" marker[i].id="i+3;" line_list[i].id="i;" marker[i].type="visualization_msgs::Marker::POINTS;" line_list[i].type="visualization_msgs::Marker::LINE_LIST;" line_list[i].color.r="0;" line_list[i].color.g="1;" line_list[i].color.="" b="1;" line_list[i].color.a="1;" marker[i].lifetime="ros::Duration(0.01);" marker[i].scale.x="0.003;" marker[i].scale.y="0.003;" marker[i].scale.z="0.003;" }="" text_view_facing.id="6;" text_view_facing.type="visualization_msgs::Marker::TEXT_VIEW_FACING;" text_view_facing.color.b="1;" text_view_facing.color.a="1.0;" text_view_facing.color.g="1.0;" text_view_facing.color.r="1.0;" text_view_facing.scale.z="0.015;" string="" obj1="first_segmented_image" ;="" bigarray="" size_arr,="" arr0;="" for(int="" p0="0;p0&lt;bound.size()" &amp;&amp;="" ros::ok();p0++)="" {="" p[0]="output.at(bound[p0].x,bound[p0].y);" if(!isnan(p[0].x)="" &amp;&amp;="" !isnan(p[0].y)="" &amp;&amp;="" !isnan(p[0].z))="" {="" point="" p0="" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" pc.r="1;" pc.g="0;" pc.b="0;" pc.a="1;" marker[0].points.clear();="" marker[0].colors.clear();="" marker[0].points.push_back(p);="" marker[0].colors.push_back(pc);="" marker_pub.publish(marker[0]);="" }="" for(int="" p1="0;p1&lt;bound.size()" &amp;&amp;="" ros::ok();p1++)="" {="" p[1]="output.at(bound[p1].x,bound[p1].y);" if(!isnan(p[1].x)="" &amp;&amp;="" !isnan(p[1].y)="" &amp;&amp;="" !isnan(p[1].z))="" {="" point="" p1="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" pc.r="0;" pc.g="1;" pc.b="0;" pc.a="1;" marker[1].points.clear();="" marker[1].colors.clear();="" marker[1].points.push_back(p);="" marker[1].colors.push_back(pc);="" marker_pub.publish(marker[1]);="" }="" if(!isnan(p[1].x)="" &amp;&amp;="" !isnan(p[1].y)="" &amp;&amp;="" !isnan(p[1].z))="" {="" line="" p0p1="" line_list[0].points.clear();="" line_list[0].colors.clear();="" line_list[0].scale.x="0.00075;" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" line_list[0].points.push_back(p);="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" line_list[0].points.push_back(p);="" marker_pub.publish(line_list[0]);="" }="" int="" d0r="sqrt(pow(p[1].x-p[0].x,2)+pow(p[1].y-p[0].y,2)+pow(p[1].z-p[0].z,2)" )*1000;="" if(d0r="=10)" {="" for(int="" p2="0;p2&lt;bound.size()" &amp;&amp;="" ros::ok();p2++)="" {="" p[2]="output.at(bound[p2].x,bound[p2].y);" if(!isnan(p[2].x)="" &amp;&amp;="" !isnan(p[2].y)="" &amp;&amp;="" !isnan(p[2].z))="" {="" point="" p2="" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" pc.r="0;" pc.g="0;" pc.b="1;" pc.a="1;" marker[2].points.clear();="" marker[2].colors.clear();="" marker[2].points.push_back(p);="" marker[2].colors.push_back(pc);="" marker_pub.publish(marker[2]);="" }="" if(!isnan(p[2].x)="" &amp;&amp;="" !isnan(p[2].y)="" &amp;&amp;="" !isnan(p[2].z))="" {="" line="" p1p2="" line_list[1].points.clear();="" line_list[1].colors.clear();="" line_list[1].scale.x="0.00075;" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" line_list[1].points.push_back(p);="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" line_list[1].points.push_back(p);="" marker_pub.publish(line_list[1]);="" line_list[2].points.clear();="" line_list[2].colors.clear();="" line="" p0p2="" line_list[2].scale.x="0.00075;" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" line_list[2].points.push_back(p);="" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" line_list[2].points.push_back(p);="" marker_pub.publish(line_list[2]);="" }="" int="" d1r="sqrt(pow(p[2].x-p[1].x,2)+pow(p[2].y-p[1].y,2)+pow(p[2].z-p[1].z,2)" )*1000;="" int="" d4r="sqrt(pow(p[2].x-p[0].x,2)+pow(p[2].y-p[0].y,2)+pow(p[2].z-p[0].z,2)" )*1000;="" if(d4r="">10 && d1r>10) { cout<<p0&lt;&lt;"\t"&lt;<p1&lt;&lt;"\t"&lt;<p2&lt;<endl; int="" key="++size_arr(1,1,d0r,d1r,d4r);" arr0(1,key,d0r,d1r,d4r)="p0;" arr0(2,key,d0r,d1r,d4r)="p1;" arr0(3,key,d0r,d1r,d4r)="p2;" }="" }="" }="" }="" }<="" p="">

example: link text

vector<visualization_msgs::marker> line_list, marker; visualization_msgs::Marker text_view_facing; line_list.resize(3); marker.resize(3); for(int i=0;i<line_list.size();i++) {="" marker[i].header.frame_id="line_list[i].header.frame_id" =="" text_view_facing.header.frame_id="/camera_link" ;="" marker[i].header.stamp="line_list[i].header.stamp" =="" text_view_facing.header.stamp="ros::Time();" marker[i].ns="line_list[i].ns" =="" text_view_facing.ns="lines" ;="" marker[i].action="line_list[i].action" =="" text_view_facing.action="visualization_msgs::Marker::ADD;" marker[i].pose.orientation.w="line_list[i].pose.orientation.w" =="" text_view_facing.pose.orientation.w="1;" marker[i].id="i+3;" line_list[i].id="i;" marker[i].type="visualization_msgs::Marker::POINTS;" line_list[i].type="visualization_msgs::Marker::LINE_LIST;" line_list[i].color.r="0;" line_list[i].color.g="1;" line_list[i].color.="" b="1;" line_list[i].color.a="1;" marker[i].lifetime="ros::Duration(0.01);" marker[i].scale.x="0.003;" marker[i].scale.y="0.003;" marker[i].scale.z="0.003;" }="" text_view_facing.id="6;" text_view_facing.type="visualization_msgs::Marker::TEXT_VIEW_FACING;" text_view_facing.color.b="1;" text_view_facing.color.a="1.0;" text_view_facing.color.g="1.0;" text_view_facing.color.r="1.0;" text_view_facing.scale.z="0.015;" string="" obj1="first_segmented_image" ;="" bigarray="" size_arr,="" arr0;="" for(int="" p0="0;p0&lt;bound.size()" &amp;&amp;="" ros::ok();p0++)="" {="" p[0]="output.at(bound[p0].x,bound[p0].y);" if(!isnan(p[0].x)="" &amp;&amp;="" !isnan(p[0].y)="" &amp;&amp;="" !isnan(p[0].z))="" {="" point="" p0="" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" pc.r="1;" pc.g="0;" pc.b="0;" pc.a="1;" marker[0].points.clear();="" marker[0].colors.clear();="" marker[0].points.push_back(p);="" marker[0].colors.push_back(pc);="" marker_pub.publish(marker[0]);="" }="" for(int="" p1="0;p1&lt;bound.size()" &amp;&amp;="" ros::ok();p1++)="" {="" p[1]="output.at(bound[p1].x,bound[p1].y);" if(!isnan(p[1].x)="" &amp;&amp;="" !isnan(p[1].y)="" &amp;&amp;="" !isnan(p[1].z))="" {="" point="" p1="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" pc.r="0;" pc.g="1;" pc.b="0;" pc.a="1;" marker[1].points.clear();="" marker[1].colors.clear();="" marker[1].points.push_back(p);="" marker[1].colors.push_back(pc);="" marker_pub.publish(marker[1]);="" }="" if(!isnan(p[1].x)="" &amp;&amp;="" !isnan(p[1].y)="" &amp;&amp;="" !isnan(p[1].z))="" {="" line="" p0p1="" line_list[0].points.clear();="" line_list[0].colors.clear();="" line_list[0].scale.x="0.00075;" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" line_list[0].points.push_back(p);="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" line_list[0].points.push_back(p);="" marker_pub.publish(line_list[0]);="" }="" int="" d0r="sqrt(pow(p[1].x-p[0].x,2)+pow(p[1].y-p[0].y,2)+pow(p[1].z-p[0].z,2)" )*1000;="" if(d0r="=10)" {="" for(int="" p2="0;p2&lt;bound.size()" &amp;&amp;="" ros::ok();p2++)="" {="" p[2]="output.at(bound[p2].x,bound[p2].y);" if(!isnan(p[2].x)="" &amp;&amp;="" !isnan(p[2].y)="" &amp;&amp;="" !isnan(p[2].z))="" {="" point="" p2="" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" pc.r="0;" pc.g="0;" pc.b="1;" pc.a="1;" marker[2].points.clear();="" marker[2].colors.clear();="" marker[2].points.push_back(p);="" marker[2].colors.push_back(pc);="" marker_pub.publish(marker[2]);="" }="" if(!isnan(p[2].x)="" &amp;&amp;="" !isnan(p[2].y)="" &amp;&amp;="" !isnan(p[2].z))="" {="" line="" p1p2="" line_list[1].points.clear();="" line_list[1].colors.clear();="" line_list[1].scale.x="0.00075;" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" line_list[1].points.push_back(p);="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" line_list[1].points.push_back(p);="" marker_pub.publish(line_list[1]);="" line_list[2].points.clear();="" line_list[2].colors.clear();="" line="" p0p2="" line_list[2].scale.x="0.00075;" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" line_list[2].points.push_back(p);="" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" line_list[2].points.push_back(p);="" marker_pub.publish(line_list[2]);="" }="" int="" d1r="sqrt(pow(p[2].x-p[1].x,2)+pow(p[2].y-p[1].y,2)+pow(p[2].z-p[1].z,2)" )*1000;="" int="" d4r="sqrt(pow(p[2].x-p[0].x,2)+pow(p[2].y-p[0].y,2)+pow(p[2].z-p[0].z,2)" )*1000;="" if(d4r="">10 && d1r>10) { cout<<p0&lt;&lt;"\t"&lt;<p1&lt;&lt;"\t"&lt;<p2&lt;<endl; int="" key="++size_arr(1,1,d0r,d1r,d4r);" arr0(1,key,d0r,d1r,d4r)="p0;" arr0(2,key,d0r,d1r,d4r)="p1;" arr0(3,key,d0r,d1r,d4r)="p2;" }="" }="" }="" }="" }<="" p="">

example: link text

vector<visualization_msgs::marker> line_list, marker; visualization_msgs::Marker text_view_facing; line_list.resize(3); marker.resize(3); for(int i=0;i<line_list.size();i++) {="" marker[i].header.frame_id="line_list[i].header.frame_id" =="" text_view_facing.header.frame_id="/camera_link" ;="" marker[i].header.stamp="line_list[i].header.stamp" =="" text_view_facing.header.stamp="ros::Time();" marker[i].ns="line_list[i].ns" =="" text_view_facing.ns="lines" ;="" marker[i].action="line_list[i].action" =="" text_view_facing.action="visualization_msgs::Marker::ADD;" marker[i].pose.orientation.w="line_list[i].pose.orientation.w" =="" text_view_facing.pose.orientation.w="1;" marker[i].id="i+3;" line_list[i].id="i;" marker[i].type="visualization_msgs::Marker::POINTS;" line_list[i].type="visualization_msgs::Marker::LINE_LIST;" line_list[i].color.r="0;" line_list[i].color.g="1;" line_list[i].color.="" b="1;" line_list[i].color.a="1;" marker[i].lifetime="ros::Duration(0.01);" marker[i].scale.x="0.003;" marker[i].scale.y="0.003;" marker[i].scale.z="0.003;" }="" text_view_facing.id="6;" text_view_facing.type="visualization_msgs::Marker::TEXT_VIEW_FACING;" text_view_facing.color.b="1;" text_view_facing.color.a="1.0;" text_view_facing.color.g="1.0;" text_view_facing.color.r="1.0;" text_view_facing.scale.z="0.015;" string="" obj1="first_segmented_image" ;="" bigarray="" size_arr,="" arr0;="" for(int="" p0="0;p0&lt;bound.size()" &amp;&amp;="" ros::ok();p0++)="" {="" p[0]="output.at(bound[p0].x,bound[p0].y);" if(!isnan(p[0].x)="" &amp;&amp;="" !isnan(p[0].y)="" &amp;&amp;="" !isnan(p[0].z))="" {="" point="" p0="" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" pc.r="1;" pc.g="0;" pc.b="0;" pc.a="1;" marker[0].points.clear();="" marker[0].colors.clear();="" marker[0].points.push_back(p);="" marker[0].colors.push_back(pc);="" marker_pub.publish(marker[0]);="" }="" for(int="" p1="0;p1&lt;bound.size()" &amp;&amp;="" ros::ok();p1++)="" {="" p[1]="output.at(bound[p1].x,bound[p1].y);" if(!isnan(p[1].x)="" &amp;&amp;="" !isnan(p[1].y)="" &amp;&amp;="" !isnan(p[1].z))="" {="" point="" p1="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" pc.r="0;" pc.g="1;" pc.b="0;" pc.a="1;" marker[1].points.clear();="" marker[1].colors.clear();="" marker[1].points.push_back(p);="" marker[1].colors.push_back(pc);="" marker_pub.publish(marker[1]);="" }="" if(!isnan(p[1].x)="" &amp;&amp;="" !isnan(p[1].y)="" &amp;&amp;="" !isnan(p[1].z))="" {="" line="" p0p1="" line_list[0].points.clear();="" line_list[0].colors.clear();="" line_list[0].scale.x="0.00075;" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" line_list[0].points.push_back(p);="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" line_list[0].points.push_back(p);="" marker_pub.publish(line_list[0]);="" }="" int="" d0r="sqrt(pow(p[1].x-p[0].x,2)+pow(p[1].y-p[0].y,2)+pow(p[1].z-p[0].z,2)" )*1000;="" if(d0r="=10)" {="" for(int="" p2="0;p2&lt;bound.size()" &amp;&amp;="" ros::ok();p2++)="" {="" p[2]="output.at(bound[p2].x,bound[p2].y);" if(!isnan(p[2].x)="" &amp;&amp;="" !isnan(p[2].y)="" &amp;&amp;="" !isnan(p[2].z))="" {="" point="" p2="" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" pc.r="0;" pc.g="0;" pc.b="1;" pc.a="1;" marker[2].points.clear();="" marker[2].colors.clear();="" marker[2].points.push_back(p);="" marker[2].colors.push_back(pc);="" marker_pub.publish(marker[2]);="" }="" if(!isnan(p[2].x)="" &amp;&amp;="" !isnan(p[2].y)="" &amp;&amp;="" !isnan(p[2].z))="" {="" line="" p1p2="" line_list[1].points.clear();="" line_list[1].colors.clear();="" line_list[1].scale.x="0.00075;" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" line_list[1].points.push_back(p);="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" line_list[1].points.push_back(p);="" marker_pub.publish(line_list[1]);="" line_list[2].points.clear();="" line_list[2].colors.clear();="" line="" p0p2="" line_list[2].scale.x="0.00075;" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" line_list[2].points.push_back(p);="" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" line_list[2].points.push_back(p);="" marker_pub.publish(line_list[2]);="" }="" int="" d1r="sqrt(pow(p[2].x-p[1].x,2)+pow(p[2].y-p[1].y,2)+pow(p[2].z-p[1].z,2)" )*1000;="" int="" d4r="sqrt(pow(p[2].x-p[0].x,2)+pow(p[2].y-p[0].y,2)+pow(p[2].z-p[0].z,2)" )*1000;="" if(d4r="">10 && d1r>10) { cout<<p0&lt;&lt;"\t"&lt;<p1&lt;&lt;"\t"&lt;<p2&lt;<endl; int="" key="++size_arr(1,1,d0r,d1r,d4r);" arr0(1,key,d0r,d1r,d4r)="p0;" arr0(2,key,d0r,d1r,d4r)="p1;" arr0(3,key,d0r,d1r,d4r)="p2;" }="" }="" }="" }="" }<="" p="">

example: link text

vector<visualization_msgs::marker> line_list, marker; visualization_msgs::Marker text_view_facing; line_list.resize(3); marker.resize(3); for(int i=0;i<line_list.size();i++) {="" marker[i].header.frame_id="line_list[i].header.frame_id" =="" text_view_facing.header.frame_id="/camera_link" ;="" marker[i].header.stamp="line_list[i].header.stamp" =="" text_view_facing.header.stamp="ros::Time();" marker[i].ns="line_list[i].ns" =="" text_view_facing.ns="lines" ;="" marker[i].action="line_list[i].action" =="" text_view_facing.action="visualization_msgs::Marker::ADD;" marker[i].pose.orientation.w="line_list[i].pose.orientation.w" =="" text_view_facing.pose.orientation.w="1;" marker[i].id="i+3;" line_list[i].id="i;" marker[i].type="visualization_msgs::Marker::POINTS;" line_list[i].type="visualization_msgs::Marker::LINE_LIST;" line_list[i].color.r="0;" line_list[i].color.g="1;" line_list[i].color.="" b="1;" line_list[i].color.a="1;" marker[i].lifetime="ros::Duration(0.01);" marker[i].scale.x="0.003;" marker[i].scale.y="0.003;" marker[i].scale.z="0.003;" }="" text_view_facing.id="6;" text_view_facing.type="visualization_msgs::Marker::TEXT_VIEW_FACING;" text_view_facing.color.b="1;" text_view_facing.color.a="1.0;" text_view_facing.color.g="1.0;" text_view_facing.color.r="1.0;" text_view_facing.scale.z="0.015;" string="" obj1="first_segmented_image" ;="" bigarray="" size_arr,="" arr0;="" for(int="" p0="0;p0&lt;bound.size()" &amp;&amp;="" ros::ok();p0++)="" {="" p[0]="output.at(bound[p0].x,bound[p0].y);" if(!isnan(p[0].x)="" &amp;&amp;="" !isnan(p[0].y)="" &amp;&amp;="" !isnan(p[0].z))="" {="" point="" p0="" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" pc.r="1;" pc.g="0;" pc.b="0;" pc.a="1;" marker[0].points.clear();="" marker[0].colors.clear();="" marker[0].points.push_back(p);="" marker[0].colors.push_back(pc);="" marker_pub.publish(marker[0]);="" }="" for(int="" p1="0;p1&lt;bound.size()" &amp;&amp;="" ros::ok();p1++)="" {="" p[1]="output.at(bound[p1].x,bound[p1].y);" if(!isnan(p[1].x)="" &amp;&amp;="" !isnan(p[1].y)="" &amp;&amp;="" !isnan(p[1].z))="" {="" point="" p1="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" pc.r="0;" pc.g="1;" pc.b="0;" pc.a="1;" marker[1].points.clear();="" marker[1].colors.clear();="" marker[1].points.push_back(p);="" marker[1].colors.push_back(pc);="" marker_pub.publish(marker[1]);="" }="" if(!isnan(p[1].x)="" &amp;&amp;="" !isnan(p[1].y)="" &amp;&amp;="" !isnan(p[1].z))="" {="" line="" p0p1="" line_list[0].points.clear();="" line_list[0].colors.clear();="" line_list[0].scale.x="0.00075;" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" line_list[0].points.push_back(p);="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" line_list[0].points.push_back(p);="" marker_pub.publish(line_list[0]);="" }="" int="" d0r="sqrt(pow(p[1].x-p[0].x,2)+pow(p[1].y-p[0].y,2)+pow(p[1].z-p[0].z,2)" )*1000;="" if(d0r="=10)" {="" for(int="" p2="0;p2&lt;bound.size()" &amp;&amp;="" ros::ok();p2++)="" {="" p[2]="output.at(bound[p2].x,bound[p2].y);" if(!isnan(p[2].x)="" &amp;&amp;="" !isnan(p[2].y)="" &amp;&amp;="" !isnan(p[2].z))="" {="" point="" p2="" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" pc.r="0;" pc.g="0;" pc.b="1;" pc.a="1;" marker[2].points.clear();="" marker[2].colors.clear();="" marker[2].points.push_back(p);="" marker[2].colors.push_back(pc);="" marker_pub.publish(marker[2]);="" }="" if(!isnan(p[2].x)="" &amp;&amp;="" !isnan(p[2].y)="" &amp;&amp;="" !isnan(p[2].z))="" {="" line="" p1p2="" line_list[1].points.clear();="" line_list[1].colors.clear();="" line_list[1].scale.x="0.00075;" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" line_list[1].points.push_back(p);="" p.x="p[1].x;" p.y="p[1].y;" p.z="p[1].z;" line_list[1].points.push_back(p);="" marker_pub.publish(line_list[1]);="" line_list[2].points.clear();="" line_list[2].colors.clear();="" line="" p0p2="" line_list[2].scale.x="0.00075;" p.x="p[2].x;" p.y="p[2].y;" p.z="p[2].z;" line_list[2].points.push_back(p);="" p.x="p[0].x;" p.y="p[0].y;" p.z="p[0].z;" line_list[2].points.push_back(p);="" marker_pub.publish(line_list[2]);="" }="" int="" d1r="sqrt(pow(p[2].x-p[1].x,2)+pow(p[2].y-p[1].y,2)+pow(p[2].z-p[1].z,2)" )*1000;="" int="" d4r="sqrt(pow(p[2].x-p[0].x,2)+pow(p[2].y-p[0].y,2)+pow(p[2].z-p[0].z,2)" )*1000;="" if(d4r="">10 && d1r>10) { cout<<p0&lt;&lt;"\t"&lt;<p1&lt;&lt;"\t"&lt;<p2&lt;<endl; int="" key="++size_arr(1,1,d0r,d1r,d4r);" arr0(1,key,d0r,d1r,d4r)="p0;" arr0(2,key,d0r,d1r,d4r)="p1;" arr0(3,key,d0r,d1r,d4r)="p2;" }="" }="" }="" }="" }<="" p="">

example: link textanswer is below the question itself:

answer is below the question itself:itself, since i'm getting some problem in editing and formatting the code in here.

click to hide/show revision 9
No.9 Revision

updated 2017-02-01 09:24:56 -0500

gvdhoorn gravatar image

Found the answer is below the question itself, since i'm getting some problem in editing and formatting the code in here.myself.

demonstration: youtube.com/watch?v=j0OzYY3aTdc

code:

vector<visualization_msgs::Marker> line_list, marker;
visualization_msgs::Marker text_view_facing;
line_list.resize(3);
marker.resize(3);
for (int i = 0; i < line_list.size(); i++) {
  marker[i].header.frame_id = line_list[i].header.frame_id =
      text_view_facing.header.frame_id = "/camera_link";
  marker[i].header.stamp = line_list[i].header.stamp =
      text_view_facing.header.stamp = ros::Time();
  marker[i].ns = line_list[i].ns = text_view_facing.ns = "lines";
  marker[i].action = line_list[i].action = text_view_facing.action =
      visualization_msgs::Marker::ADD;
  marker[i].pose.orientation.w = line_list[i].pose.orientation.w =
      text_view_facing.pose.orientation.w = 1;
  marker[i].id = i + 3;
  line_list[i].id = i;
  marker[i].type = visualization_msgs::Marker::POINTS;
  line_list[i].type = visualization_msgs::Marker::LINE_LIST;
  line_list[i].color.r = 0;
  line_list[i].color.g = 1;
  line_list[i].color.b = 1;
  line_list[i].color.a = 1;
  marker[i].lifetime = ros::Duration(0.01);
  marker[i].scale.x = 0.003;
  marker[i].scale.y = 0.003;
  marker[i].scale.z = 0.003;
}
text_view_facing.id = 6;
text_view_facing.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
text_view_facing.color.b = 1;
text_view_facing.color.a = 1.0;
text_view_facing.color.g = 1.0;
text_view_facing.color.r = 1.0;
text_view_facing.scale.z = 0.015;
string obj1 = "first_segmented_image";
bigarray size_arr, arr0;
for (int p0 = 0; p0 < bound.size() && ros::ok(); p0++) {
  p[0] = output.at(bound[p0].x, bound[p0].y);
  if (!isnan(p[0].x) && !isnan(p[0].y) && !isnan(p[0].z)) {  // point p0
    P.x = p[0].x;
    P.y = p[0].y;
    P.z = p[0].z;
    pc.r = 1;
    pc.g = 0;
    pc.b = 0;
    pc.a = 1;
    marker[0].points.clear();
    marker[0].colors.clear();
    marker[0].points.push_back(P);
    marker[0].colors.push_back(pc);
    marker_pub.publish(marker[0]);
  }
  for (int p1 = 0; p1 < bound.size() && ros::ok(); p1++) {
    p[1] = output.at(bound[p1].x, bound[p1].y);
    if (!isnan(p[1].x) && !isnan(p[1].y) && !isnan(p[1].z)) {  // point p1
      P.x = p[1].x;
      P.y = p[1].y;
      P.z = p[1].z;
      pc.r = 0;
      pc.g = 1;
      pc.b = 0;
      pc.a = 1;
      marker[1].points.clear();
      marker[1].colors.clear();
      marker[1].points.push_back(P);
      marker[1].colors.push_back(pc);
      marker_pub.publish(marker[1]);
    }
    if (!isnan(p[1].x) && !isnan(p[1].y) && !isnan(p[1].z)) {  // line p0p1
      line_list[0].points.clear();
      line_list[0].colors.clear();
      line_list[0].scale.x = 0.00075;
      P.x = p[0].x;
      P.y = p[0].y;
      P.z = p[0].z;
      line_list[0].points.push_back(P);
      P.x = p[1].x;
      P.y = p[1].y;
      P.z = p[1].z;
      line_list[0].points.push_back(P);
      marker_pub.publish(line_list[0]);
    }
    int d0r = sqrt(pow(p[1].x - p[0].x, 2) + pow(p[1].y - p[0].y, 2) +
                   pow(p[1].z - p[0].z, 2)) *
              1000;
    if (d0r == 10) {
      for (int p2 = 0; p2 < bound.size() && ros::ok(); p2++) {
        p[2] = output.at(bound[p2].x, bound[p2].y);
        if (!isnan(p[2].x) && !isnan(p[2].y) && !isnan(p[2].z)) {  // point p2
          P.x = p[2].x;
          P.y = p[2].y;
          P.z = p[2].z;
          pc.r = 0;
          pc.g = 0;
          pc.b = 1;
          pc.a = 1;
          marker[2].points.clear();
          marker[2].colors.clear();
          marker[2].points.push_back(P);
          marker[2].colors.push_back(pc);
          marker_pub.publish(marker[2]);
        }
        if (!isnan(p[2].x) && !isnan(p[2].y) && !isnan(p[2].z)) {  // line p1p2
          line_list[1].points.clear();
          line_list[1].colors.clear();
          line_list[1].scale.x = 0.00075;
          P.x = p[2].x;
          P.y = p[2].y;
          P.z = p[2].z;
          line_list[1].points.push_back(P);
          P.x = p[1].x;
          P.y = p[1].y;
          P.z = p[1].z;
          line_list[1].points.push_back(P);
          marker_pub.publish(line_list[1]);
          line_list[2].points.clear();
          line_list[2].colors.clear();  // line p0p2
          line_list[2].scale.x = 0.00075;
          P.x = p[2].x;
          P.y = p[2].y;
          P.z = p[2].z;
          line_list[2].points.push_back(P);
          P.x = p[0].x;
          P.y = p[0].y;
          P.z = p[0].z;
          line_list[2].points.push_back(P);
          marker_pub.publish(line_list[2]);
        }
        int d1r = sqrt(pow(p[2].x - p[1].x, 2) + pow(p[2].y - p[1].y, 2) +
                       pow(p[2].z - p[1].z, 2)) *
                  1000;
        int d4r = sqrt(pow(p[2].x - p[0].x, 2) + pow(p[2].y - p[0].y, 2) +
                       pow(p[2].z - p[0].z, 2)) *
                  1000;
        if (d4r > 10 && d1r > 10) {
          cout << p0 << "\t" << p1 << "\t" << p2 << endl;
          int key = ++size_arr(1, 1, d0r, d1r, d4r);
          arr0(1, key, d0r, d1r, d4r) = p0;
          arr0(2, key, d0r, d1r, d4r) = p1;
          arr0(3, key, d0r, d1r, d4r) = p2;
        }
      }
    }
  }
}

Found the answer myself.

demonstration: youtube.com/watch?v=j0OzYY3aTdc

Here what i did basically is i created a vector of line_list and markers of type visualization_msgs::Marker, in this way now i can use more than one line_list and marker by using its index, so if i want to delete given specific marker no other markers will be deleted. Here i also had to specify different marker id for different line list and marker so that rviz can identify tham seperately. so basic is very simple: 1. make vector of marker instead of single marker. 2. specify different marker id for each of them, and delete any one of them whenever u want. code:

vector<visualization_msgs::Marker> line_list, marker;
visualization_msgs::Marker text_view_facing;
line_list.resize(3);
marker.resize(3);
for (int i = 0; i < line_list.size(); i++) {
  marker[i].header.frame_id = line_list[i].header.frame_id =
      text_view_facing.header.frame_id = "/camera_link";
  marker[i].header.stamp = line_list[i].header.stamp =
      text_view_facing.header.stamp = ros::Time();
  marker[i].ns = line_list[i].ns = text_view_facing.ns = "lines";
  marker[i].action = line_list[i].action = text_view_facing.action =
      visualization_msgs::Marker::ADD;
  marker[i].pose.orientation.w = line_list[i].pose.orientation.w =
      text_view_facing.pose.orientation.w = 1;
  marker[i].id = i + 3;
  line_list[i].id = i;
  marker[i].type = visualization_msgs::Marker::POINTS;
  line_list[i].type = visualization_msgs::Marker::LINE_LIST;
  line_list[i].color.r = 0;
  line_list[i].color.g = 1;
  line_list[i].color.b = 1;
  line_list[i].color.a = 1;
  marker[i].lifetime = ros::Duration(0.01);
  marker[i].scale.x = 0.003;
  marker[i].scale.y = 0.003;
  marker[i].scale.z = 0.003;
}
text_view_facing.id = 6;
text_view_facing.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
text_view_facing.color.b = 1;
text_view_facing.color.a = 1.0;
text_view_facing.color.g = 1.0;
text_view_facing.color.r = 1.0;
text_view_facing.scale.z = 0.015;
string obj1 = "first_segmented_image";
bigarray size_arr, arr0;
for (int p0 = 0; p0 < bound.size() && ros::ok(); p0++) {
  p[0] = output.at(bound[p0].x, bound[p0].y);
  if (!isnan(p[0].x) && !isnan(p[0].y) && !isnan(p[0].z)) {  // point p0
    P.x = p[0].x;
    P.y = p[0].y;
    P.z = p[0].z;
    pc.r = 1;
    pc.g = 0;
    pc.b = 0;
    pc.a = 1;
    marker[0].points.clear();
    marker[0].colors.clear();
    marker[0].points.push_back(P);
    marker[0].colors.push_back(pc);
    marker_pub.publish(marker[0]);
  }
  for (int p1 = 0; p1 < bound.size() && ros::ok(); p1++) {
    p[1] = output.at(bound[p1].x, bound[p1].y);
    if (!isnan(p[1].x) && !isnan(p[1].y) && !isnan(p[1].z)) {  // point p1
      P.x = p[1].x;
      P.y = p[1].y;
      P.z = p[1].z;
      pc.r = 0;
      pc.g = 1;
      pc.b = 0;
      pc.a = 1;
      marker[1].points.clear();
      marker[1].colors.clear();
      marker[1].points.push_back(P);
      marker[1].colors.push_back(pc);
      marker_pub.publish(marker[1]);
    }
    if (!isnan(p[1].x) && !isnan(p[1].y) && !isnan(p[1].z)) {  // line p0p1
      line_list[0].points.clear();
      line_list[0].colors.clear();
      line_list[0].scale.x = 0.00075;
      P.x = p[0].x;
      P.y = p[0].y;
      P.z = p[0].z;
      line_list[0].points.push_back(P);
      P.x = p[1].x;
      P.y = p[1].y;
      P.z = p[1].z;
      line_list[0].points.push_back(P);
      marker_pub.publish(line_list[0]);
    }
    int d0r = sqrt(pow(p[1].x - p[0].x, 2) + pow(p[1].y - p[0].y, 2) +
                   pow(p[1].z - p[0].z, 2)) *
              1000;
    if (d0r == 10) {
      for (int p2 = 0; p2 < bound.size() && ros::ok(); p2++) {
        p[2] = output.at(bound[p2].x, bound[p2].y);
        if (!isnan(p[2].x) && !isnan(p[2].y) && !isnan(p[2].z)) {  // point p2
          P.x = p[2].x;
          P.y = p[2].y;
          P.z = p[2].z;
          pc.r = 0;
          pc.g = 0;
          pc.b = 1;
          pc.a = 1;
          marker[2].points.clear();
          marker[2].colors.clear();
          marker[2].points.push_back(P);
          marker[2].colors.push_back(pc);
          marker_pub.publish(marker[2]);
        }
        if (!isnan(p[2].x) && !isnan(p[2].y) && !isnan(p[2].z)) {  // line p1p2
          line_list[1].points.clear();
          line_list[1].colors.clear();
          line_list[1].scale.x = 0.00075;
          P.x = p[2].x;
          P.y = p[2].y;
          P.z = p[2].z;
          line_list[1].points.push_back(P);
          P.x = p[1].x;
          P.y = p[1].y;
          P.z = p[1].z;
          line_list[1].points.push_back(P);
          marker_pub.publish(line_list[1]);
          line_list[2].points.clear();
          line_list[2].colors.clear();  // line p0p2
          line_list[2].scale.x = 0.00075;
          P.x = p[2].x;
          P.y = p[2].y;
          P.z = p[2].z;
          line_list[2].points.push_back(P);
          P.x = p[0].x;
          P.y = p[0].y;
          P.z = p[0].z;
          line_list[2].points.push_back(P);
          marker_pub.publish(line_list[2]);
        }
        int d1r = sqrt(pow(p[2].x - p[1].x, 2) + pow(p[2].y - p[1].y, 2) +
                       pow(p[2].z - p[1].z, 2)) *
                  1000;
        int d4r = sqrt(pow(p[2].x - p[0].x, 2) + pow(p[2].y - p[0].y, 2) +
                       pow(p[2].z - p[0].z, 2)) *
                  1000;
        if (d4r > 10 && d1r > 10) {
          cout << p0 << "\t" << p1 << "\t" << p2 << endl;
          int key = ++size_arr(1, 1, d0r, d1r, d4r);
          arr0(1, key, d0r, d1r, d4r) = p0;
          arr0(2, key, d0r, d1r, d4r) = p1;
          arr0(3, key, d0r, d1r, d4r) = p2;
        }
      }
    }
  }
}

Found the answer myself.

demonstration: youtube.com/watch?v=j0OzYY3aTdc

Here what i did basically is i created a vector of line_list and markers of type visualization_msgs::Marker, in this way now i can use more than one line_list and marker by using its index, so if i want to delete given specific marker no other markers will be deleted. Here i also had to specify different marker id for different line list and marker so that rviz can identify tham seperately. so basic is very simple: 1.

  1. make vector of marker instead of single marker. 2.
  2. specify different marker id for each of them, and delete any one of them whenever u want. want.

code:

vector<visualization_msgs::Marker> line_list, marker;
visualization_msgs::Marker text_view_facing;
line_list.resize(3);
marker.resize(3);
for (int i = 0; i < line_list.size(); i++) {
  marker[i].header.frame_id = line_list[i].header.frame_id =
      text_view_facing.header.frame_id = "/camera_link";
  marker[i].header.stamp = line_list[i].header.stamp =
      text_view_facing.header.stamp = ros::Time();
  marker[i].ns = line_list[i].ns = text_view_facing.ns = "lines";
  marker[i].action = line_list[i].action = text_view_facing.action =
      visualization_msgs::Marker::ADD;
  marker[i].pose.orientation.w = line_list[i].pose.orientation.w =
      text_view_facing.pose.orientation.w = 1;
  marker[i].id = i + 3;
  line_list[i].id = i;
  marker[i].type = visualization_msgs::Marker::POINTS;
  line_list[i].type = visualization_msgs::Marker::LINE_LIST;
  line_list[i].color.r = 0;
  line_list[i].color.g = 1;
  line_list[i].color.b = 1;
  line_list[i].color.a = 1;
  marker[i].lifetime = ros::Duration(0.01);
  marker[i].scale.x = 0.003;
  marker[i].scale.y = 0.003;
  marker[i].scale.z = 0.003;
}
text_view_facing.id = 6;
text_view_facing.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
text_view_facing.color.b = 1;
text_view_facing.color.a = 1.0;
text_view_facing.color.g = 1.0;
text_view_facing.color.r = 1.0;
text_view_facing.scale.z = 0.015;
string obj1 = "first_segmented_image";
bigarray size_arr, arr0;
for (int p0 = 0; p0 < bound.size() && ros::ok(); p0++) {
  p[0] = output.at(bound[p0].x, bound[p0].y);
  if (!isnan(p[0].x) && !isnan(p[0].y) && !isnan(p[0].z)) {  // point p0
    P.x = p[0].x;
    P.y = p[0].y;
    P.z = p[0].z;
    pc.r = 1;
    pc.g = 0;
    pc.b = 0;
    pc.a = 1;
    marker[0].points.clear();
    marker[0].colors.clear();
    marker[0].points.push_back(P);
    marker[0].colors.push_back(pc);
    marker_pub.publish(marker[0]);
  }
  for (int p1 = 0; p1 < bound.size() && ros::ok(); p1++) {
    p[1] = output.at(bound[p1].x, bound[p1].y);
    if (!isnan(p[1].x) && !isnan(p[1].y) && !isnan(p[1].z)) {  // point p1
      P.x = p[1].x;
      P.y = p[1].y;
      P.z = p[1].z;
      pc.r = 0;
      pc.g = 1;
      pc.b = 0;
      pc.a = 1;
      marker[1].points.clear();
      marker[1].colors.clear();
      marker[1].points.push_back(P);
      marker[1].colors.push_back(pc);
      marker_pub.publish(marker[1]);
    }
    if (!isnan(p[1].x) && !isnan(p[1].y) && !isnan(p[1].z)) {  // line p0p1
      line_list[0].points.clear();
      line_list[0].colors.clear();
      line_list[0].scale.x = 0.00075;
      P.x = p[0].x;
      P.y = p[0].y;
      P.z = p[0].z;
      line_list[0].points.push_back(P);
      P.x = p[1].x;
      P.y = p[1].y;
      P.z = p[1].z;
      line_list[0].points.push_back(P);
      marker_pub.publish(line_list[0]);
    }
    int d0r = sqrt(pow(p[1].x - p[0].x, 2) + pow(p[1].y - p[0].y, 2) +
                   pow(p[1].z - p[0].z, 2)) *
              1000;
    if (d0r == 10) {
      for (int p2 = 0; p2 < bound.size() && ros::ok(); p2++) {
        p[2] = output.at(bound[p2].x, bound[p2].y);
        if (!isnan(p[2].x) && !isnan(p[2].y) && !isnan(p[2].z)) {  // point p2
          P.x = p[2].x;
          P.y = p[2].y;
          P.z = p[2].z;
          pc.r = 0;
          pc.g = 0;
          pc.b = 1;
          pc.a = 1;
          marker[2].points.clear();
          marker[2].colors.clear();
          marker[2].points.push_back(P);
          marker[2].colors.push_back(pc);
          marker_pub.publish(marker[2]);
        }
        if (!isnan(p[2].x) && !isnan(p[2].y) && !isnan(p[2].z)) {  // line p1p2
          line_list[1].points.clear();
          line_list[1].colors.clear();
          line_list[1].scale.x = 0.00075;
          P.x = p[2].x;
          P.y = p[2].y;
          P.z = p[2].z;
          line_list[1].points.push_back(P);
          P.x = p[1].x;
          P.y = p[1].y;
          P.z = p[1].z;
          line_list[1].points.push_back(P);
          marker_pub.publish(line_list[1]);
          line_list[2].points.clear();
          line_list[2].colors.clear();  // line p0p2
          line_list[2].scale.x = 0.00075;
          P.x = p[2].x;
          P.y = p[2].y;
          P.z = p[2].z;
          line_list[2].points.push_back(P);
          P.x = p[0].x;
          P.y = p[0].y;
          P.z = p[0].z;
          line_list[2].points.push_back(P);
          marker_pub.publish(line_list[2]);
        }
        int d1r = sqrt(pow(p[2].x - p[1].x, 2) + pow(p[2].y - p[1].y, 2) +
                       pow(p[2].z - p[1].z, 2)) *
                  1000;
        int d4r = sqrt(pow(p[2].x - p[0].x, 2) + pow(p[2].y - p[0].y, 2) +
                       pow(p[2].z - p[0].z, 2)) *
                  1000;
        if (d4r > 10 && d1r > 10) {
          cout << p0 << "\t" << p1 << "\t" << p2 << endl;
          int key = ++size_arr(1, 1, d0r, d1r, d4r);
          arr0(1, key, d0r, d1r, d4r) = p0;
          arr0(2, key, d0r, d1r, d4r) = p1;
          arr0(3, key, d0r, d1r, d4r) = p2;
        }
      }
    }
  }
}
ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.
about | faq | help | privacy policy | terms of service
Powered by Askbot version 0.10.2