@@ -143,8 +143,8 @@ def add_points_kernel(
143143):
144144
145145 add_points_kernel = cp .ElementwiseKernel (
146- in_params = "raw U p, raw U center_x, raw U center_y, raw U R, raw U t, raw U norm_map" ,
147- out_params = "raw U map, raw T newmap" ,
146+ in_params = "raw U center_x, raw U center_y, raw U R, raw U t, raw U norm_map" ,
147+ out_params = "raw U p, raw U map, raw T newmap" ,
148148 preamble = map_utils (
149149 resolution ,
150150 width ,
@@ -165,8 +165,8 @@ def add_points_kernel(
165165 U y = transform_p(rx, ry, rz, R[3], R[4], R[5], t[1]);
166166 U z = transform_p(rx, ry, rz, R[6], R[7], R[8], t[2]);
167167 U v = z_noise(rz);
168+ int idx = get_idx(x, y, center_x[0], center_y[0]);
168169 if (is_valid(x, y, z, t[0], t[1], t[2])) {
169- int idx = get_idx(x, y, center_x[0], center_y[0]);
170170 if (is_inside(idx)) {
171171 U map_h = map[get_map_idx(idx, 0)];
172172 U map_v = map[get_map_idx(idx, 1)];
@@ -258,6 +258,9 @@ def add_points_kernel(
258258 }
259259 }
260260 }
261+ p[i * 3]= idx;
262+ p[i * 3 + 1] = is_valid(x, y, z, t[0], t[1], t[2]);
263+ p[i * 3 + 2] = is_inside(idx);
261264 """
262265 ).substitute (
263266 mahalanobis_thresh = mahalanobis_thresh ,
@@ -653,6 +656,263 @@ def polygon_mask_kernel(width, height, resolution):
653656 return polygon_mask_kernel
654657
655658
659+ def sum_kernel (
660+ resolution ,
661+ width ,
662+ height ,
663+
664+ ):
665+ # input the list of layers, amount of channels can slo be input through kernel
666+ 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" ,
668+ out_params = "raw U map, raw T newmap" ,
669+ preamble = string .Template (
670+ """
671+ __device__ int get_map_idx(int idx, int layer_n) {
672+ const int layer = ${width} * ${height};
673+ return layer * layer_n + idx;
674+ }
675+ """
676+ ).substitute (resolution = resolution ,width = width , height = height ),
677+ operation = string .Template (
678+ """
679+ U idx = p[i * pcl_channels[0]];
680+ U valid = p[i * pcl_channels[0] + 1];
681+ 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)];
689+ if (inside) {
690+ for ( int it=0;it<pcl_channels[1];it++){
691+ T feat = p[i * pcl_channels[0] + pcl_chan[it]];
692+ atomicAdd(&newmap[get_map_idx(idx, map_lay[it])], feat);
693+ }
694+ }
695+ //}
696+ """
697+ ).substitute (
698+ # pcl_channels=pcl_channels,
699+ # amount_features=amount_features,
700+ ),
701+ name = "sum_kernel" ,
702+ )
703+ return sum_kernel
704+
705+
706+ def average_kernel (
707+ width ,
708+ height ,
709+ max_variance ,
710+ initial_variance ,
711+ pcl_channels ,
712+ amount_features ,
713+ amount_class_prob ,
714+ sem_class ,
715+ rgb ,
716+ ):
717+ average_kernel = cp .ElementwiseKernel (
718+ in_params = "raw U newmap" ,
719+ out_params = "raw U map" ,
720+ preamble = string .Template (
721+ """
722+ __device__ int get_map_idx(int idx, int layer_n) {
723+ const int layer = ${width} * ${height};
724+ return layer * layer_n + idx;
725+ }
726+ """
727+ ).substitute (width = width , height = height ),
728+ operation = string .Template (
729+ """
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+ // }
771+ }
772+
773+ """
774+ ).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 ),
782+ ),
783+ name = "average_map_kernel" ,
784+ )
785+ return average_kernel
786+
787+
788+ def add_color_kernel (
789+ width ,
790+ height ,
791+ ):
792+ 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" ,
795+ preamble = string .Template (
796+ """
797+ __device__ int get_map_idx(int idx, int layer_n) {
798+ const int layer = ${width} * ${height};
799+ return layer * layer_n + idx;
800+ }
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+ }
815+ __device__ unsigned int get_r(unsigned int color){
816+ unsigned int red = 0xFF0000;
817+ unsigned int reds = (color & red) >> 16;
818+ return reds;
819+ }
820+ __device__ unsigned int get_g(unsigned int color){
821+ unsigned int green = 0xFF00;
822+ unsigned int greens = (color & green) >> 8;
823+ return greens;
824+ }
825+ __device__ unsigned int get_b(unsigned int color){
826+ unsigned int blue = 0xFF;
827+ unsigned int blues = ( color & blue);
828+ return blues;
829+ }
830+ """
831+ ).substitute (width = width , height = height ),
832+ operation = string .Template (
833+ """
834+ U idx = p[i * pcl_channels[0]];
835+ U valid = p[i * pcl_channels[0] + 1];
836+ U inside = p[i * pcl_channels[0] + 2];
837+ if (true){
838+ 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);}
848+ }
849+ }
850+ """
851+ ).substitute (
852+ width = width
853+ ),
854+ name = "add_color_kernel" ,
855+ )
856+ return add_color_kernel
857+
858+ def color_average_kernel (
859+ width ,
860+ height ,
861+ ):
862+ color_average_kernel = cp .ElementwiseKernel (
863+ in_params = "raw V color_map, raw W pcl_chan, raw W map_lay, raw W pcl_channels" ,
864+ out_params = "raw U map" ,
865+ preamble = string .Template (
866+ """
867+ __device__ int get_map_idx(int idx, int layer_n) {
868+ const int layer = ${width} * ${height};
869+ return layer * layer_n + idx;
870+ }
871+ __device__ unsigned int get_r(unsigned int color){
872+ unsigned int red = 0xFF0000;
873+ unsigned int reds = (color & red) >> 16;
874+ return reds;
875+ }
876+ __device__ unsigned int get_g(unsigned int color){
877+ unsigned int green = 0xFF00;
878+ unsigned int greens = (color & green) >> 8;
879+ return greens;
880+ }
881+ __device__ unsigned int get_b(unsigned int color){
882+ unsigned int blue = 0xFF;
883+ unsigned int blues = ( color & blue);
884+ return blues;
885+ }
886+ """
887+ ).substitute (width = width , height = height ),
888+ operation = string .Template (
889+ """
890+ unsigned int cnt = color_map[get_map_idx(i, pcl_channels[1]*3)];
891+ if (cnt>0){
892+ for ( int it=0;it<pcl_channels[1];it++){
893+ // U prev_color = map[get_map_idx(i, map_lay[it])];
894+ unsigned int r = color_map[get_map_idx(i, it*3)]/(1*cnt);
895+ unsigned int g = color_map[get_map_idx(i, it*3+1)]/(1*cnt);
896+ unsigned int b = color_map[get_map_idx(i, it*3+2)]/(1*cnt);
897+ //if (prev_color>=0){
898+ // unsigned int prev_r = get_r(prev_color);
899+ // unsigned int prev_g = get_g(prev_color);
900+ // unsigned int prev_b = get_b(prev_color);
901+ // unsigned int r = prev_r/2 + color_map[get_map_idx(i, it*3)]/(2*cnt);
902+ // unsigned int g = prev_g/2 + color_map[get_map_idx(i, it*3+1)]/(2*cnt);
903+ // unsigned int b = prev_b/2 + color_map[get_map_idx(i, it*3+2)]/(2*cnt);
904+ //}
905+ //unsigned int rgb = (r<<16) + (g << 8) + b;
906+ // map[get_map_idx(i, map_lay[it])] = __uint_as_float(rgb);
907+ }
908+ }
909+ """
910+ ).substitute (
911+ ),
912+ name = "color_average_kernel" ,
913+ )
914+ return color_average_kernel
915+
656916if __name__ == "__main__" :
657917 for i in range (10 ):
658918 import random
0 commit comments