| 146 | } |
| 147 | |
| 148 | void |
| 149 | pcl::simulation::SimExample::write_depth_image(const float* depth_buffer, |
| 150 | std::string fname) |
| 151 | { |
| 152 | int npixels = rl_->getWidth() * rl_->getHeight(); |
| 153 | auto* depth_img = new std::uint8_t[npixels * 3]; |
| 154 | |
| 155 | float min_depth = depth_buffer[0]; |
| 156 | float max_depth = depth_buffer[0]; |
| 157 | for (int i = 1; i < npixels; i++) { |
| 158 | if (depth_buffer[i] < min_depth) |
| 159 | min_depth = depth_buffer[i]; |
| 160 | if (depth_buffer[i] > max_depth) |
| 161 | max_depth = depth_buffer[i]; |
| 162 | } |
| 163 | |
| 164 | for (int y = 0; y < height_; ++y) { |
| 165 | for (int x = 0; x < width_; ++x) { |
| 166 | int i = y * width_ + x; |
| 167 | int i_in = (height_ - 1 - y) * width_ + x; // flip up down |
| 168 | |
| 169 | float zn = 0.7; |
| 170 | float zf = 20.0; |
| 171 | float d = depth_buffer[i_in]; |
| 172 | float z = -zf * zn / ((zf - zn) * (d - zf / (zf - zn))); |
| 173 | float b = 0.075; |
| 174 | float f = 580.0; |
| 175 | auto kd = static_cast<std::uint16_t>(1090 - b * f / z * 8); |
| 176 | if (kd > 2047) |
| 177 | kd = 2047; |
| 178 | |
| 179 | int pval = t_gamma[kd]; |
| 180 | int lb = pval & 0xff; |
| 181 | switch (pval >> 8) { |
| 182 | case 0: |
| 183 | depth_img[3 * i + 0] = 255; |
| 184 | depth_img[3 * i + 1] = 255 - lb; |
| 185 | depth_img[3 * i + 2] = 255 - lb; |
| 186 | break; |
| 187 | case 1: |
| 188 | depth_img[3 * i + 0] = 255; |
| 189 | depth_img[3 * i + 1] = lb; |
| 190 | depth_img[3 * i + 2] = 0; |
| 191 | break; |
| 192 | case 2: |
| 193 | depth_img[3 * i + 0] = 255 - lb; |
| 194 | depth_img[3 * i + 1] = 255; |
| 195 | depth_img[3 * i + 2] = 0; |
| 196 | break; |
| 197 | case 3: |
| 198 | depth_img[3 * i + 0] = 0; |
| 199 | depth_img[3 * i + 1] = 255; |
| 200 | depth_img[3 * i + 2] = lb; |
| 201 | break; |
| 202 | case 4: |
| 203 | depth_img[3 * i + 0] = 0; |
| 204 | depth_img[3 * i + 1] = 255 - lb; |
| 205 | depth_img[3 * i + 2] = 255; |
no test coverage detected