41 #include <visp3/core/vpConfig.h>
43 #ifdef VISP_HAVE_CATCH2
45 #include <visp3/core/vpHomogeneousMatrix.h>
46 #include <visp3/core/vpMath.h>
48 #define CATCH_CONFIG_RUNNER
55 #include <visp3/core/vpIoTools.h>
58 TEST_CASE(
"Lon-Lat generator",
"[math_lonlat]")
60 const int lonStart = 0, lonEnd = 360, nlon = 20;
61 const int latStart = 0, latEnd = 90, nLat = 10;
64 const double radius = 5;
66 std::vector<std::pair<double, double> > lonlatVec;
67 lonlatVec.reserve(longitudes.size() * latitudes.size());
68 for (
auto lon : longitudes) {
69 for (
auto lat : latitudes) {
70 lonlatVec.emplace_back(lon, lat);
76 std::vector<vpHomogeneousMatrix> ecef_M_ned_vec =
78 for (
const auto &ecef_M_ned : ecef_M_ned_vec) {
80 std::cout <<
"Lon-Lat ecef_M_ned:\n" << ecef_M_ned << std::endl;
82 CHECK(ecef_M_ned.isValid());
83 CHECK(ecef_M_ned.getRotationMatrix().isARotationMatrix());
84 CHECK(
vpMath::equal(ecef_M_ned.getTranslationVector().sumSquare(), radius * radius));
93 const std::string folder =
"NED/lon-lat/";
96 for (
const auto &ecef_M_ned : ecef_M_ned_vec) {
97 std::stringstream buffer;
98 buffer << folder <<
"ecef_M_cv_" << std::setw(4) << std::setfill(
'0') << i++ <<
".txt";
99 std::string filename = buffer.str();
100 std::ofstream file(filename);
101 if (file.is_open()) {
102 (ecef_M_ned * ned_M_cv).save(file);
110 std::vector<vpHomogeneousMatrix> ecef_M_enu_vec =
112 for (
const auto &ecef_M_enu : ecef_M_enu_vec) {
114 std::cout <<
"Lon-Lat ecef_M_enu:\n" << ecef_M_enu << std::endl;
116 CHECK(ecef_M_enu.isValid());
117 CHECK(ecef_M_enu.getRotationMatrix().isARotationMatrix());
118 CHECK(
vpMath::equal(ecef_M_enu.getTranslationVector().sumSquare(), radius * radius));
124 const std::string folder =
"ENU/lon-lat/";
127 for (
const auto &ecef_M_enu : ecef_M_enu_vec) {
128 std::stringstream buffer;
129 buffer << folder <<
"ecef_M_cv_" << std::setw(4) << std::setfill(
'0') << i++ <<
".txt";
130 std::string filename = buffer.str();
131 std::ofstream file(filename);
132 if (file.is_open()) {
133 (ecef_M_enu * enu_M_cv).save(file);
141 TEST_CASE(
"Equidistributed sphere point",
"[math_equi_sphere_pts]")
143 const unsigned int maxPoints = 200;
145 const double radius = 5;
149 std::vector<vpHomogeneousMatrix> ecef_M_ned_vec =
151 CHECK(!ecef_M_ned_vec.empty());
152 for (
const auto &ecef_M_ned : ecef_M_ned_vec) {
154 std::cout <<
"Equidistributed ecef_M_ned:\n" << ecef_M_ned << std::endl;
156 CHECK(ecef_M_ned.isValid());
157 CHECK(ecef_M_ned.getRotationMatrix().isARotationMatrix());
158 CHECK(
vpMath::equal(ecef_M_ned.getTranslationVector().sumSquare(), radius * radius));
167 const std::string folder =
"NED/equi/";
170 for (
const auto &ecef_M_ned : ecef_M_ned_vec) {
171 std::stringstream buffer;
172 buffer << folder <<
"ecef_M_cv_" << std::setw(4) << std::setfill(
'0') << i++ <<
".txt";
173 std::string filename = buffer.str();
174 std::ofstream file(filename);
175 if (file.is_open()) {
176 (ecef_M_ned * ned_M_cv).save(file);
184 std::vector<vpHomogeneousMatrix> ecef_M_enu_vec =
186 CHECK(!ecef_M_enu_vec.empty());
187 for (
const auto &ecef_M_enu : ecef_M_enu_vec) {
189 std::cout <<
"Equidistributed ecef_M_enu:\n" << ecef_M_enu << std::endl;
191 CHECK(ecef_M_enu.isValid());
192 CHECK(ecef_M_enu.getRotationMatrix().isARotationMatrix());
193 CHECK(
vpMath::equal(ecef_M_enu.getTranslationVector().sumSquare(), radius * radius));
200 const std::string folder =
"ENU/equi/";
203 for (
const auto &ecef_M_enu : ecef_M_enu_vec) {
204 std::stringstream buffer;
205 buffer << folder <<
"ecef_M_cv_" << std::setw(4) << std::setfill(
'0') << i++ <<
".txt";
206 std::string filename = buffer.str();
207 std::ofstream file(filename);
208 if (file.is_open()) {
209 (ecef_M_enu * enu_M_cv).save(file);
216 TEST_CASE(
"Look-at",
"[math_look_at]")
219 vpColVector from_blender = {8.867762565612793, -1.1965436935424805, 2.1211400032043457};
222 blender_M_gl[0][0] = 0;
223 blender_M_gl[0][2] = 1;
224 blender_M_gl[1][0] = 1;
225 blender_M_gl[1][1] = 0;
226 blender_M_gl[2][1] = 1;
227 blender_M_gl[2][2] = 0;
238 std::cout <<
"\ngl_M_cam:\n" << gl_M_cam << std::endl;
246 std::cout <<
"\nbl_M_cv:\n" << bl_M_cv << std::endl;
249 vpHomogeneousMatrix bl_M_cv_gt = {0.13372008502483368, 0.22858507931232452, -0.9642965197563171,
250 8.867762565612793, 0.9910191297531128, -0.030843468382954597,
251 0.13011434674263, -1.1965436935424805, -5.4016709327697754e-08,
252 -0.9730352163314819, -0.23065657913684845, 2.121140241622925};
253 std::cout <<
"\nbl_M_cv_gt:\n" << bl_M_cv_gt << std::endl;
255 const double tolerance = 1e-6;
256 for (
unsigned int i = 0; i < 3; i++) {
257 for (
unsigned int j = 0; j < 4; j++) {
258 CHECK(
vpMath::equal(bl_M_cv[i][j], bl_M_cv_gt[i][j], tolerance));
263 int main(
int argc,
char *argv[])
265 Catch::Session session;
268 session.applyCommandLine(argc, argv);
270 int numFailed = session.run();
280 int main() {
return EXIT_SUCCESS; }
Implementation of column vector and the associated operations.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
static std::vector< vpHomogeneousMatrix > getLocalTangentPlaneTransformations(const std::vector< std::pair< double, double > > &lonlatVec, double radius, vpHomogeneousMatrix(*toECEF)(double lonDeg, double latDeg, double radius))
static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp)
static bool equal(double x, double y, double threshold=0.001)
static vpHomogeneousMatrix enu2ecef(double lonDeg, double latDeg, double radius)
static std::vector< std::pair< double, double > > computeRegularPointsOnSphere(unsigned int maxPoints)
static std::vector< double > linspace(T start_in, T end_in, unsigned int num_in)
static vpHomogeneousMatrix ned2ecef(double lonDeg, double latDeg, double radius)
vpRotationMatrix t() const