Skip to content

Commit 904e112

Browse files
gerni17gerni17
committed
fixed polygon checker, added comments to all the functions that are being tested and added tests
1 parent 059ff96 commit 904e112

File tree

9 files changed

+328
-311
lines changed

9 files changed

+328
-311
lines changed

‎elevation_mapping_cupy/config/sensor_parameter.yaml‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@ subscribers:
44
# fusion: ['average','average']
55
# topic_name: '/elevation_mapping/pointcloud_semantic'
66
front_cam:
7-
channels: ['rgb','feat_0','feat_1']
8-
fusion: ['color','average','average']
7+
channels: ['rgb','feat_0','feat_1','c_prob_0']
8+
fusion: ['color','average','average','class_average']
99
topic_name: '/elvation_mapping/pointcloud_semantic'
1010

‎elevation_mapping_cupy/script/elevation_mapping_cupy/custom_kernels.py‎

Lines changed: 78 additions & 115 deletions
Original file line numberDiff line numberDiff line change
@@ -655,16 +655,16 @@ def polygon_mask_kernel(width, height, resolution):
655655
)
656656
return polygon_mask_kernel
657657

658+
##################### additional kernels #################################
658659

659660
def sum_kernel(
660661
resolution,
661662
width,
662663
height,
663-
664664
):
665665
# input the list of layers, amount of channels can slo be input through kernel
666666
sum_kernel = cp.ElementwiseKernel(
667-
in_params="raw U p, raw U center_x, raw U center_y, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels",
667+
in_params="raw U p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels",
668668
out_params="raw U map, raw T newmap",
669669
preamble=string.Template(
670670
"""
@@ -679,24 +679,16 @@ def sum_kernel(
679679
U idx = p[i * pcl_channels[0]];
680680
U valid = p[i * pcl_channels[0] + 1];
681681
U inside = p[i * pcl_channels[0] + 2];
682-
//U x = transform_p(rx, ry, rz, R[0], R[1], R[2], t[0]);
683-
//U y = transform_p(rx, ry, rz, R[3], R[4], R[5], t[1]);
684-
//U z = transform_p(rx, ry, rz, R[6], R[7], R[8], t[2]);
685-
// U v = z_noise(rz);
686-
//if (valid) {
687-
// int idx = get_idx(x, y, center_x[0], center_y[0]);
688-
U map_h = map[get_map_idx(idx, 0)];
682+
if (valid) {
689683
if (inside) {
690684
for ( int it=0;it<pcl_channels[1];it++){
691685
T feat = p[i * pcl_channels[0] + pcl_chan[it]];
692686
atomicAdd(&newmap[get_map_idx(idx, map_lay[it])], feat);
693687
}
694688
}
695-
//}
689+
}
696690
"""
697691
).substitute(
698-
# pcl_channels=pcl_channels,
699-
# amount_features=amount_features,
700692
),
701693
name="sum_kernel",
702694
)
@@ -706,16 +698,9 @@ def sum_kernel(
706698
def average_kernel(
707699
width,
708700
height,
709-
max_variance,
710-
initial_variance,
711-
pcl_channels,
712-
amount_features,
713-
amount_class_prob,
714-
sem_class,
715-
rgb,
716701
):
717702
average_kernel = cp.ElementwiseKernel(
718-
in_params="raw U newmap",
703+
in_params="raw V newmap, raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap",
719704
out_params="raw U map",
720705
preamble=string.Template(
721706
"""
@@ -727,105 +712,85 @@ def average_kernel(
727712
).substitute(width=width, height=height),
728713
operation=string.Template(
729714
"""
730-
U h = map[get_map_idx(i, 0)];
731-
U v = map[get_map_idx(i, 1)];
732-
U valid = map[get_map_idx(i, 2)];
733-
U new_h = newmap[get_map_idx(i, 0)];
734-
U new_v = newmap[get_map_idx(i, 1)];
735-
U new_cnt = newmap[get_map_idx(i, 2)];
736-
if (new_cnt > 0) {
737-
//U sem_cnt = newmap[get_map_idx(i, 7)];
738-
//for ( int it=0;it<${amount_features};it++){
739-
// U orig_f=map[get_map_idx(i, 7+${rgb}+it)];
740-
// U new_f=newmap[get_map_idx(i, 7+${rgb}+it)];
741-
// if (orig_f==0){
742-
// map[get_map_idx(i, 7+${rgb}+it)]=new_f/sem_cnt;
743-
// }
744-
// else{
745-
// map[get_map_idx(i, 7+${rgb}+it)]=orig_f*0.5+new_f/sem_cnt*0.5;
746-
// }
747-
//}
748-
// int stored_it = 0;
749-
// U highest_probability = 0;
750-
// for ( int it=0;it<${amount_class_prob};it++){
751-
// U orig_p=map[get_map_idx(i, 7+${rgb}+${amount_features}+it)];
752-
// U new_p=newmap[get_map_idx(i, 7+${rgb}+${amount_features}+it)];
753-
// U probability =0;
754-
// if (orig_p==0){
755-
// probability = new_p/sem_cnt;
756-
// map[get_map_idx(i, 7+${rgb}+${amount_features}+it)]=probability;
757-
// }
758-
// else{
759-
// probability = orig_p*0.5+new_p/sem_cnt*0.5;
760-
// map[get_map_idx(i, 7+${rgb}+${amount_features}+it)]=probability;
761-
// }
762-
// if(probability>= highest_probability){
763-
// stored_it = it;
764-
// highest_probability = probability;
765-
// }
766-
// }
767-
// if (${sem_class}){
768-
// // check max classs prob
769-
// map[get_map_idx(i, 7+${rgb}+${amount_features}+${amount_class_prob})] = stored_it;
770-
// }
715+
U cnt = new_elmap[get_map_idx(i, 2)];
716+
if (cnt>0){
717+
for ( int it=0;it<pcl_channels[1];it++){
718+
U feat = newmap[get_map_idx(i, it*3)]/(1*cnt);
719+
map[get_map_idx(i, map_lay[it])] = feat;
720+
}
771721
}
772-
773722
"""
774723
).substitute(
775-
max_variance=max_variance,
776-
initial_variance=initial_variance,
777-
pcl_channels=pcl_channels,
778-
amount_features=amount_features,
779-
amount_class_prob=amount_class_prob,
780-
sem_class=int(sem_class),
781-
rgb=int(rgb),
782724
),
783725
name="average_map_kernel",
784726
)
785727
return average_kernel
786728

729+
def class_average_kernel(
730+
width,
731+
height,
732+
):
733+
class_average_kernel = cp.ElementwiseKernel(
734+
in_params="raw V newmap, raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap",
735+
out_params="raw U map",
736+
preamble=string.Template(
737+
"""
738+
__device__ int get_map_idx(int idx, int layer_n) {
739+
const int layer = ${width} * ${height};
740+
return layer * layer_n + idx;
741+
}
742+
"""
743+
).substitute(width=width, height=height),
744+
operation=string.Template(
745+
"""
746+
U cnt = new_elmap[get_map_idx(i, 2)];
747+
if (cnt>0){
748+
for ( int it=0;it<pcl_channels[1];it++){
749+
U prev_val = map[get_map_idx(i, map_lay[it])];
750+
if (prev_val==0){
751+
U val = newmap[get_map_idx(i, it*3)]/(1*cnt);
752+
map[get_map_idx(i, map_lay[it])] = val;
753+
}
754+
else{
755+
U val = prev_val /2 + newmap[get_map_idx(i, it*3)]/(2*cnt);
756+
map[get_map_idx(i, map_lay[it])] = val;
757+
}
758+
}
759+
}
760+
"""
761+
).substitute(
762+
),
763+
name="class_average_kernel",
764+
)
765+
return class_average_kernel
787766

788767
def add_color_kernel(
789768
width,
790769
height,
791770
):
792771
add_color_kernel = cp.ElementwiseKernel(
793-
in_params="raw T p, raw U center_x, raw U center_y, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels",
794-
out_params="raw U map, raw V color_map",
772+
in_params="raw T p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels",
773+
out_params="raw V color_map",
795774
preamble=string.Template(
796775
"""
797776
__device__ int get_map_idx(int idx, int layer_n) {
798777
const int layer = ${width} * ${height};
799778
return layer * layer_n + idx;
800779
}
801-
__device__ unsigned int get_color(unsigned int color){
802-
unsigned int red = 0xFF0000;
803-
unsigned int green = 0xFF00;
804-
unsigned int blue = 0xFF;
805-
unsigned int reds = (color & red) >> 16;
806-
unsigned int greens = (color & green) >> 8;
807-
unsigned int blues = ( color & blue);
808-
unsigned int rgbValue = (reds<<16 ) + (greens << 8) + blues;
809-
// TODO: at the moment it seems to be working, but if we want
810-
//to average over all points we need to use the color layer
811-
//color_out[get_map_idx(idx, 1)] = color;
812-
//[get_map_idx(idx, 2)] = color;
813-
return rgbValue;
814-
}
815780
__device__ unsigned int get_r(unsigned int color){
816-
unsigned int red = 0xFF0000;
817-
unsigned int reds = (color & red) >> 16;
818-
return reds;
781+
unsigned int red = 0xFF0000;
782+
unsigned int reds = (color & red) >> 16;
783+
return reds;
819784
}
820785
__device__ unsigned int get_g(unsigned int color){
821-
unsigned int green = 0xFF00;
822-
unsigned int greens = (color & green) >> 8;
823-
return greens;
786+
unsigned int green = 0xFF00;
787+
unsigned int greens = (color & green) >> 8;
788+
return greens;
824789
}
825790
__device__ unsigned int get_b(unsigned int color){
826-
unsigned int blue = 0xFF;
827-
unsigned int blues = ( color & blue);
828-
return blues;
791+
unsigned int blue = 0xFF;
792+
unsigned int blues = ( color & blue);
793+
return blues;
829794
}
830795
"""
831796
).substitute(width=width, height=height),
@@ -834,17 +799,13 @@ def add_color_kernel(
834799
U idx = p[i * pcl_channels[0]];
835800
U valid = p[i * pcl_channels[0] + 1];
836801
U inside = p[i * pcl_channels[0] + 2];
837-
if (true){
802+
if (valid && inside){
838803
for ( int it=0;it<pcl_channels[1];it++){
839-
unsigned int color = __float_as_uint(p[i * pcl_channels[0] + pcl_chan[it]]);
840-
// unsigned int rgbValue = get_color(color);
841-
842-
if (! false){
843-
map[get_map_idx(idx, map_lay[it])] = color;
844-
atomicAdd(&color_map[get_map_idx(idx, it*3)], get_r(color));
845-
atomicAdd(&color_map[get_map_idx(idx, it*3+1)], get_g(color));
846-
atomicAdd(&color_map[get_map_idx(idx, it*3 + 2)], get_b(color));
847-
atomicAdd(&color_map[get_map_idx(idx, pcl_channels[1]*3)], 1);}
804+
unsigned int color = __float_as_uint(p[i * pcl_channels[0] + pcl_chan[it]]);
805+
atomicAdd(&color_map[get_map_idx(idx, it*3)], get_r(color));
806+
atomicAdd(&color_map[get_map_idx(idx, it*3+1)], get_g(color));
807+
atomicAdd(&color_map[get_map_idx(idx, it*3 + 2)], get_b(color));
808+
atomicAdd(&color_map[get_map_idx(idx, pcl_channels[1]*3)], 1);
848809
}
849810
}
850811
"""
@@ -869,19 +830,19 @@ def color_average_kernel(
869830
return layer * layer_n + idx;
870831
}
871832
__device__ unsigned int get_r(unsigned int color){
872-
unsigned int red = 0xFF0000;
873-
unsigned int reds = (color & red) >> 16;
874-
return reds;
833+
unsigned int red = 0xFF0000;
834+
unsigned int reds = (color & red) >> 16;
835+
return reds;
875836
}
876837
__device__ unsigned int get_g(unsigned int color){
877-
unsigned int green = 0xFF00;
878-
unsigned int greens = (color & green) >> 8;
879-
return greens;
838+
unsigned int green = 0xFF00;
839+
unsigned int greens = (color & green) >> 8;
840+
return greens;
880841
}
881842
__device__ unsigned int get_b(unsigned int color){
882-
unsigned int blue = 0xFF;
883-
unsigned int blues = ( color & blue);
884-
return blues;
843+
unsigned int blue = 0xFF;
844+
unsigned int blues = (color & blue);
845+
return blues;
885846
}
886847
"""
887848
).substitute(width=width, height=height),
@@ -902,8 +863,8 @@ def color_average_kernel(
902863
// unsigned int g = prev_g/2 + color_map[get_map_idx(i, it*3+1)]/(2*cnt);
903864
// unsigned int b = prev_b/2 + color_map[get_map_idx(i, it*3+2)]/(2*cnt);
904865
//}
905-
//unsigned int rgb = (r<<16) + (g << 8) + b;
906-
// map[get_map_idx(i, map_lay[it])] = __uint_as_float(rgb);
866+
unsigned int rgb = (r<<16) + (g << 8) + b;
867+
map[get_map_idx(i, map_lay[it])] = rgb;
907868
}
908869
}
909870
"""
@@ -913,6 +874,8 @@ def color_average_kernel(
913874
)
914875
return color_average_kernel
915876

877+
# TODO: in future add more kernels e.g. bayesian kernel
878+
916879
if __name__ == "__main__":
917880
for i in range(10):
918881
import random

0 commit comments

Comments
 (0)