Semantic Color Randomly Changes

[Rikkert-Lensen]

The problem:

Voxels randomly change colors as seen on the images below

Expected output:

A noiseless map where Voxels do not randomly change colors

What I found:

I went over e few steps to trace back the origin of the problem: 1. In semantic_sensor.py add to: lines 93-101:

def generate_cloud_semantic(self, bgr_img, semantic_color, depth_img, stamp):
        self.generate_cloud_data_common(bgr_img, depth_img)
        # Transform semantic color
        self.semantic_color_vect[:,0:1] = semantic_color[:, :, 0].reshape(-1, 1)
        self.semantic_color_vect[:,1:2] = semantic_color[:, :, 1].reshape(-1, 1)
        self.semantic_color_vect[:,2:3] = semantic_color[:, :, 2].reshape(-1, 1)

        # loop over all labels to remap the color
        for key, value in self.label_colors.items():
            label = int(key.split("_")[1])
            self.semantic_color_vect[:,0:1][self.semantic_color_vect[:, 0:1] == label] = value[0]
            self.semantic_color_vect[:,1:2][self.semantic_color_vect[:, 1:2] == label] = value[1]
            self.semantic_color_vect[:,2:3][self.semantic_color_vect[:, 2:3] == label] = value[2]

        # Concatenate data
        self.ros_data[:, 5:6] = self.semantic_color_vect.view('<f4')
        return self.make_ros_cloud(stamp)

Where self.label_colors is defined as:

self.label_colors = 
{
 'color_0': [0, 0, 0],
 'color_1': [0, 0, 255],
 'color_2': [0, 255, 0],
 'color_3': [255, 0, 0],
 'color_4': [255, 255, 0],
 'color_5': [255, 0, 255], 
 'color_6': [0, 255, 255],
 'color_7': [0, 127, 255],
 'color_8': [0, 0, 125],
 'color_9': [0, 127, 0], 
 'color_10': [127, 0, 0], 
 'color_11': [127, 0, 127], 
 'color_12': [0, 127, 127], 
 'color_13': [127, 127, 0],
 'color_14': [127, 127, 127], 
 'color_15': [127, 255, 0], 
 'color_16': [255, 127, 0]
}

Thus forcing the self.semantic_colors_vect to contain only defined colors. (This is verified by looping over it and checking if the vector still contains any unknown colors) After running the code again the problem still not solved.

Therefore it can be concluded that the problem is caused somewhere after lines 93-101!

2. Investigating octomap_generator.cpp In line 64 the it->semantic_color memory gets copied into the rgb variable.

Since we know from the first step that the color values were indeed correctly set in the self.semantic_color_vect we expect the rgb variable to be consistent with the self.semantic_color_vect. Thus after lines 65-67 I added:

// These are all the allowed colors 
// [0, 0, 0] in hex is 0x000000            
// [0, 0, 255] in hex is 0x0000ff
// [0, 255, 0] in hex is 0x00ff00
// [255, 0, 0] in hex is 0xff0000
// [255, 255, 0] in hex is 0xffff00
// [255, 0, 255] in hex is 0xff00ff
// [0, 255, 255] in hex is 0x00ffff
// [0, 127, 255] in hex is 0x007fff
// [0, 0, 127] in hex is 0x00007f
// [0, 127, 0] in hex is 0x007f00
// [127, 0, 0] in hex is 0x7f0000 
// [127, 0, 127] in hex is 0x7f007f
// [0, 127, 127] in hex is 0x007f7f
// [127, 127, 0] in hex is 0x7f7f00
// [127, 127, 127] in hex is 0x7f7f7f
// [127, 255, 0] in hex is 0x7fff00
// [255, 127, 0] in hex is 0xff7f00
// Check if the rgb is one of the allowed colors and print out the color if it is not
if(rgb != 0x000000 && rgb != 0x0000ff && rgb != 0x00ff00 && rgb != 0xff0000 && rgb != 0xffff00 && rgb != 0xff00ff && rgb != 0x00ffff && rgb != 0x007fff && rgb != 0x00007f && rgb != 0x007f00 && rgb != 0x7f0000 && rgb != 0x7f007f && rgb != 0x007f7f && rgb != 0x7f7f00 && rgb != 0x7f7f7f && rgb != 0x7fff00 && rgb != 0xff7f00)
{
    std::cout << "this color is not allowed: " << std::hex << rgb << std::endl;
}

After running I do receive some "this color is not allowed:" messages in the terminal

Thus it can be concluded that the value of it->semantic_color is already incorrect!

3. Going back to semantic_sensor.py: The only code that could be responsible for the issue can be found in:

• lines 36-55 (Maybe the fields and their offsets are not correctly defined and sometimes overflow?)
• Line 100 (Using the .view('<f4') changes the array to be represented as a float32. Maybe during this conversion something goes wrong?)
• One more possible line that could cause the problem is line 89 when converting tobytes?

Conclusion

I am not sure which one of the two is the actual cause of the problem and I am not sure how to fix it.

[Rikkert-Lensen]

Changed function to:

    def generate_cloud_semantic(self, bgr_img, semantic_color, depth_img, stamp):
        self.generate_cloud_data_common(bgr_img, depth_img)
        # Transform semantic color
        self.semantic_color_vect[:,
                                 0:1] = semantic_color[:, :, 0].reshape(-1, 1)
        self.semantic_color_vect[:,
                                 1:2] = semantic_color[:, :, 1].reshape(-1, 1)
        self.semantic_color_vect[:,
                                 2:3] = semantic_color[:, :, 2].reshape(-1, 1)

        # loop over all labels to remap the color
        for key, value in self.label_colors.items():
            label = int(key.split("_")[1])
            self.semantic_color_vect[:,0:1][self.semantic_color_vect[:, 0:1] == label] = value[0]
            self.semantic_color_vect[:,1:2][self.semantic_color_vect[:, 1:2] == label] = value[1]
            self.semantic_color_vect[:,2:3][self.semantic_color_vect[:, 2:3] == label] = value[2]

        with open("semantic_color.txt", "w") as f:    
            total_number_of_points = self.semantic_color_vect.shape[0]
            for i, color in enumerate(self.semantic_color_vect):
                f.write(f"{i}/{total_number_of_points} {color} {color.view(np.float32).tobytes()} {color.view(np.float32).tobytes().hex()}\n")

        print("saved semantic_color.txt")
        # exit the program here for debugging
        exit()

Yields output: semantic_color.txt