Environment Perception: 3D Truss Environment Mapping and Parametric Expression Extraction

Experiments

Preparation

roscore
rosrun pcl_ros pcd_to_pointcloud ~/.ros/wh2_lg707070_1ms0.01_filtered.pcd _frame_id:=/map

In the directory of ~/truss_ws/src/truss

rviz -d node_pole.rviz

In another terminal, open rviz -> Panels -> Selection. Choose Select, then left click the point whose position you want to know. On the **left Selection ** you can see it.

The point on the pole it's position is:

// X Y Z
0.251859
0.0151518
-0.0633736

Modify values of RES and init_x, init_y and init_z of the truss_pointcloud.cpp and skeleton_truss.cpp in the directory of /home/william/truss_ws/src/truss/src

In truss_pointcloud.cpp, it seems like:

#define RES 100 //occmap resulotion, number of cell per meter(one demention)

      init_x=0.251859;
      init_y=-0.251859;
      init_z=-0.251859;

In skeleton_truss.cpp, it seems like:

#define RES 100 //occmap resulotion, number of cell per meter(one demention)

float init_x=0.251859,init_y=-0.251859,init_z=-0.251859;    // must be in the truss

Successful demo

      init_x=0.12;
      init_y=0.12;
      init_z=0.526;

Run node truss_pointcloud.

rosrun truss truss_pointcloud

Publish pointcloud

rosrun pcl_ros pcd_to_pointcloud /home/william/Documents/demos/cylinders/preprocessing/data/realSense02.pcd 10 _frame_id:=/map

output:

size 92 80 122
[ INFO] [1555152658.163952435]: n=137,DETECT_time=0,tot=10.46
[ INFO] [1555152658.235489374]: n=350,DETECT_time=1,tot=11.89
[ INFO] [1555152658.365974220]: n=734,DETECT_time=2,tot=15.27
[ INFO] [1555152658.451850002]: n=991,DETECT_time=3,tot=15.29
[ INFO] [1555152658.854427484]: n=2179,DETECT_time=4,tot=12.83
[ INFO] [1555152659.265372521]: n=3354,DETECT_time=5,tot=12.42
[ INFO] [1555152659.665302454]: n=4504,DETECT_time=6,tot=26.82
[ INFO] [1555152660.201265043]: n=6039,DETECT_time=7,tot=10.18
[ INFO] [1555152660.736159441]: collect time=678,rollback time=178
[ INFO] [1555152660.738436210]: Finish

Run node skeleton_truss

rosrun truss skeleton_truss

output:

[ INFO] [1555153870.996387804]: Skeleton start
[ INFO] [1555153911.036309147]: collect time=20
[ INFO] [1555153911.036339322]: Skeleton finish

Run node node_pole

rosrun truss node_pole

Output:

vector[1].x=0.02,y=0.12,z=0.00,size of vector[3]=77
vector[2].x=0.00,y=0.08,z=-0.04,size of vector[4]=26
vector[3].x=-0.02,y=0.00,z=0.00,size of vector[6]=97
vector[4].x=0.00,y=-0.02,z=0.02,size of vector[17]=30
vector[5].x=0.00,y=0.00,z=-0.02,size of vector[19]=46
vector[6].x=-0.04,y=0.02,z=0.00,size of vector[29]=24
vector[7].x=0.04,y=-0.04,z=0.00,size of vector[73]=24
[ INFO] [1555153941.073078888]: Type of vector=7
[ INFO] [1555153941.073190788]: Number of original pole=39
[ INFO] [1555153941.073590573]: Number of true poleos=8
[ INFO] [1555153941.826587964]: Pole 0: radius=0.07;end1.x=0.12,end1.y=-0.29,end1.z=0.64;end2.x=-0.09,end2.y=0.09,end2.z=0.68
[ INFO] [1555153941.826616263]: polenumber=2

Then Fixd Frame select map, see topic /pcl_node in rviz

Question:

Does the position of the point on the pole will be affected whether it is near the background?

Valify it by choosing another point it is not near by the background.

observe the scenario, noise exist near the pole, maybe should tune down the search distance.

modify truss_pointcloud.cpp line 354, init_x, init_y, init_z

save settings of rviz in node_pole.

rviz -d node_pole

In order to run node truss pointCloud once, we will run it at first. Then publish pointcloud only one time.

Update in 20190528

Tune down the values of RANGE and ADDLI when the truss is nearer to the background.

#define DIS 8  //Fisrt filtering
#define RES 100 //occmap resulotion, number of cell per meter(one demention)
// #define RES 500 //occmap resulotion, number of cell per meter(one demention)
#define SDEP 200 //depth of search, size of close list
#define RANGE 5 //range of initial search, rely on the resulotion of pointcloud
#define ADDLI 3 //limit of additional search range, rely on the Climbot max
#define ERROR 2 //allowable error
#define POSTEP 3000 //max step of search.
#define LOCALDIS 0.1 //distance for identify the local optima seperately
#define ALLOWDUP 2 //when fall into local optima, times to duplicate and escape
#define COLLECT 3000 //Sample time
#define DESCRITH 10

The original pointcloud:

Picture after truss_pointcloud

#define RES 100 //occmap resulotion, number of cell per meter(one demention)
#define SDEP 100 //depth of search, size of close list
#define RANGE 6 //range of initial search, rely on the max step length of Climbot
#define ADDLI 4 //limit of additional search range, to increace the search range when needed
#define ERROR 8 //allowable error, cell
#define POSTEP 300 //max step of forward search.
#define OPDIR 100 //max step of leaving search.
#define LOCALDIS 0.1 //distance for identify the local optima seperately
#define ALLOWDUP 2 //when fall into local optima, times to duplicate and escape

#define SAMPLE_TH 10   //vector size threshold 24
#define ANGLE 27       //vector angle range 27
#define POLE_LENGTH 4 //pole length threshold
#define POLE_DIS 0.5  //repeated poles check distance
#define SHOW_POLE 99   //99 means all the true poles, 999 means all the poles
#define OBB_SIZE 0.08  //the size of expand ball from one point
#define PP_SKIP  4     //the allowed breakup of pointcloud along one pole

[ INFO] [1559107770.253840402]: Pole 1: radius=0.08;end1.x=-0.65,end1.y=-0.69,end1.z=-0.31;end2.x=-0.58,end2.y=-0.75,end2.z=0.34
[ INFO] [1559107770.253870503]: Pole 5: radius=0.08;end1.x=0.33,end1.y=0.02,end1.z=0.39;end2.x=0.40,end2.y=-0.09,end2.z=1.02
[ INFO] [1559107770.253906031]: Pole 12: radius=0.08;end1.x=-0.35,end1.y=0.77,end1.z=-0.14;end2.x=-1.03,end2.y=-0.06,end2.z=0.49
[ INFO] [1559107770.253920717]: Pole 15: radius=0.08;end1.x=0.37,end1.y=0.01,end1.z=0.18;end2.x=-0.32,end2.y=0.78,end2.z=0.43
[ INFO] [1559107770.253934686]: Pole 17: radius=0.07;end1.x=0.40,end1.y=-0.13,end1.z=0.98;end2.x=-0.18,end2.y=0.44,end2.z=0.71
[ INFO] [1559107770.253947612]: Pole 18: radius=0.05;end1.x=-0.81,end1.y=0.01,end1.z=0.51;end2.x=-0.69,end2.y=-0.64,end2.z=-0.16
[ INFO] [1559107770.253957812]: Pole 19: radius=0.06;end1.x=0.00,end1.y=-0.32,end1.z=0.36;end2.x=-0.47,end2.y=-0.80,end2.z=0.91
[ INFO] [1559107770.253969030]: Pole 20: radius=0.08;end1.x=-0.35,end1.y=0.77,end1.z=-0.14;end2.x=-1.03,end2.y=-0.06,end2.z=0.49
[ INFO] [1559107770.253980318]: Pole 21: radius=0.08;end1.x=-0.33,end1.y=0.82,end1.z=-0.21;end2.x=-0.26,end2.y=0.66,end2.z=0.35
[ INFO] [1559107770.253996517]: Pole 22: radius=0.07;end1.x=-0.00,end1.y=-0.35,end1.z=0.35;end2.x=-0.47,end2.y=-0.80,end2.z=0.40
[ INFO] [1559107770.254006475]: Pole 23: radius=0.08;end1.x=-0.53,end1.y=-0.87,end1.z=0.87;end2.x=-0.47,end2.y=-0.75,end2.z=0.96
[ INFO] [1559107770.254018620]: polenumber=11

Publish topics

/allpath
/allpath2
/cube
/detectpoints
/envelopeline
/ldetectpoints
/openvelopeline
/output2_pcl
/pcl_node

truss_pointcloud

/allpath
/output2_pcl

skeleton_truss

/allpath2

node_pole

/envelopeline
/openvelopeline
/pcl_node

Updated in 190731

Updated in 190805