diff --git a/VirtualRobot/math/Helpers.h b/VirtualRobot/math/Helpers.h
index 0391c9e273663009c6e3e823e988fc19405fc327..b9d83d681262c4ceccffd775a8871918f48a0c09 100644
--- a/VirtualRobot/math/Helpers.h
+++ b/VirtualRobot/math/Helpers.h
@@ -247,9 +247,10 @@ namespace math
static std::vector<Eigen::Matrix4f> CreatePoses(const std::vector<Eigen::Vector3f>& positions, const std::vector<Eigen::Matrix3f>& orientations);
// Conversions
- static std::vector<float> VectorToStd(const Eigen::VectorXf& vec){
+ static std::vector<float> VectorToStd(const Eigen::VectorXf& vec)
+ {
std::vector<float> res;
- res.resize(vec.size());
+ res.resize(static_cast<std::size_t>(vec.size()));
Eigen::VectorXf::Map(res.data(), vec.size()) = vec;
return res;
}
diff --git a/VirtualRobot/tests/MathHelpersTest.cpp b/VirtualRobot/tests/MathHelpersTest.cpp
index aab08877854ebf5075281c3a5ec58838a182bef9..73e0961d9837dbe099927aaa450779aec5a268e8 100644
--- a/VirtualRobot/tests/MathHelpersTest.cpp
+++ b/VirtualRobot/tests/MathHelpersTest.cpp
@@ -48,7 +48,7 @@ BOOST_AUTO_TEST_CASE(test_GetRotationMatrix)
{
Eigen::Vector3f source(1, 2, 3), target(-3, 2, 5); // not normalized
Eigen::Matrix3f matrix = Helpers::GetRotationMatrix(source, target);
-
+
BOOST_CHECK((matrix * matrix.transpose()).isIdentity(1e-6f));
BOOST_CHECK((matrix * source.normalized()).isApprox(target.normalized(), 1e-6f));
}
@@ -59,31 +59,31 @@ BOOST_AUTO_TEST_CASE(test_TransformPosition)
Eigen::Vector3f translation(4, 5, 6);
Eigen::AngleAxisf rotation(static_cast<float>(M_PI_2), Eigen::Vector3f::UnitY());
-
+
Eigen::Matrix4f transform = transform.Identity();
-
+
// identity
transform.setIdentity();
- BOOST_CHECK_EQUAL(Helpers::TransformPosition(transform, vector),
+ BOOST_CHECK_EQUAL(Helpers::TransformPosition(transform, vector),
vector);
-
+
// translation only
transform.setIdentity();
Helpers::Position(transform) = translation;
- BOOST_CHECK_EQUAL(Helpers::TransformPosition(transform, vector),
+ BOOST_CHECK_EQUAL(Helpers::TransformPosition(transform, vector),
vector + translation);
-
+
// rotation only
transform.setIdentity();
Helpers::Orientation(transform) = rotation.toRotationMatrix();
- BOOST_CHECK_EQUAL(Helpers::TransformPosition(transform, vector),
+ BOOST_CHECK_EQUAL(Helpers::TransformPosition(transform, vector),
rotation * vector);
// full transform
transform.setIdentity();
Helpers::Position(transform) = translation;
Helpers::Orientation(transform) = rotation.toRotationMatrix();
- BOOST_CHECK_EQUAL(Helpers::TransformPosition(transform, vector),
+ BOOST_CHECK_EQUAL(Helpers::TransformPosition(transform, vector),
rotation * vector + translation);
}
@@ -92,16 +92,16 @@ BOOST_AUTO_TEST_CASE(test_InvertPose)
{
Eigen::Vector3f translation(4, 5, 6);
Eigen::AngleAxisf rotation(static_cast<float>(M_PI_2), Eigen::Vector3f::UnitY());
-
+
Eigen::Matrix4f pose = Helpers::Pose(translation, rotation);
Eigen::Matrix4f inv;
-
+
// in-place
inv = pose;
Helpers::InvertPose(inv);
BOOST_CHECK((pose * inv).isIdentity(1e-6f));
BOOST_CHECK((inv * pose).isIdentity(1e-6f));
-
+
// returned
inv.setIdentity();
inv = Helpers::InvertedPose(pose);
@@ -119,23 +119,23 @@ struct BlockFixture
BlockFixture()
{
quat = Quaternionf{
- AngleAxisf(static_cast<float>(M_PI), Vector3f::UnitZ())
+ AngleAxisf(static_cast<float>(M_PI), Vector3f::UnitZ())
* AngleAxisf(static_cast<float>(M_PI_2), Vector3f::UnitY())
};
-
+
quat2 = AngleAxisf(static_cast<float>(M_PI_4), Vector3f::UnitX()) * quat;
-
+
pos = Vector3f{ 1, 2, 3 };
pos2 = Vector3f{ 4, 5, 6 };
-
+
ori = quat.toRotationMatrix();
ori2 = quat2.toRotationMatrix();
-
+
pose.setIdentity();
pose.block<3, 1>(0, 3) = pos;
pose.block<3, 3>(0, 0) = ori;
}
-
+
Matrix4f pose;
Vector3f pos, pos2;
Matrix3f ori, ori2;
@@ -156,7 +156,7 @@ BOOST_AUTO_TEST_CASE(test_Position_const)
BOOST_AUTO_TEST_CASE(test_Position_nonconst)
{
BOOST_CHECK_EQUAL(Helpers::Position(pose), pos);
-
+
Helpers::Position(pose) = pos2;
BOOST_CHECK_EQUAL(Helpers::Position(pose), pos2);
}
@@ -170,7 +170,7 @@ BOOST_AUTO_TEST_CASE(test_Orientation_const)
BOOST_AUTO_TEST_CASE(test_Orientation_nonconst)
{
BOOST_CHECK_EQUAL(Helpers::Orientation(pose), ori);
-
+
Helpers::Orientation(pose) = ori2;
BOOST_CHECK_EQUAL(Helpers::Orientation(pose), ori2);
}
@@ -215,7 +215,7 @@ struct OrthogonolizeFixture
{
void test(double angle, const Vector3d& axis, float noiseAmpl, float precAngularDist, float precOrth = -1);
Eigen::Matrix3f test(Matrix3f matrix, float noiseAmpl, float precOrth = -1);
-
+
template <typename Distribution>
static Eigen::Matrix3f Random(Distribution& distrib)
{
@@ -231,11 +231,11 @@ void OrthogonolizeFixture::test(
// construct matrix with double to avoid rounding errors
Eigen::AngleAxisd rot(angle, axis);
Eigen::Quaterniond quat(rot);
-
+
Eigen::Matrix3f matrix = quat.toRotationMatrix().cast<float>();
-
+
Eigen::Matrix3f orth = test(matrix, noiseAmpl, precOrth);
-
+
Quaternionf quatOrth(orth);
BOOST_TEST_MESSAGE("Angular distance: " << quatOrth.angularDistance(quat.cast<float>()));
BOOST_CHECK_LE(quatOrth.angularDistance(quat.cast<float>()), precAngularDist);
@@ -244,34 +244,34 @@ void OrthogonolizeFixture::test(
Matrix3f OrthogonolizeFixture::test(Matrix3f matrix, float noiseAmpl, float _precOrth)
{
const float precOrth = _precOrth > 0 ? _precOrth : 1e-6f;
-
+
const Eigen::Vector3f pos(3, -1, 2);
Eigen::Matrix4f pose = Helpers::Pose(pos, matrix);
pose.row(3) << 1, 2, 3, 4; // destroy last row
-
+
BOOST_TEST_MESSAGE("Rotation matrix: \n" << matrix);
BOOST_CHECK(math::Helpers::IsMatrixOrthogonal(matrix, precOrth));
-
+
BOOST_TEST_MESSAGE("Pose matrix: \n" << pose);
-
-
+
+
// add noise (random coeffs are in [-1, 1])
std::normal_distribution<float> distrib(0, noiseAmpl);
const Eigen::Matrix3f noise = noiseAmpl * this->Random(distrib);
-
+
matrix += noise;
Helpers::Orientation(pose) += noise;
-
+
BOOST_TEST_MESSAGE("Rotation matrix with noise: \n" << matrix);
if (noiseAmpl > 0)
{
BOOST_CHECK(!math::Helpers::IsMatrixOrthogonal(matrix, precOrth));
BOOST_CHECK(!math::Helpers::IsMatrixOrthogonal(Helpers::Orientation(pose), precOrth));
}
-
+
Eigen::Matrix3f orth = math::Helpers::Orthogonalize(matrix);
Eigen::Matrix4f poseOrth = math::Helpers::Orthogonalize(pose);
-
+
BOOST_TEST_MESSAGE("Orthogonalized: \n" << orth);
BOOST_TEST_MESSAGE("R * R.T: (should be Identitiy) \n" << (orth * orth.transpose()));
BOOST_CHECK(math::Helpers::IsMatrixOrthogonal(orth, precOrth));
@@ -283,7 +283,7 @@ Matrix3f OrthogonolizeFixture::test(Matrix3f matrix, float noiseAmpl, float _pre
BOOST_CHECK_EQUAL(math::Helpers::Position(poseOrth), pos);
BOOST_CHECK_EQUAL(poseOrth.row(3).head<3>(), Eigen::Vector3f::Zero().transpose());
BOOST_CHECK_EQUAL(poseOrth(3, 3), 1);
-
+
return orth;
}
@@ -294,7 +294,7 @@ BOOST_AUTO_TEST_CASE(test_orthogonalize_zero_rotation)
{
test(Eigen::Matrix3f::Identity(), 0);
test(Eigen::Matrix3f::Identity(), 0.1f);
-
+
test(0, Eigen::Vector3d::UnitX(), 0.0f, 0.0f);
test(0, Eigen::Vector3d::UnitX(), 1e-3f, 1e-3f);
}
@@ -303,10 +303,10 @@ BOOST_AUTO_TEST_CASE(test_orthogonalize_aligned_axis)
{
test(M_PI / 2, Eigen::Vector3d::UnitX(), 1e-3f, 1e-3f);
test(M_PI / 2, Eigen::Vector3d::UnitX(), 0.1f, 0.2f);
-
+
test(.75 * M_PI, Eigen::Vector3d::UnitZ(), 1e-3f, 1e-3f);
test(.75 * M_PI, Eigen::Vector3d::UnitZ(), 0.1f, 0.2f);
-
+
test(M_PI, Eigen::Vector3d::UnitY(), 1e-3f, 1e-3f);
test(M_PI, Eigen::Vector3d::UnitY(), 0.1f, 0.2f);
}
@@ -315,7 +315,7 @@ BOOST_AUTO_TEST_CASE(test_orthogonalize_arbitrary_rotation)
{
test(2.3, Eigen::Vector3d( 0.3, 1., -.5).normalized(), 1e-3f, 1e-3f);
test(2.3, Eigen::Vector3d( 0.3, 1., -.5).normalized(), 0.1f, 0.2f);
-
+
test(1.02, Eigen::Vector3d( -2, .3, -.25).normalized(), 1e-3f, 1e-3f);
test(1.02, Eigen::Vector3d( -3, 2, -10).normalized(), 0.1f, 0.2f, 1e-5f);
}
@@ -332,12 +332,12 @@ struct DegreeRadianConversionFixture
const double deg2radd = static_cast<double>(M_PI / 180.);
const float deg2radf = static_cast<float>(deg2radd);
-
+
const float prec = 1e-5f;
-
+
const Eigen::Vector2f vector2f = {0, 1};
const Eigen::Vector2d vector2d = {0, 1};
-
+
const Eigen::Vector3f vector3f = {0, 1, -2};
const Eigen::Vector3d vector3d = {0, 1, -2};
};
@@ -351,17 +351,17 @@ BOOST_AUTO_TEST_CASE(test_rad2deg_scalar)
BOOST_CHECK_CLOSE(Helpers::rad2deg( 0.0f), 0.0f, prec);
BOOST_CHECK_CLOSE(Helpers::rad2deg( 1.0f), rad2degf, prec);
BOOST_CHECK_CLOSE(Helpers::rad2deg(-2.0f), -2 * rad2degf, prec);
-
+
// double
BOOST_CHECK_CLOSE(Helpers::rad2deg( 0.0), 0.0, prec);
BOOST_CHECK_CLOSE(Helpers::rad2deg( 1.0), rad2degd, prec);
BOOST_CHECK_CLOSE(Helpers::rad2deg(-2.0), -2 * rad2degd, prec);
-
+
// int
BOOST_CHECK_CLOSE(Helpers::rad2deg( 0), 0.0f, prec);
BOOST_CHECK_CLOSE(Helpers::rad2deg( 1), rad2degf, prec);
BOOST_CHECK_CLOSE(Helpers::rad2deg(-2), -2 * rad2degf, prec);
-
+
// long
BOOST_CHECK_CLOSE(Helpers::rad2deg( 0l), 0.0f, prec);
BOOST_CHECK_CLOSE(Helpers::rad2deg( 1l), rad2degf, prec);
@@ -373,10 +373,10 @@ BOOST_AUTO_TEST_CASE(test_rad2deg_vector)
{
BOOST_CHECK(math::Helpers::rad2deg(vector2f).isApprox(vector2f * rad2degf));
BOOST_CHECK(math::Helpers::rad2deg(vector2d).isApprox(vector2d * rad2degd));
-
+
BOOST_CHECK(math::Helpers::rad2deg(vector3f).isApprox(vector3f * rad2degf));
BOOST_CHECK(math::Helpers::rad2deg(vector3d).isApprox(vector3d * rad2degd));
-
+
// Insert expression instead of value.
BOOST_CHECK(math::Helpers::rad2deg(3*vector3f).isApprox(3 * vector3f * rad2degf));
BOOST_CHECK(math::Helpers::rad2deg(3*vector3d).isApprox(3 * vector3d * rad2degd));
@@ -389,17 +389,17 @@ BOOST_AUTO_TEST_CASE(test_deg2rad_scalar)
BOOST_CHECK_CLOSE(Helpers::deg2rad( 0.0f), 0.0f, prec);
BOOST_CHECK_CLOSE(Helpers::deg2rad( 1.0f), deg2radf, prec);
BOOST_CHECK_CLOSE(Helpers::deg2rad(-2.0f), -2 * deg2radf, prec);
-
+
// double
BOOST_CHECK_CLOSE(Helpers::deg2rad( 0.0), 0.0, prec);
BOOST_CHECK_CLOSE(Helpers::deg2rad( 1.0), deg2radd, prec);
BOOST_CHECK_CLOSE(Helpers::deg2rad(-2.0), -2 * deg2radf, prec);
-
+
// int
BOOST_CHECK_CLOSE(Helpers::deg2rad( 0), 0, prec);
BOOST_CHECK_CLOSE(Helpers::deg2rad( 1), deg2radf, prec);
BOOST_CHECK_CLOSE(Helpers::deg2rad(-2), -2 * deg2radf, prec);
-
+
// long
BOOST_CHECK_CLOSE(Helpers::deg2rad( 0l), 0.0f, prec);
BOOST_CHECK_CLOSE(Helpers::deg2rad( 1l), deg2radf, prec);
@@ -411,10 +411,10 @@ BOOST_AUTO_TEST_CASE(test_deg2rad_vector)
{
BOOST_CHECK(math::Helpers::deg2rad(vector2f).isApprox(vector2f * deg2radf));
BOOST_CHECK(math::Helpers::deg2rad(vector2d).isApprox(vector2d * deg2radd));
-
+
BOOST_CHECK(math::Helpers::deg2rad(vector3f).isApprox(vector3f * deg2radf));
BOOST_CHECK(math::Helpers::deg2rad(vector3d).isApprox(vector3d * deg2radd));
-
+
// Insert expression instead of value.
BOOST_CHECK(math::Helpers::deg2rad(3*vector3f).isApprox(3 * vector3f * deg2radf));
BOOST_CHECK(math::Helpers::deg2rad(3*vector3d).isApprox(3 * vector3d * deg2radd));