32 #include <gtest/gtest.h> 41 std::srand(std::time(0));
49 unsigned int runs = 100;
54 std::vector<double>
values(runs);
59 for ( uint64_t i = 1; i < runs ; i++ )
61 values[i] = 10.0 * ((double) std::rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
73 for ( uint64_t i = 1; i < runs ; i++ )
85 unsigned int runs = 100;
90 std::vector<double>
values(runs);
95 for (
int i = runs -1; i >= 0 ; i-- )
97 values[i] = 10.0 * ((double) std::rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
106 for ( uint64_t i = 1; i < runs ; i++ )
120 double my_vals[] = {13,2,5,4,9,7,3,11,15,14,12,1,6,10,0,8};
121 std::vector<double>
values (my_vals, my_vals +
sizeof(my_vals)/
sizeof(
double));
122 unsigned int runs = values.size();
127 for ( uint64_t i = 0; i <runs ; i++ )
129 values[i] = 10.0 * ((double) std::rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
138 for ( uint64_t i = 1; i < runs ; i++ )
153 std::vector<double>
values(runs);
158 for ( uint64_t i = 0; i <runs ; i++ )
160 values[i] = 10.0 * ((double) std::rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
172 for ( uint64_t i = 1; i < runs ; i++ )
201 std::vector<double> xvalues(2);
202 std::vector<double> yvalues(2);
203 std::vector<double> zvalues(2);
205 uint64_t offset = 200;
210 for ( uint64_t i = 1; i < runs ; i++ )
214 xvalues[
step] = 10.0 * ((double) std::rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
215 yvalues[
step] = 10.0 * ((double) std::rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
216 zvalues[
step] = 10.0 * ((double) std::rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
218 t.
setOrigin(tf::Vector3(xvalues[
step], yvalues[step], zvalues[step]));
225 for (
int pos = 0; pos < 100 ; pos++)
231 EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (
double)pos/100.0, x_out, epsilon);
232 EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (
double)pos/100.0, y_out, epsilon);
233 EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (
double)pos/100.0, z_out, epsilon);
349 for (uint64_t i = 0 ; i < runs ; i++)
351 q2.
setEuler(1.0 * ((
double) std::rand() - (
double)RAND_MAX /2.0) /(
double)RAND_MAX,
352 1.0 * ((
double) std::rand() - (
double)RAND_MAX /2.0) /(
double)RAND_MAX,
353 1.0 * ((
double) std::rand() - (
double)RAND_MAX /2.0) /(
double)RAND_MAX);
358 EXPECT_NEAR(q3.
angle(q1), q2.
angle(q3), 1e-5);
371 std::vector<double> yawvalues(2);
372 std::vector<double> pitchvalues(2);
373 std::vector<double> rollvalues(2);
374 uint64_t offset = 200;
376 std::vector<tf::Quaternion> quats(2);
381 for ( uint64_t i = 1; i < runs ; i++ )
385 yawvalues[
step] = 10.0 * ((double) std::rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX / 100.0;
386 pitchvalues[
step] = 0;
387 rollvalues[
step] = 0;
388 quats[
step].setRPY(yawvalues[
step], pitchvalues[step], rollvalues[step]);
396 for (
int pos = 0; pos < 100 ; pos ++)
405 EXPECT_NEAR(0,
angle(ground_truth, quat), epsilon);
430 EXPECT_TRUE(!std::isnan(stor.
rotation_.x()));
431 EXPECT_TRUE(!std::isnan(stor.
rotation_.y()));
432 EXPECT_TRUE(!std::isnan(stor.
rotation_.z()));
433 EXPECT_TRUE(!std::isnan(stor.
rotation_.w()));
437 int main(
int argc,
char **argv){
438 testing::InitGoogleTest(&argc, argv);
439 return RUN_ALL_TESTS();
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
TEST(TimeCache, Repeatability)
Time & fromNSec(uint64_t t)
std::vector< double > values
tfScalar angle(const Quaternion &q) const
Return the half angle between this quaternion and the other.
A class to keep a sorted linked list in time This builds and maintains a list of timestamped data...
TFSIMD_FORCE_INLINE Quaternion slerp(const Quaternion &q1, const Quaternion &q2, const tfScalar &t)
Return the result of spherical linear interpolation betwen two quaternions.
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
Return the half angle between two quaternions.
int main(int argc, char **argv)
bool insertData(const TransformStorage &new_data)
Quaternion slerp(const Quaternion &q, const tfScalar &t) const
Return the quaternion which is the result of Spherical Linear Interpolation between this and the othe...
void setEuler(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)
Set the quaternion using Euler angles.
bool getData(ros::Time time, TransformStorage &data_out, std::string *error_str=0)