| 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 <gtest/gtest.h> |
| 11 |
#include <mrpt/math/ops_vectors.h> |
| 12 |
#include <mrpt/poses/CPose3D.h> |
| 13 |
#include <mrpt/poses/CPose3DQuat.h> |
| 14 |
#include <mrpt/random.h> |
| 15 |
#include <mrpt/tfest.h> |
| 16 |
|
| 17 |
using namespace mrpt; |
| 18 |
using namespace mrpt::tfest; |
| 19 |
using namespace mrpt::math; |
| 20 |
using namespace mrpt::random; |
| 21 |
using namespace mrpt::poses; |
| 22 |
using namespace std; |
| 23 |
|
| 24 |
using TPoints = std::vector<std::vector<double>>; |
| 25 |
|
| 26 |
// ------------------------------------------------------ |
| 27 |
// Generate both sets of points |
| 28 |
// ------------------------------------------------------ |
| 29 |
CPose3DQuat generate_points(TPoints& pA, TPoints& pB) |
| 30 |
{ |
| 31 |
const double Dx = 0.5; |
| 32 |
const double Dy = 1.5; |
| 33 |
const double Dz = 0.75; |
| 34 |
|
| 35 |
const double yaw = 10.0_deg; |
| 36 |
const double pitch = 20.0_deg; |
| 37 |
const double roll = 5.0_deg; |
| 38 |
|
| 39 |
pA.resize(5); // A set of points at "A" reference system |
| 40 |
pB.resize(5); // A set of points at "B" reference system |
| 41 |
|
| 42 |
pA[0].resize(3); |
| 43 |
pA[0][0] = 0.0; |
| 44 |
pA[0][1] = 0.5; |
| 45 |
pA[0][2] = 0.4; |
| 46 |
pA[1].resize(3); |
| 47 |
pA[1][0] = 1.0; |
| 48 |
pA[1][1] = 1.5; |
| 49 |
pA[1][2] = -0.1; |
| 50 |
pA[2].resize(3); |
| 51 |
pA[2][0] = 1.2; |
| 52 |
pA[2][1] = 1.1; |
| 53 |
pA[2][2] = 0.9; |
| 54 |
pA[3].resize(3); |
| 55 |
pA[3][0] = 0.7; |
| 56 |
pA[3][1] = 0.3; |
| 57 |
pA[3][2] = 3.4; |
| 58 |
pA[4].resize(3); |
| 59 |
pA[4][0] = 1.9; |
| 60 |
pA[4][1] = 2.5; |
| 61 |
pA[4][2] = -1.7; |
| 62 |
|
| 63 |
CPose3DQuat qPose = CPose3DQuat(CPose3D(Dx, Dy, Dz, yaw, pitch, roll)); |
| 64 |
for (unsigned int i = 0; i < 5; ++i) |
| 65 |
{ |
| 66 |
pB[i].resize(3); |
| 67 |
qPose.inverseComposePoint(pA[i][0], pA[i][1], pA[i][2], pB[i][0], pB[i][1], pB[i][2]); |
| 68 |
} |
| 69 |
|
| 70 |
return qPose; |
| 71 |
|
| 72 |
} // end generate_points |
| 73 |
|
| 74 |
// ------------------------------------------------------ |
| 75 |
// Generate a list of matched points |
| 76 |
// ------------------------------------------------------ |
| 77 |
template <typename T> |
| 78 |
void generate_list_of_points(const TPoints& pA, const TPoints& pB, TMatchingPairListTempl<T>& list) |
| 79 |
{ |
| 80 |
TMatchingPairTempl<T> pair; |
| 81 |
for (unsigned int i = 0; i < 5; ++i) |
| 82 |
{ |
| 83 |
pair.globalIdx = pair.localIdx = i; |
| 84 |
pair.global.x = d2f(pA[i][0]); |
| 85 |
pair.global.y = d2f(pA[i][1]); |
| 86 |
pair.global.z = d2f(pA[i][2]); |
| 87 |
|
| 88 |
pair.local.x = d2f(pB[i][0]); |
| 89 |
pair.local.y = d2f(pB[i][1]); |
| 90 |
pair.local.z = d2f(pB[i][2]); |
| 91 |
|
| 92 |
list.push_back(pair); |
| 93 |
} |
| 94 |
} // end generate_list_of_points |
| 95 |
|
| 96 |
// ------------------------------------------------------ |
| 97 |
// Genreate a vector of matched points |
| 98 |
// ------------------------------------------------------ |
| 99 |
void generate_vector_of_points( |
| 100 |
const TPoints& pA, |
| 101 |
const TPoints& pB, |
| 102 |
vector<mrpt::math::TPoint3D>& ptsA, |
| 103 |
vector<mrpt::math::TPoint3D>& ptsB) |
| 104 |
{ |
| 105 |
// The input vector: inV = [pA1x, pA1y, pA1z, pB1x, pB1y, pB1z, ... ] |
| 106 |
ptsA.resize(pA.size()); |
| 107 |
ptsB.resize(pA.size()); |
| 108 |
for (unsigned int i = 0; i < pA.size(); ++i) |
| 109 |
{ |
| 110 |
ptsA[i] = mrpt::math::TPoint3D(pA[i][0], pA[i][1], pA[i][2]); |
| 111 |
ptsB[i] = mrpt::math::TPoint3D(pB[i][0], pB[i][1], pB[i][2]); |
| 112 |
} |
| 113 |
} // end generate_vector_of_points |
| 114 |
|
| 115 |
template <typename T> |
| 116 |
void se3_l2_MatchList_test() |
| 117 |
{ |
| 118 |
TPoints pA, pB; // The input points |
| 119 |
CPose3DQuat qPose = generate_points(pA, pB); |
| 120 |
|
| 121 |
TMatchingPairListTempl<T> list; |
| 122 |
generate_list_of_points(pA, pB, list); // Generate a list of matched points |
| 123 |
|
| 124 |
CPose3DQuat outQuat; // Output CPose3DQuat for the LSRigidTransformation |
| 125 |
double scale; // Output scale value |
| 126 |
|
| 127 |
bool res = mrpt::tfest::se3_l2(list, outQuat, scale); |
| 128 |
EXPECT_TRUE(res); |
| 129 |
|
| 130 |
double err = 0.0; |
| 131 |
if ((qPose[3] * outQuat[3] > 0 && qPose[4] * outQuat[4] > 0 && qPose[5] * outQuat[5] > 0 && |
| 132 |
qPose[6] * outQuat[6] > 0) || |
| 133 |
(qPose[3] * outQuat[3] < 0 && qPose[4] * outQuat[4] < 0 && qPose[5] * outQuat[5] < 0 && |
| 134 |
qPose[6] * outQuat[6] < 0)) |
| 135 |
{ |
| 136 |
for (unsigned int i = 0; i < 7; ++i) err += square(std::fabs(qPose[i]) - std::fabs(outQuat[i])); |
| 137 |
err = sqrt(err); |
| 138 |
EXPECT_TRUE(err < 1e-6) << "Applied quaternion: " << endl |
| 139 |
<< qPose << endl |
| 140 |
<< "Out CPose3DQuat: " << endl |
| 141 |
<< outQuat << " [Err: " << err << "]" << endl; |
| 142 |
} |
| 143 |
else |
| 144 |
{ |
| 145 |
GTEST_FAIL() << "Applied quaternion: " << endl |
| 146 |
<< qPose << endl |
| 147 |
<< "Out CPose3DQuat: " << endl |
| 148 |
<< outQuat << endl; |
| 149 |
} |
| 150 |
} |
| 151 |
|
| 152 |
TEST(tfest, se3_l2_MatchList_float) { se3_l2_MatchList_test<float>(); } |
| 153 |
TEST(tfest, se3_l2_MatchList_double) { se3_l2_MatchList_test<double>(); } |
| 154 |
|
| 155 |
TEST(tfest, se3_l2_PtsLists) |
| 156 |
{ |
| 157 |
TPoints pA, pB; // The input points |
| 158 |
CPose3DQuat qPose = generate_points(pA, pB); |
| 159 |
|
| 160 |
vector<mrpt::math::TPoint3D> ptsA, ptsB; |
| 161 |
generate_vector_of_points(pA, pB, ptsA, |
| 162 |
ptsB); // Generate a vector of matched points |
| 163 |
|
| 164 |
mrpt::poses::CPose3DQuat qu; |
| 165 |
double scale; |
| 166 |
mrpt::tfest::se3_l2(ptsA, ptsB, qu, |
| 167 |
scale); // Output quaternion for the Horn Method |
| 168 |
|
| 169 |
double err = 0.0; |
| 170 |
if ((qPose[3] * qu[3] > 0 && qPose[4] * qu[4] > 0 && qPose[5] * qu[5] > 0 && |
| 171 |
qPose[6] * qu[6] > 0) || |
| 172 |
(qPose[3] * qu[3] < 0 && qPose[4] * qu[4] < 0 && qPose[5] * qu[5] < 0 && |
| 173 |
qPose[6] * qu[6] < 0)) |
| 174 |
{ |
| 175 |
for (unsigned int i = 0; i < 7; ++i) err += square(std::fabs(qPose[i]) - std::fabs(qu[i])); |
| 176 |
err = sqrt(err); |
| 177 |
EXPECT_TRUE(err < 1e-6) << "Applied quaternion: " << endl |
| 178 |
<< qPose << endl |
| 179 |
<< "Out CPose3DQuat: " << endl |
| 180 |
<< qu << " [Err: " << err << "]" << endl; |
| 181 |
} |
| 182 |
else |
| 183 |
{ |
| 184 |
GTEST_FAIL() << "Applied quaternion: " << endl |
| 185 |
<< qPose << endl |
| 186 |
<< "Out CPose3DQuat: " << endl |
| 187 |
<< qu << endl; |
| 188 |
} |
| 189 |
} // end |
| 190 |
|
| 191 |
TEST(tfest, se3_l2_robust) |
| 192 |
{ |
| 193 |
TPoints pA, pB; // The input points |
| 194 |
CPose3DQuat qPose = generate_points(pA, pB); |
| 195 |
|
| 196 |
TMatchingPairList list; |
| 197 |
generate_list_of_points(pA, pB, list); // Generate a list of matched points |
| 198 |
|
| 199 |
mrpt::tfest::TSE3RobustResult estim_result; |
| 200 |
mrpt::tfest::TSE3RobustParams params; |
| 201 |
params.ransac_minSetSize = 3; |
| 202 |
params.ransac_maxSetSizePct = 3.0 / list.size(); |
| 203 |
mrpt::tfest::se3_l2_robust(list, params, estim_result); |
| 204 |
|
| 205 |
EXPECT_GT(estim_result.inliers_idx.size(), 0u); |
| 206 |
const CPose3DQuat& outQuat = estim_result.transformation; |
| 207 |
double err = 0.0; |
| 208 |
if ((qPose[3] * outQuat[3] > 0 && qPose[4] * outQuat[4] > 0 && qPose[5] * outQuat[5] > 0 && |
| 209 |
qPose[6] * outQuat[6] > 0) || |
| 210 |
(qPose[3] * outQuat[3] < 0 && qPose[4] * outQuat[4] < 0 && qPose[5] * outQuat[5] < 0 && |
| 211 |
qPose[6] * outQuat[6] < 0)) |
| 212 |
{ |
| 213 |
for (unsigned int i = 0; i < 7; ++i) err += square(std::fabs(qPose[i]) - std::fabs(outQuat[i])); |
| 214 |
err = sqrt(err); |
| 215 |
EXPECT_TRUE(err < 1e-6) << "Applied quaternion: " << endl |
| 216 |
<< qPose << endl |
| 217 |
<< "Out CPose3DQuat: " << endl |
| 218 |
<< outQuat << " [Err: " << err << "]" << endl; |
| 219 |
} |
| 220 |
else |
| 221 |
{ |
| 222 |
GTEST_FAIL() << "Applied quaternion: " << endl |
| 223 |
<< qPose << endl |
| 224 |
<< "Out CPose3DQuat: " << endl |
| 225 |
<< outQuat << endl; |
| 226 |
} |
| 227 |
} |