| 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 <CTraitsTest.h> |
| 11 |
#include <gtest/gtest.h> |
| 12 |
#include <mrpt/config.h> |
| 13 |
#include <mrpt/config/CConfigFileMemory.h> |
| 14 |
#include <mrpt/containers/yaml.h> |
| 15 |
#include <mrpt/img/TCamera.h> |
| 16 |
|
| 17 |
template class mrpt::CTraitsTest<mrpt::img::TCamera>; |
| 18 |
|
| 19 |
using mrpt::img::TCamera; |
| 20 |
|
| 21 |
static TCamera getSampleCameraParamsBase() |
| 22 |
{ |
| 23 |
TCamera c1; |
| 24 |
c1.ncols = 800; |
| 25 |
c1.nrows = 600; |
| 26 |
c1.fx(350); |
| 27 |
c1.fy(340); |
| 28 |
c1.cx(400); |
| 29 |
c1.cy(300); |
| 30 |
return c1; |
| 31 |
} |
| 32 |
|
| 33 |
static TCamera getSampleCameraParamsPlumbBob() |
| 34 |
{ |
| 35 |
TCamera c1 = getSampleCameraParamsBase(); |
| 36 |
c1.distortion = mrpt::img::DistortionModel::plumb_bob; |
| 37 |
c1.k1(1e-3); |
| 38 |
c1.k2(2e-3); |
| 39 |
c1.k3(3e-3); |
| 40 |
c1.p1(4.5e-3); |
| 41 |
c1.p2(5.6e-9); |
| 42 |
return c1; |
| 43 |
} |
| 44 |
|
| 45 |
static TCamera getSampleCameraParamsFishEye() |
| 46 |
{ |
| 47 |
TCamera c1 = getSampleCameraParamsBase(); |
| 48 |
c1.distortion = mrpt::img::DistortionModel::kannala_brandt; |
| 49 |
c1.k1(1e-3); |
| 50 |
c1.k2(2e-3); |
| 51 |
c1.k3(3e-3); |
| 52 |
c1.k4(-5e-6); |
| 53 |
return c1; |
| 54 |
} |
| 55 |
|
| 56 |
TEST(TCamera, EqualOperator) |
| 57 |
{ |
| 58 |
const auto c1 = getSampleCameraParamsPlumbBob(); |
| 59 |
auto c2 = c1; |
| 60 |
EXPECT_TRUE(c1 == c2); |
| 61 |
|
| 62 |
c2.fx(c1.fx() + 0.1); |
| 63 |
EXPECT_FALSE(c1 == c2); |
| 64 |
} |
| 65 |
|
| 66 |
TEST(TCamera, CopyCtor) |
| 67 |
{ |
| 68 |
const auto c1 = getSampleCameraParamsPlumbBob(); |
| 69 |
const auto c2 = c1; |
| 70 |
EXPECT_EQ(c2, c1); |
| 71 |
|
| 72 |
EXPECT_EQ(c2.ncols, 800U); |
| 73 |
EXPECT_EQ(c2.nrows, 600U); |
| 74 |
EXPECT_EQ(c2.fx(), 350); |
| 75 |
EXPECT_EQ(c2.fy(), 340); |
| 76 |
EXPECT_EQ(c2.cx(), 400); |
| 77 |
EXPECT_EQ(c2.cy(), 300); |
| 78 |
EXPECT_EQ(c2.k1(), 1e-3); |
| 79 |
EXPECT_EQ(c2.k2(), 2e-3); |
| 80 |
EXPECT_EQ(c2.k3(), 3e-3); |
| 81 |
EXPECT_EQ(c2.p1(), 4.5e-3); |
| 82 |
EXPECT_EQ(c2.p2(), 5.6e-9); |
| 83 |
} |
| 84 |
|
| 85 |
TEST(TCamera, ToFromINI_PlumbBob) |
| 86 |
{ |
| 87 |
const auto c1 = getSampleCameraParamsPlumbBob(); |
| 88 |
|
| 89 |
mrpt::config::CConfigFileMemory cfg; |
| 90 |
c1.saveToConfigFile("cam1", cfg); |
| 91 |
|
| 92 |
mrpt::img::TCamera c2; |
| 93 |
c2.loadFromConfigFile("cam1", cfg); |
| 94 |
|
| 95 |
EXPECT_TRUE(c1 == c2) << "c1:\n" << c1.dumpAsText() << "\nc2:\n" << c2.dumpAsText(); |
| 96 |
} |
| 97 |
|
| 98 |
TEST(TCamera, ToFromINI_FishEye) |
| 99 |
{ |
| 100 |
const auto c1 = getSampleCameraParamsFishEye(); |
| 101 |
|
| 102 |
mrpt::config::CConfigFileMemory cfg; |
| 103 |
c1.saveToConfigFile("cam1", cfg); |
| 104 |
|
| 105 |
mrpt::img::TCamera c2; |
| 106 |
c2.loadFromConfigFile("cam1", cfg); |
| 107 |
|
| 108 |
EXPECT_TRUE(c1 == c2) << "c1:\n" << c1.dumpAsText() << "\nc2:\n" << c2.dumpAsText(); |
| 109 |
} |
| 110 |
|
| 111 |
#if MRPT_HAS_FYAML |
| 112 |
TEST(TCamera, FromYAML_PlumbBob) |
| 113 |
{ |
| 114 |
const std::string sampleParams = R"XXX(#yaml camera in OpenCV format |
| 115 |
image_width: 2448 |
| 116 |
image_height: 2050 |
| 117 |
camera_name: prosilica |
| 118 |
camera_matrix: |
| 119 |
rows: 3 |
| 120 |
cols: 3 |
| 121 |
data: [4827.94, 0, 1223.5, 0, 4835.62, 1024.5, 0, 0, 1] |
| 122 |
distortion_model: plumb_bob |
| 123 |
distortion_coefficients: |
| 124 |
rows: 1 |
| 125 |
cols: 5 |
| 126 |
data: [-0.41527, 0.31874, -0.00197, 0.00071, 0] |
| 127 |
)XXX"; |
| 128 |
|
| 129 |
const auto cfg = mrpt::containers::yaml::FromText(sampleParams); |
| 130 |
|
| 131 |
const auto c = TCamera::FromYAML(cfg); |
| 132 |
|
| 133 |
EXPECT_EQ(c.distortion, mrpt::img::DistortionModel::plumb_bob); |
| 134 |
|
| 135 |
EXPECT_EQ(c.ncols, 2448U); |
| 136 |
EXPECT_EQ(c.nrows, 2050U); |
| 137 |
EXPECT_EQ(c.fx(), 4827.94); |
| 138 |
EXPECT_EQ(c.fy(), 4835.62); |
| 139 |
EXPECT_EQ(c.cx(), 1223.5); |
| 140 |
EXPECT_EQ(c.cy(), 1024.5); |
| 141 |
EXPECT_EQ(c.k1(), -0.41527); |
| 142 |
EXPECT_EQ(c.k2(), 0.31874); |
| 143 |
EXPECT_EQ(c.p1(), -0.00197); |
| 144 |
EXPECT_EQ(c.p2(), 0.00071); |
| 145 |
EXPECT_EQ(c.k3(), 0); |
| 146 |
} |
| 147 |
|
| 148 |
TEST(TCamera, FromYAML_FishEye) |
| 149 |
{ |
| 150 |
const std::string sampleParams = R"XXX(#yaml camera in OpenCV format |
| 151 |
image_width: 2448 |
| 152 |
image_height: 2050 |
| 153 |
camera_name: prosilica |
| 154 |
camera_matrix: |
| 155 |
rows: 3 |
| 156 |
cols: 3 |
| 157 |
data: [4827.94, 0, 1223.5, 0, 4835.62, 1024.5, 0, 0, 1] |
| 158 |
distortion_model: kannala_brandt |
| 159 |
distortion_coefficients: |
| 160 |
rows: 1 |
| 161 |
cols: 4 |
| 162 |
data: [-0.41527, 0.31874, -0.00197, 0.00071] |
| 163 |
)XXX"; |
| 164 |
|
| 165 |
const auto cfg = mrpt::containers::yaml::FromText(sampleParams); |
| 166 |
|
| 167 |
const auto c = TCamera::FromYAML(cfg); |
| 168 |
|
| 169 |
EXPECT_EQ(c.distortion, mrpt::img::DistortionModel::kannala_brandt); |
| 170 |
|
| 171 |
EXPECT_EQ(c.ncols, 2448U); |
| 172 |
EXPECT_EQ(c.nrows, 2050U); |
| 173 |
EXPECT_EQ(c.fx(), 4827.94); |
| 174 |
EXPECT_EQ(c.fy(), 4835.62); |
| 175 |
EXPECT_EQ(c.cx(), 1223.5); |
| 176 |
EXPECT_EQ(c.cy(), 1024.5); |
| 177 |
EXPECT_EQ(c.k1(), -0.41527); |
| 178 |
EXPECT_EQ(c.k2(), 0.31874); |
| 179 |
EXPECT_EQ(c.k3(), -0.00197); |
| 180 |
EXPECT_EQ(c.k4(), 0.00071); |
| 181 |
} |
| 182 |
|
| 183 |
TEST(TCamera, ToFromYAML_PlumbBob) |
| 184 |
{ |
| 185 |
const auto c1 = getSampleCameraParamsPlumbBob(); |
| 186 |
std::stringstream ss; |
| 187 |
ss << c1.asYAML(); |
| 188 |
|
| 189 |
const auto c2 = TCamera::FromYAML(mrpt::containers::yaml::FromText(ss.str())); |
| 190 |
|
| 191 |
EXPECT_TRUE(c1 == c2) << "c1:\n" << c1.asYAML() << "\nc2:\n" << c2.asYAML(); |
| 192 |
} |
| 193 |
TEST(TCamera, ToFromYAML_FishEye) |
| 194 |
{ |
| 195 |
const auto c1 = getSampleCameraParamsFishEye(); |
| 196 |
std::stringstream ss; |
| 197 |
ss << c1.asYAML(); |
| 198 |
|
| 199 |
const auto c2 = TCamera::FromYAML(mrpt::containers::yaml::FromText(ss.str())); |
| 200 |
|
| 201 |
EXPECT_TRUE(c1 == c2) << "c1:\n" << c1.asYAML() << "\nc2:\n" << c2.asYAML(); |
| 202 |
} |
| 203 |
#endif |