| 1 |
/* +------------------------------------------------------------------------+ |
| 2 |
| Mobile Robot Programming Toolkit (MRPT) | |
| 3 |
| https://www.mrpt.org/ | |
| 4 |
| | |
| 5 |
| Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | |
| 6 |
| See: https://www.mrpt.org/Authors - All rights reserved. | |
| 7 |
| Released under BSD License. See: https://www.mrpt.org/License | |
| 8 |
+------------------------------------------------------------------------+ */ |
| 9 |
|
| 10 |
#include "img-precomp.h" // Precompiled headers |
| 11 |
// |
| 12 |
#include <mrpt/core/reverse_bytes.h> |
| 13 |
#include <mrpt/core/round.h> |
| 14 |
#include <mrpt/img/CCanvas.h> |
| 15 |
#include <mrpt/img/CImage.h> |
| 16 |
#include <mrpt/io/zip.h> |
| 17 |
#include <mrpt/math/CMatrixFixed.h> |
| 18 |
#include <mrpt/system/os.h> |
| 19 |
#include <mrpt/system/string_utils.h> |
| 20 |
|
| 21 |
#include <Eigen/Dense> |
| 22 |
#include <cstring> // memcpy |
| 23 |
#include <map> |
| 24 |
|
| 25 |
// Include the MRPT bitmap fonts: |
| 26 |
#include "mrpt_font_10x20.h" |
| 27 |
#include "mrpt_font_5x7.h" |
| 28 |
#include "mrpt_font_6x13.h" |
| 29 |
#include "mrpt_font_6x13B.h" |
| 30 |
#include "mrpt_font_6x13O.h" |
| 31 |
#include "mrpt_font_9x15.h" |
| 32 |
#include "mrpt_font_9x15B.h" |
| 33 |
|
| 34 |
// Japanese fonts? |
| 35 |
#if MRPT_HAS_ASIAN_FONTS |
| 36 |
#include "mrpt_font_18x18ja.h" |
| 37 |
#endif |
| 38 |
|
| 39 |
// Each font has a block a data with this header (It's actually zip-compressed |
| 40 |
// since mrpt >0.6.5) |
| 41 |
// const uint32_t mrpt_font_9x15B [] = { |
| 42 |
// 9,15, /* width, height */ |
| 43 |
// 0x0000,0x00FF, /* UNICODE characters range: */ |
| 44 |
|
| 45 |
using namespace mrpt; |
| 46 |
using namespace mrpt::img; |
| 47 |
using namespace std; |
| 48 |
|
| 49 |
struct FontData |
| 50 |
{ |
| 51 |
std::vector<uint8_t> data; |
| 52 |
bool prepared_to_big_endian = false; |
| 53 |
}; |
| 54 |
|
| 55 |
// Each vector is the target place where to uncompress each font. |
| 56 |
map<string, FontData> list_registered_fonts; |
| 57 |
bool list_fonts_init = false; |
| 58 |
|
| 59 |
void init_fonts_list() |
| 60 |
{ |
| 61 |
if (!list_fonts_init) |
| 62 |
{ |
| 63 |
list_registered_fonts.clear(); |
| 64 |
|
| 65 |
// This was used only once |
| 66 |
#if 0 |
| 67 |
#define SAVE_COMPRESSED(ARR) \ |
| 68 |
{ \ |
| 69 |
list_registered_fonts[#ARR].resize(sizeof(mrpt_font_##ARR)); \ |
| 70 |
memcpy(&list_registered_fonts[#ARR][0], mrpt_font_##ARR, sizeof(mrpt_font_##ARR)); \ |
| 71 |
cout << #ARR << " -> " << sizeof(mrpt_font_##ARR) << endl; \ |
| 72 |
CCompressedOutputStream f(string("mrpt_font_") + string(#ARR) + string(".gz")); \ |
| 73 |
f.WriteBuffer(mrpt_font_##ARR, sizeof(mrpt_font_##ARR)); \ |
| 74 |
/*mrpt::compress::zip::compress( list_registered_fonts[#ARR], f ); */ \ |
| 75 |
} |
| 76 |
|
| 77 |
SAVE_COMPRESSED(5x7) |
| 78 |
// SAVE_COMPRESSED(6x13) |
| 79 |
// SAVE_COMPRESSED(6x13B) |
| 80 |
// SAVE_COMPRESSED(6x13O) |
| 81 |
// SAVE_COMPRESSED(9x15) |
| 82 |
// SAVE_COMPRESSED(9x15B) |
| 83 |
// SAVE_COMPRESSED(10x20) |
| 84 |
|
| 85 |
#if MRPT_HAS_ASIAN_FONTS |
| 86 |
// SAVE_COMPRESSED(18x18ja) |
| 87 |
#endif |
| 88 |
|
| 89 |
#endif |
| 90 |
|
| 91 |
#if 1 // Normal operation: Load fonts and uncompress them: |
| 92 |
|
| 93 |
#define LOAD_FONT(FONTNAME) \ |
| 94 |
{ \ |
| 95 |
std::vector<uint8_t> tmpBuf(sizeof(mrpt_font_gz_##FONTNAME)); \ |
| 96 |
memcpy(&tmpBuf[0], mrpt_font_gz_##FONTNAME, sizeof(mrpt_font_gz_##FONTNAME)); \ |
| 97 |
mrpt::io::zip::decompress_gz_data_block(tmpBuf, list_registered_fonts[#FONTNAME].data); \ |
| 98 |
} |
| 99 |
|
| 100 |
LOAD_FONT(5x7) |
| 101 |
LOAD_FONT(6x13) |
| 102 |
LOAD_FONT(6x13B) |
| 103 |
LOAD_FONT(6x13O) |
| 104 |
LOAD_FONT(9x15) |
| 105 |
LOAD_FONT(9x15B) |
| 106 |
LOAD_FONT(10x20) |
| 107 |
#if MRPT_HAS_ASIAN_FONTS |
| 108 |
LOAD_FONT(18x18ja) |
| 109 |
#endif |
| 110 |
|
| 111 |
#endif |
| 112 |
|
| 113 |
list_fonts_init = true; |
| 114 |
} |
| 115 |
} |
| 116 |
|
| 117 |
/*--------------------------------------------------------------- |
| 118 |
line |
| 119 |
---------------------------------------------------------------*/ |
| 120 |
void CCanvas::line( |
| 121 |
int x0, |
| 122 |
int y0, |
| 123 |
int x1, |
| 124 |
int y1, |
| 125 |
const mrpt::img::TColor color, |
| 126 |
[[maybe_unused]] unsigned int width, |
| 127 |
[[maybe_unused]] TPenStyle penStyle) |
| 128 |
{ |
| 129 |
float x, y; |
| 130 |
|
| 131 |
auto Ax = (float)(x1 - x0); |
| 132 |
auto Ay = (float)(y1 - y0); |
| 133 |
|
| 134 |
// In this cases, there is nothing to do! |
| 135 |
if (Ax == 0 && Ay == 0) return; |
| 136 |
if (x0 < 0 && x1 < 0) return; |
| 137 |
if (y0 < 0 && y1 < 0) return; |
| 138 |
if (x0 >= (int)getWidth() && x1 >= (int)getWidth()) return; |
| 139 |
if (y0 >= (int)getHeight() && y1 >= (int)getHeight()) return; |
| 140 |
|
| 141 |
float dist = sqrt(square(Ax) + square(Ay)); |
| 142 |
int i, N = (int)ceil(dist); |
| 143 |
|
| 144 |
// The N steps to perform next: |
| 145 |
Ax /= N; |
| 146 |
Ay /= N; |
| 147 |
x = (float)x0; |
| 148 |
y = (float)y0; |
| 149 |
|
| 150 |
for (i = 0; i < N; i++) |
| 151 |
{ |
| 152 |
x += Ax; |
| 153 |
y += Ay; |
| 154 |
setPixel((int)x, (int)y, color); |
| 155 |
} // end for i |
| 156 |
} |
| 157 |
|
| 158 |
/*--------------------------------------------------------------- |
| 159 |
rectangle |
| 160 |
---------------------------------------------------------------*/ |
| 161 |
void CCanvas::rectangle( |
| 162 |
int x0, int y0, int x1, int y1, const mrpt::img::TColor color, unsigned int width) |
| 163 |
{ |
| 164 |
int w_min = (int)-ceil(((float)width) / 2); |
| 165 |
int w_max = (int)floor(((float)width) / 2); |
| 166 |
// Draw "width" rectangles one into another: |
| 167 |
for (int w = w_min; w <= w_max; w++) |
| 168 |
{ |
| 169 |
line(x0 - w, y0 - w, x1 + w, y0 - w, color); |
| 170 |
line(x1 + w, y0 - w, x1 + w, y1 + w, color); |
| 171 |
line(x1 + w, y1 + w, x0 - w, y1 + w, color); |
| 172 |
line(x0 - w, y1 + w, x0 - w, y0 - w, color); |
| 173 |
} // end for "w" |
| 174 |
} |
| 175 |
|
| 176 |
/*****************************************************AJOGD***************************************************/ |
| 177 |
/*--------------------------------------------------------------- |
| 178 |
triangle |
| 179 |
---------------------------------------------------------------*/ |
| 180 |
void CCanvas::triangle( |
| 181 |
int x0, int y0, int size, const mrpt::img::TColor color, bool inferior, unsigned int width) |
| 182 |
{ |
| 183 |
int ts = round(0.866 * size); |
| 184 |
int tc = round(0.5 * size); |
| 185 |
if (inferior) |
| 186 |
{ |
| 187 |
line(x0, y0 + size, x0 + ts, y0 - tc, color, width); |
| 188 |
line(x0, y0 + size, x0 - ts, y0 - tc, color, width); |
| 189 |
line(x0 + ts, y0 - tc, x0 - ts, y0 - tc, color, width); |
| 190 |
} |
| 191 |
else |
| 192 |
{ |
| 193 |
line(x0, y0 - size, x0 + ts, y0 + tc, color, width); |
| 194 |
line(x0, y0 - size, x0 - ts, y0 + tc, color, width); |
| 195 |
line(x0 + ts, y0 + tc, x0 - ts, y0 + tc, color, width); |
| 196 |
} |
| 197 |
} |
| 198 |
/************************************************************************************************************/ |
| 199 |
|
| 200 |
/*--------------------------------------------------------------- |
| 201 |
filledRectangle |
| 202 |
---------------------------------------------------------------*/ |
| 203 |
void CCanvas::filledRectangle(int x0, int y0, int x1, int y1, const mrpt::img::TColor color) |
| 204 |
{ |
| 205 |
int x_min = max(x0, 0); |
| 206 |
int x_max = min(x1, (int)getWidth() - 1); |
| 207 |
int y_min = max(y0, 0); |
| 208 |
int y_max = min(y1, (int)getHeight() - 1); |
| 209 |
|
| 210 |
for (int y = y_min; y <= y_max; y++) |
| 211 |
for (int x = x_min; x <= x_max; x++) setPixel(x, y, color); |
| 212 |
} |
| 213 |
|
| 214 |
/*--------------------------------------------------------------- |
| 215 |
selectTextFont |
| 216 |
---------------------------------------------------------------*/ |
| 217 |
void CCanvas::selectTextFont(const std::string& fontName) |
| 218 |
{ |
| 219 |
init_fonts_list(); |
| 220 |
|
| 221 |
// Assure list name is in the list: |
| 222 |
auto it = list_registered_fonts.find(fontName); |
| 223 |
if (it == list_registered_fonts.end()) |
| 224 |
{ |
| 225 |
// Error |
| 226 |
cerr << "[CCanvas::selectTextFont] Warning: Unknown font: " << fontName << endl; |
| 227 |
return; |
| 228 |
} |
| 229 |
else |
| 230 |
{ |
| 231 |
FontData& fd = it->second; |
| 232 |
m_selectedFontBitmaps = reinterpret_cast<const uint32_t*>(&fd.data[0]); |
| 233 |
m_selectedFont = fontName; |
| 234 |
|
| 235 |
#if MRPT_IS_BIG_ENDIAN |
| 236 |
// Fix endianness of char tables: |
| 237 |
if (!fd.prepared_to_big_endian) |
| 238 |
{ |
| 239 |
fd.prepared_to_big_endian = true; // Only do once |
| 240 |
uint32_t* ptr = reinterpret_cast<uint32_t*>(&fd.data[0]); |
| 241 |
for (size_t i = 0; i < fd.data.size() / sizeof(uint32_t); i++) |
| 242 |
mrpt::reverseBytesInPlace(ptr[i]); |
| 243 |
} |
| 244 |
#endif |
| 245 |
} |
| 246 |
} |
| 247 |
|
| 248 |
/*--------------------------------------------------------------- |
| 249 |
drawImage |
| 250 |
---------------------------------------------------------------*/ |
| 251 |
void CCanvas::drawImage(int x, int y, const mrpt::img::CImage& img) |
| 252 |
{ |
| 253 |
MRPT_START |
| 254 |
ASSERT_(img.getPixelDepth() == mrpt::img::PixelDepth::D8U); |
| 255 |
|
| 256 |
int img_lx = img.getWidth(); |
| 257 |
int img_ly = img.getHeight(); |
| 258 |
|
| 259 |
if (img.isColor()) |
| 260 |
{ |
| 261 |
for (int xx = 0; xx < img_lx; xx++) |
| 262 |
for (int yy = 0; yy < img_ly; yy++) |
| 263 |
{ |
| 264 |
auto ptr = img(xx, yy); |
| 265 |
const int p = ptr[0] | (ptr[1] << 8) | (ptr[2] << 16); |
| 266 |
setPixel(x + xx, y + yy, p); |
| 267 |
} |
| 268 |
} |
| 269 |
else |
| 270 |
{ |
| 271 |
unsigned char c; |
| 272 |
int col; |
| 273 |
for (int xx = 0; xx < img_lx; xx++) |
| 274 |
for (int yy = 0; yy < img_ly; yy++) |
| 275 |
{ |
| 276 |
c = *((unsigned char*)img(xx, yy)); |
| 277 |
col = c | (c << 8) | (c << 16); |
| 278 |
setPixel(x + xx, y + yy, col); |
| 279 |
} |
| 280 |
} |
| 281 |
|
| 282 |
MRPT_END |
| 283 |
} |
| 284 |
|
| 285 |
/*--------------------------------------------------------------- |
| 286 |
drawImage |
| 287 |
---------------------------------------------------------------*/ |
| 288 |
void CCanvas::drawImage( |
| 289 |
[[maybe_unused]] int x, |
| 290 |
[[maybe_unused]] int y, |
| 291 |
[[maybe_unused]] const mrpt::img::CImage& img, |
| 292 |
[[maybe_unused]] float rotation, |
| 293 |
[[maybe_unused]] float scale) |
| 294 |
{ |
| 295 |
MRPT_START |
| 296 |
|
| 297 |
THROW_EXCEPTION("Not implemented yet!! Try yourself! ;-)"); |
| 298 |
|
| 299 |
MRPT_END |
| 300 |
} |
| 301 |
|
| 302 |
void CCanvas::drawMark( |
| 303 |
int x0, int y0, const mrpt::img::TColor color, char type, int size, unsigned int width) |
| 304 |
{ |
| 305 |
switch (type) |
| 306 |
{ |
| 307 |
case '+': |
| 308 |
line(x0 - size, y0, x0 + size, y0, color, width); |
| 309 |
line(x0, y0 - size, x0, y0 + size, color, width); |
| 310 |
break; |
| 311 |
case 's': |
| 312 |
line(x0 - size, y0 - size, x0 + size, y0 - size, color, width); |
| 313 |
line(x0 + size, y0 - size, x0 + size, y0 + size, color, width); |
| 314 |
line(x0 - size, y0 + size, x0 + size, y0 + size, color, width); |
| 315 |
line(x0 - size, y0 - size, x0 - size, y0 + size, color, width); |
| 316 |
break; |
| 317 |
case 'x': |
| 318 |
line(x0 - size, y0 - size, x0 + size, y0 + size, color, width); |
| 319 |
line(x0 + size, y0 - size, x0 - size, y0 + size, color, width); |
| 320 |
break; |
| 321 |
case ':': |
| 322 |
line(x0 - size, y0, x0 - 2, y0, color, width); |
| 323 |
line(x0 + 2, y0, x0 + size, y0, color, width); |
| 324 |
line(x0, y0 - size, x0, y0 - 2, color, width); |
| 325 |
line(x0, y0 + 2, x0, y0 + size, color, width); |
| 326 |
break; |
| 327 |
default: |
| 328 |
THROW_EXCEPTION("Unexpected 'type' of cross to be drawn"); |
| 329 |
} |
| 330 |
} |
| 331 |
|
| 332 |
/*--------------------------------------------------------------- |
| 333 |
drawCircle |
| 334 |
---------------------------------------------------------------*/ |
| 335 |
void CCanvas::drawCircle( |
| 336 |
int x, int y, int radius, const mrpt::img::TColor& color, unsigned int width) |
| 337 |
{ |
| 338 |
if (radius < 0) radius = -radius; |
| 339 |
|
| 340 |
int nSegments; |
| 341 |
|
| 342 |
if (radius == 0) |
| 343 |
{ |
| 344 |
nSegments = 2; |
| 345 |
} |
| 346 |
else |
| 347 |
{ |
| 348 |
nSegments = int(M_2PI * radius); |
| 349 |
} |
| 350 |
|
| 351 |
int x1 = 0, y1 = 0, x2 = 0, y2 = 0; |
| 352 |
double ang, Aa = M_2PI / (nSegments - 1); |
| 353 |
int i; |
| 354 |
|
| 355 |
for (i = 0, ang = 0; i < nSegments; i++, ang += Aa) |
| 356 |
{ |
| 357 |
x2 = round(x + radius * cos(ang)); |
| 358 |
y2 = round(y + radius * sin(ang)); |
| 359 |
|
| 360 |
if (i > 0) line(x1, y1, x2, y2, color, width); |
| 361 |
|
| 362 |
x1 = x2; |
| 363 |
y1 = y2; |
| 364 |
} // end for points on ellipse |
| 365 |
} |
| 366 |
|
| 367 |
/*--------------------------------------------------------------- |
| 368 |
textOut |
| 369 |
---------------------------------------------------------------*/ |
| 370 |
void CCanvas::textOut(int x0, int y0, const std::string& str, const mrpt::img::TColor color) |
| 371 |
{ |
| 372 |
MRPT_START |
| 373 |
|
| 374 |
if (!m_selectedFontBitmaps) // First call: load fonts |
| 375 |
this->selectTextFont("9x15"); |
| 376 |
|
| 377 |
// Am I an image? |
| 378 |
bool y_axis_reversed = false; |
| 379 |
auto* im_image = dynamic_cast<CImage*>(this); |
| 380 |
if (im_image) y_axis_reversed = !im_image->isOriginTopLeft(); |
| 381 |
|
| 382 |
// Decode UNICODE string: |
| 383 |
std::vector<uint16_t> uniStr; |
| 384 |
mrpt::system::decodeUTF8(str, uniStr); |
| 385 |
|
| 386 |
int px = x0; |
| 387 |
int py = y0; |
| 388 |
|
| 389 |
// Char size: |
| 390 |
int char_w = m_selectedFontBitmaps[0]; |
| 391 |
int char_h = m_selectedFontBitmaps[1]; |
| 392 |
|
| 393 |
for (unsigned short unichar : uniStr) |
| 394 |
{ |
| 395 |
// look for the character in the table: |
| 396 |
const uint32_t* table_ptr = m_selectedFontBitmaps + 2; |
| 397 |
uint32_t charset_ini = table_ptr[0]; |
| 398 |
uint32_t charset_end = table_ptr[1]; |
| 399 |
|
| 400 |
while (charset_end) |
| 401 |
{ |
| 402 |
// Is in this range? |
| 403 |
if (unichar <= charset_end && unichar >= charset_ini) |
| 404 |
{ |
| 405 |
// Draw this character: |
| 406 |
int pyy = y_axis_reversed ? (py + char_h - 1) : py; |
| 407 |
|
| 408 |
const uint32_t* char_bitmap = table_ptr + 2 + char_h * (unichar - charset_ini); |
| 409 |
|
| 410 |
for (int y = 0; y < char_h; y++, pyy += y_axis_reversed ? -1 : 1) |
| 411 |
{ |
| 412 |
// Use memcpy() here since directly dereferencing is an |
| 413 |
// invalid operation in architectures (S390X) where |
| 414 |
// unaligned accesses are forbiden: |
| 415 |
uint32_t row; |
| 416 |
memcpy(&row, char_bitmap, sizeof(row)); |
| 417 |
char_bitmap++; |
| 418 |
for (int x = 0, pxx = px; x < char_w; x++, pxx++) |
| 419 |
if (!!(row & (1 << x))) setPixel(pxx, pyy, color); |
| 420 |
} |
| 421 |
|
| 422 |
// Advance the raster cursor: |
| 423 |
px += char_w; |
| 424 |
|
| 425 |
// Next char! |
| 426 |
break; |
| 427 |
} |
| 428 |
else |
| 429 |
{ |
| 430 |
// No: Move to the next block and keep searching: |
| 431 |
uint32_t n_chars = charset_end - charset_ini + 1; |
| 432 |
table_ptr += 2 /* Header */ + n_chars * char_h; |
| 433 |
|
| 434 |
// get new block header: |
| 435 |
charset_ini = table_ptr[0]; |
| 436 |
charset_end = table_ptr[1]; |
| 437 |
} |
| 438 |
} |
| 439 |
// Char not in the font! |
| 440 |
} |
| 441 |
|
| 442 |
MRPT_END |
| 443 |
} |
| 444 |
|
| 445 |
void CCanvas::ellipseGaussian( |
| 446 |
const mrpt::math::CMatrixFixed<double, 2, 2>& cov2D, |
| 447 |
const double mean_x, |
| 448 |
const double mean_y, |
| 449 |
double confIntervalStds, |
| 450 |
const mrpt::img::TColor& color, |
| 451 |
unsigned int width, |
| 452 |
int nEllipsePoints) |
| 453 |
{ |
| 454 |
MRPT_START |
| 455 |
int x1 = 0, y1 = 0, x2 = 0, y2 = 0; |
| 456 |
double ang; |
| 457 |
mrpt::math::CMatrixFixed<double, 2, 2> eigVec, eigVals; |
| 458 |
std::vector<double> eVals; |
| 459 |
int i; |
| 460 |
|
| 461 |
// Compute the eigen-vectors & values: |
| 462 |
cov2D.eig(eigVec, eVals); |
| 463 |
eigVals.setDiagonal(eVals); |
| 464 |
|
| 465 |
eigVals.asEigen() = eigVals.array().sqrt().matrix(); |
| 466 |
|
| 467 |
mrpt::math::CMatrixFixed<double, 2, 2> M; |
| 468 |
M.asEigen() = eigVals.asEigen() * eigVec.transpose(); |
| 469 |
|
| 470 |
// Compute the points of the 2D ellipse: |
| 471 |
for (i = 0, ang = 0; i < nEllipsePoints; i++, ang += (M_2PI / (nEllipsePoints - 1))) |
| 472 |
{ |
| 473 |
double ccos = cos(ang); |
| 474 |
double ssin = sin(ang); |
| 475 |
|
| 476 |
x2 = round(mean_x + confIntervalStds * (ccos * M(0, 0) + ssin * M(1, 0))); |
| 477 |
y2 = round(mean_y + confIntervalStds * (ccos * M(0, 1) + ssin * M(1, 1))); |
| 478 |
|
| 479 |
if (i > 0) line(x1, y1, x2, y2, color, width); |
| 480 |
|
| 481 |
x1 = x2; |
| 482 |
y1 = y2; |
| 483 |
} // end for points on ellipse |
| 484 |
|
| 485 |
MRPT_END_WITH_CLEAN_UP(std::cout << "Covariance matrix leading to error is:" << std::endl |
| 486 |
<< cov2D << std::endl;); |
| 487 |
} |