Skip to content

Commit

Permalink
C++11 alignas() instead of Eigen macros (cc #66)
Browse files Browse the repository at this point in the history
  • Loading branch information
jlblancoc committed Nov 3, 2017
1 parent b944a6e commit c35ffb9
Show file tree
Hide file tree
Showing 19 changed files with 49 additions and 61 deletions.
4 changes: 2 additions & 2 deletions apps/mrpt-performance/perf-matrix2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -118,7 +118,7 @@ double matrix_test_chol_Nx6x6_sparse(int DIM, int a2)

double matrix_test_loadFromArray(int N, int a2)
{
MRPT_ALIGN16 double nums[4 * 4] = {0, 1, 2, 3, 4, 5, 6, 7,
alignas(16) double nums[4 * 4] = {0, 1, 2, 3, 4, 5, 6, 7,
8, 9, 10, 11, 12, 13, 14, 15};

CMatrixFixedNumeric<double, 4, 4> M;
Expand All @@ -130,7 +130,7 @@ double matrix_test_loadFromArray(int N, int a2)

double matrix_test_loadWithEigenMap(int N, int a2)
{
MRPT_ALIGN16 double nums[4 * 4] = {0, 1, 2, 3, 4, 5, 6, 7,
alignas(16) double nums[4 * 4] = {0, 1, 2, 3, 4, 5, 6, 7,
8, 9, 10, 11, 12, 13, 14, 15};

CMatrixFixedNumeric<double, 4, 4> M;
Expand Down
2 changes: 1 addition & 1 deletion libs/base/include/mrpt/utils/SSE_macros.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
#define BUILD_128BIT_CONST( \
_name, B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, B10, B11, B12, B13, B14, \
B15) \
MRPT_ALIGN16 const unsigned long long _name[2] = { \
alignas(16) const unsigned long long _name[2] = { \
0x##B7##B6##B5##B4##B3##B2##B1##B0##ull, \
0x##B15##B14##B13##B12##B11##B10##B9##B8##ull};

Expand Down
12 changes: 0 additions & 12 deletions libs/base/include/mrpt/utils/mrpt_macros.h
Original file line number Diff line number Diff line change
Expand Up @@ -96,18 +96,6 @@ need to account for this with an extra offset.
#define MRPT_scanf_format_check(_FMT_, _VARARGS_)
#endif

// A cross-compiler definition for aligned memory structures:
#if defined(_MSC_VER)
#define MRPT_ALIGN16 __declspec(align(16))
#define MRPT_ALIGN32 __declspec(align(32))
#elif defined(__GNUC__)
#define MRPT_ALIGN16 __attribute__((aligned(16)))
#define MRPT_ALIGN32 __attribute__((aligned(32)))
#else
#define MRPT_ALIGN16
#define MRPT_ALIGN32
#endif

/** A macro for obtaining the name of the current function: */
#if defined(_MSC_VER) && (_MSC_VER >= 1300)
#define __CURRENT_FUNCTION_NAME__ __FUNCTION__
Expand Down
2 changes: 1 addition & 1 deletion libs/base/src/math/lightweight_geom_data.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -287,7 +287,7 @@ void TPose3D::getAsQuaternion(
// http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
if (out_dq_dr)
{
MRPT_ALIGN16 const double nums[4 * 3] = {
alignas(16) const double nums[4 * 3] = {
-0.5 * q[3], 0.5 * (-csc + scs), -0.5 * q[1],
-0.5 * q[2], 0.5 * (-ssc - ccs), 0.5 * q[0],
0.5 * q[1], 0.5 * (ccc - sss), 0.5 * q[3],
Expand Down
6 changes: 3 additions & 3 deletions libs/base/src/math/matrix_ops5_unittest.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ using namespace std;

TEST(Matrices, loadFromArray)
{
MRPT_ALIGN16 const double nums[3 * 4] = {1, 2, 3, 4, 5, 6,
alignas(16) const double nums[3 * 4] = {1, 2, 3, 4, 5, 6,
7, 8, 9, 10, 11, 12};

CMatrixFixedNumeric<double, 3, 4> mat;
Expand All @@ -38,7 +38,7 @@ TEST(Matrices, loadFromArray)

TEST(Matrices, CMatrixFixedNumeric_loadWithEigenMap)
{
MRPT_ALIGN16 double nums[3 * 4] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};
alignas(16) double nums[3 * 4] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};

// Row major
const CMatrixFixedNumeric<double, 3, 4> mat =
Expand All @@ -51,7 +51,7 @@ TEST(Matrices, CMatrixFixedNumeric_loadWithEigenMap)

TEST(Matrices, EigenMatrix_loadWithEigenMap)
{
MRPT_ALIGN16 double nums[3 * 4] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};
alignas(16) double nums[3 * 4] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};
// Col major
const Eigen::Matrix<double, 3, 4> mat =
Eigen::Map<Eigen::Matrix<double, 3, 4>, Eigen::Aligned>(nums);
Expand Down
16 changes: 8 additions & 8 deletions libs/base/src/poses/CPose3D.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -282,7 +282,7 @@ void CPose3D::rebuildRotationMatrix()
const double sr = sin(m_roll);
#endif

MRPT_ALIGN16 const double rot_vals[] = {cy * cp,
alignas(16) const double rot_vals[] = {cy * cp,
cy * sp * sr - sy * cr,
cy * sp * cr + sy * sr,
sy * cp,
Expand Down Expand Up @@ -466,7 +466,7 @@ void CPose3D::composePoint(
if (use_small_rot_approx)
{
// Linearized Jacobians around (yaw,pitch,roll)=(0,0,0):
MRPT_ALIGN16 const double nums[3 * 6] = {
alignas(16) const double nums[3 * 6] = {
1, 0, 0, -ly, lz, 0, 0, 1, 0, lx, 0, -lz, 0, 0, 1, 0, -lx, ly};
out_jacobian_df_dpose->loadFromArray(nums);
}
Expand All @@ -490,7 +490,7 @@ void CPose3D::composePoint(
const double sr = sin(m_roll);
#endif

MRPT_ALIGN16 const double nums[3 * 6] = {
alignas(16) const double nums[3 * 6] = {
1, 0, 0,
-lx * sy * cp + ly * (-sy * sp * sr - cy * cr) +
lz * (-sy * sp * cr + cy * sr), // d_x'/d_yaw
Expand Down Expand Up @@ -523,7 +523,7 @@ void CPose3D::composePoint(
// Jacob: df/dse3
if (out_jacobian_df_dse3)
{
MRPT_ALIGN16 const double nums[3 * 6] = {
alignas(16) const double nums[3 * 6] = {
1, 0, 0, 0, gz, -gy, 0, 1, 0, -gz, 0, gx, 0, 0, 1, gy, -gx, 0};
out_jacobian_df_dse3->loadFromArray(nums);
}
Expand Down Expand Up @@ -791,7 +791,7 @@ void CPose3D::inverseComposePoint(
const double Ay = gy - m_coords[1];
const double Az = gz - m_coords[2];

MRPT_ALIGN16 const double nums[3 * 6] = {
alignas(16) const double nums[3 * 6] = {
-m_ROT(0, 0),
-m_ROT(1, 0),
-m_ROT(2, 0),
Expand Down Expand Up @@ -823,7 +823,7 @@ void CPose3D::inverseComposePoint(
// Jacob: df/dse3
if (out_jacobian_df_dse3)
{
MRPT_ALIGN16 const double nums[3 * 6] = {
alignas(16) const double nums[3 * 6] = {
-1, 0, 0, 0, -lz, ly, 0, -1, 0, lz, 0, -lx, 0, 0, -1, -ly, lx, 0};
out_jacobian_df_dse3->loadFromArray(nums);
}
Expand Down Expand Up @@ -892,7 +892,7 @@ inline void deltaR(const MAT33& R, VEC3& v)
template <typename VEC3, typename MAT3x3, typename MAT3x9>
inline void M3x9(const VEC3& a, const MAT3x3& B, MAT3x9& RES)
{
MRPT_ALIGN16 const double vals[] = {
alignas(16) const double vals[] = {
a[0], -B(0, 2), B(0, 1), B(0, 2), a[0], -B(0, 0), -B(0, 1),
B(0, 0), a[0], a[1], -B(1, 2), B(1, 1), B(1, 2), a[1],
-B(1, 0), -B(1, 1), B(1, 0), a[1], a[2], -B(2, 2), B(2, 1),
Expand All @@ -911,7 +911,7 @@ inline CMatrixDouble33 ddeltaRt_dR(const CPose3D& P)
double b = abc[1];
double c = abc[2];

MRPT_ALIGN16 const double vals[] = {
alignas(16) const double vals[] = {
-b * t[1] - c * t[2], 2 * b * t[0] - a * t[1],
2 * c * t[0] - a * t[2], -b * t[0] + 2 * a * t[1],
-a * t[0] - c * t[2], 2 * c * t[1] - b * t[2],
Expand Down
12 changes: 6 additions & 6 deletions libs/base/src/poses/CPose3DQuat.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -127,7 +127,7 @@ void CPose3DQuat::composePoint(
{
// 3x3: df_{qr} / da

MRPT_ALIGN16 const double vals[3 * 3] = {
alignas(16) const double vals[3 * 3] = {
1 - 2 * (qy2 + qz2),
2 * (m_quat.x() * m_quat.y() - m_quat.r() * m_quat.z()),
2 * (m_quat.r() * m_quat.y() + m_quat.x() * m_quat.z()),
Expand All @@ -146,11 +146,11 @@ void CPose3DQuat::composePoint(
if (out_jacobian_df_dpose)
{
// 3x7: df_{qr} / dp
MRPT_ALIGN16 const double vals1[3 * 7] = {
alignas(16) const double vals1[3 * 7] = {
1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0};
out_jacobian_df_dpose->loadFromArray(vals1);

MRPT_ALIGN16 const double vals[3 * 4] = {
alignas(16) const double vals[3 * 4] = {
2 * (-m_quat.z() * ly + m_quat.y() * lz),
2 * (m_quat.y() * ly + m_quat.z() * lz),
2 * (-2 * m_quat.y() * lx + m_quat.x() * ly + m_quat.r() * lz),
Expand Down Expand Up @@ -212,7 +212,7 @@ void CPose3DQuat::inverseComposePoint(
// 2*qy^2 + 1]
//

MRPT_ALIGN16 const double vals[3 * 3] = {
alignas(16) const double vals[3 * 3] = {
1 - 2 * (qy2 + qz2),
2 * (m_quat.x() * m_quat.y() + m_quat.r() * m_quat.z()),
2 * (-m_quat.r() * m_quat.y() + m_quat.x() * m_quat.z()),
Expand Down Expand Up @@ -250,7 +250,7 @@ void CPose3DQuat::inverseComposePoint(
const double qy = m_quat.y();
const double qz = m_quat.z();

MRPT_ALIGN16 const double vals1[3 * 7] = {
alignas(16) const double vals1[3 * 7] = {
2 * qy2 + 2 * qz2 - 1,
-2 * qr * qz - 2 * qx * qy,
2 * qr * qy - 2 * qx * qz,
Expand Down Expand Up @@ -282,7 +282,7 @@ void CPose3DQuat::inverseComposePoint(
const double Ay = 2 * (gy - m_coords[1]);
const double Az = 2 * (gz - m_coords[2]);

MRPT_ALIGN16 const double vals[3 * 4] = {
alignas(16) const double vals[3 * 4] = {
-qy * Az + qz * Ay,
qy * Ay + qz * Az,
qx * Ay - 2 * qy * Ax - qr * Az,
Expand Down
6 changes: 3 additions & 3 deletions libs/base/src/poses/CPose3DQuatPDF.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ void CPose3DQuatPDF::jacobiansPoseComposition(
df_dx.set_unsafe(1, 1, 1);
df_dx.set_unsafe(2, 2, 1);

MRPT_ALIGN16 const double vals2[3 * 4] = {
alignas(16) const double vals2[3 * 4] = {
2 * (-qz * ay + qy * az),
2 * (qy * ay + qz * az),
2 * (-2 * qy * ax + qx * ay + qr * az),
Expand All @@ -97,7 +97,7 @@ void CPose3DQuatPDF::jacobiansPoseComposition(

// second part:
{
MRPT_ALIGN16 const double aux44_data[4 * 4] = {
alignas(16) const double aux44_data[4 * 4] = {
q2r, -q2x, -q2y, -q2z, q2x, q2r, q2z, -q2y,
q2y, -q2z, q2r, q2x, q2z, q2y, -q2x, q2r};

Expand All @@ -123,7 +123,7 @@ void CPose3DQuatPDF::jacobiansPoseComposition(

// Second part:
{
MRPT_ALIGN16 const double aux44_data[4 * 4] = {
alignas(16) const double aux44_data[4 * 4] = {
qr, -qx, -qy, -qz, qx, qr, -qz, qy,
qy, qz, qr, -qx, qz, -qy, qx, qr};

Expand Down
8 changes: 4 additions & 4 deletions libs/base/src/poses/CPose3DRotVec.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -424,7 +424,7 @@ void CPose3DRotVec::composeFrom(
const double C = 1 / (1 - qCr * qCr);
const double D = acos(qCr) / sqrt(1 - qCr * qCr);

MRPT_ALIGN16 const double aux_valsH[] = {
alignas(16) const double aux_valsH[] = {
2 * C * qCx * (D * qCr - 1), 2 * D, 0, 0,
2 * C * qCy * (D * qCr - 1), 0, 2 * D, 0,
2 * C * qCz * (D * qCr - 1), 0, 0, 2 * D};
Expand All @@ -435,7 +435,7 @@ void CPose3DRotVec::composeFrom(
const double alpha2 = alpha * alpha;
const double KA = alpha * cos(alpha / 2) - 2 * sin(alpha / 2);

MRPT_ALIGN16 const double aux_valsG[] = {
alignas(16) const double aux_valsG[] = {
-r1 * alpha2 * sin(alpha / 2),
-r2 * alpha2 * sin(alpha / 2),
-r3 * alpha2 * sin(alpha / 2),
Expand All @@ -460,7 +460,7 @@ void CPose3DRotVec::composeFrom(
const double& qAy = qA.m_quat[2];
const double& qAz = qA.m_quat[3];

MRPT_ALIGN16 const double aux_valsQA[] = {
alignas(16) const double aux_valsQA[] = {
qAr, -qAx, -qAy, -qAz, qAx, qAr, qAz, -qAy,
qAy, -qAz, qAr, qAx, qAz, qAy, -qAx, qAr};
CMatrixDouble44 QA(aux_valsQA);
Expand All @@ -481,7 +481,7 @@ void CPose3DRotVec::composeFrom(
const double& qBy = qB.m_quat[2];
const double& qBz = qB.m_quat[3];

MRPT_ALIGN16 const double aux_valsQB[] = {
alignas(16) const double aux_valsQB[] = {
qBr, -qBx, -qBy, -qBz, qBx, qBr, -qBz, qBy,
qBy, qBz, qBr, -qBx, qBz, -qBy, qBx, qBr};
CMatrixDouble44 QB(aux_valsQB);
Expand Down
4 changes: 2 additions & 2 deletions libs/base/src/poses/CPosePDFGaussian.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -216,7 +216,7 @@ void CPosePDFGaussian::rotateCov(const double ang)
const double ccos = cos(ang);
const double ssin = sin(ang);

MRPT_ALIGN16 const double rot_vals[] = {ccos, -ssin, 0., ssin, ccos,
alignas(16) const double rot_vals[] = {ccos, -ssin, 0., ssin, ccos,
0., 0., 0., 1.};

const CMatrixFixedNumeric<double, 3, 3> rot(rot_vals);
Expand Down Expand Up @@ -328,7 +328,7 @@ void CPosePDFGaussian::inverse(CPosePDF& o) const
const double ssin = ::sin(mean.phi());

// jacobian:
MRPT_ALIGN16 const double H_values[] = {
alignas(16) const double H_values[] = {
-ccos, -ssin, mean.x() * ssin - mean.y() * ccos,
ssin, -ccos, mean.x() * ccos + mean.y() * ssin,
0, 0, -1};
Expand Down
4 changes: 2 additions & 2 deletions libs/base/src/poses/CPosePDFGaussianInf.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -204,7 +204,7 @@ void CPosePDFGaussianInf::rotateCov(const double ang)
const double ccos = cos(ang);
const double ssin = sin(ang);

MRPT_ALIGN16 const double rot_vals[] = {ccos, -ssin, 0., ssin, ccos,
alignas(16) const double rot_vals[] = {ccos, -ssin, 0., ssin, ccos,
0., 0., 0., 1.};

const CMatrixFixedNumeric<double, 3, 3> rot(rot_vals);
Expand Down Expand Up @@ -325,7 +325,7 @@ void CPosePDFGaussianInf::inverse(CPosePDF& o) const
const double ssin = ::sin(mean.phi());

// jacobian:
MRPT_ALIGN16 const double H_values[] = {
alignas(16) const double H_values[] = {
-ccos, -ssin, mean.x() * ssin - mean.y() * ccos,
ssin, -ccos, mean.x() * ccos + mean.y() * ssin,
0, 0, -1};
Expand Down
4 changes: 2 additions & 2 deletions libs/base/src/poses/SE_traits.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ void SE_traits<3>::jacobian_dP1DP2inv_depsilon(
J1(2, 3) = P1DP2inv.y();
J1(2, 4) = -P1DP2inv.x();

MRPT_ALIGN16 const double aux_vals[] = {
alignas(16) const double aux_vals[] = {
0, R(2, 0), -R(1, 0), -R(2, 0), 0, R(0, 0), R(1, 0), -R(0, 0), 0,
// -----------------------
0, R(2, 1), -R(1, 1), -R(2, 1), 0, R(0, 1), R(1, 1), -R(0, 1), 0,
Expand All @@ -90,7 +90,7 @@ void SE_traits<3>::jacobian_dP1DP2inv_depsilon(
for (int j = 0; j < 3; j++)
J2.set_unsafe(i, j, -R.get_unsafe(i, j));

MRPT_ALIGN16 const double aux_vals[] = {
alignas(16) const double aux_vals[] = {
0, R(0, 2), -R(0, 1), 0, R(1, 2), -R(1, 1), 0, R(2, 2), -R(2, 1),
// -----------------------
-R(0, 2), 0, R(0, 0), -R(1, 2), 0, R(1, 0), -R(2, 2), 0, R(2, 0),
Expand Down
4 changes: 2 additions & 2 deletions libs/base/src/utils/CImage_SSE2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@
*/
void image_SSE2_scale_half_1c8u(const uint8_t* in, uint8_t* out, int w, int h)
{
MRPT_ALIGN16 const unsigned long long mask[2] = {0x00FF00FF00FF00FFull,
alignas(32) const unsigned long long mask[2] = {0x00FF00FF00FF00FFull,
0x00FF00FF00FF00FFull};
const __m128i m = _mm_load_si128((const __m128i*)mask);

Expand Down Expand Up @@ -76,7 +76,7 @@ void image_SSE2_scale_half_1c8u(const uint8_t* in, uint8_t* out, int w, int h)
void image_SSE2_scale_half_smooth_1c8u(
const uint8_t* in, uint8_t* out, int w, int h)
{
MRPT_ALIGN16 const unsigned long long mask[2] = {0x00FF00FF00FF00FFull,
alignas(16) const unsigned long long mask[2] = {0x00FF00FF00FF00FFull,
0x00FF00FF00FF00FFull};
const uint8_t* nextRow = in + w;
__m128i m = _mm_load_si128((const __m128i*)mask);
Expand Down
8 changes: 4 additions & 4 deletions libs/base/src/utils/CImage_SSE3.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,15 +39,15 @@
*/
void image_SSSE3_scale_half_3c8u(const uint8_t* in, uint8_t* out, int w, int h)
{
MRPT_ALIGN16 const unsigned long long mask0[2] = {
alignas(16) const unsigned long long mask0[2] = {
0x0D0C080706020100ull, 0x808080808080800Eull}; // Long words are in
// inverse order due to
// little endianness
MRPT_ALIGN16 const unsigned long long mask1[2] = {0x8080808080808080ull,
alignas(16) const unsigned long long mask1[2] = {0x8080808080808080ull,
0x0E0A090804030280ull};
MRPT_ALIGN16 const unsigned long long mask2[2] = {0x0C0B0A0605040080ull,
alignas(16) const unsigned long long mask2[2] = {0x0C0B0A0605040080ull,
0x8080808080808080ull};
MRPT_ALIGN16 const unsigned long long mask3[2] = {0x808080808080800Full,
alignas(16) const unsigned long long mask3[2] = {0x808080808080800Full,
0x8080808080808080ull};

const __m128i m0 = _mm_load_si128((const __m128i*)mask0);
Expand Down
4 changes: 2 additions & 2 deletions libs/graphs/include/mrpt/graphs/CDirectedGraph.h
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@ class CDirectedGraph
inline void insertEdge(
TNodeID from_nodeID, TNodeID to_nodeID, const edge_t& edge_value)
{
MRPT_ALIGN16 typename edges_map_t::value_type entry(
alignas(16) typename edges_map_t::value_type entry(
std::make_pair(from_nodeID, to_nodeID), edge_value);
edges.insert(entry);
}
Expand All @@ -134,7 +134,7 @@ class CDirectedGraph
inline void insertEdgeAtEnd(
TNodeID from_nodeID, TNodeID to_nodeID, const edge_t& edge_value)
{
MRPT_ALIGN16 typename edges_map_t::value_type entry(
alignas(16) typename edges_map_t::value_type entry(
std::make_pair(from_nodeID, to_nodeID), edge_value);
edges.insert(edges.end(), entry);
}
Expand Down
2 changes: 1 addition & 1 deletion libs/graphslam/include/mrpt/graphslam/levmarq_impl.h
Original file line number Diff line number Diff line change
Expand Up @@ -232,7 +232,7 @@ double computeJacobiansAndErrors(
P1DP2inv, errs.back(), edge);

// Compute the jacobians:
MRPT_ALIGN16
alignas(16)
std::pair<mrpt::utils::TPairNodeIDs, typename gst::TPairJacobs>
newMapEntry;
newMapEntry.first = ids;
Expand Down
Loading

0 comments on commit c35ffb9

Please sign in to comment.