mapping.cpp

Go to the documentation of this file.

#include "toposens_pointcloud/mapping.h"

namespace toposens_pointcloud
{
void Mapping::doTransform(pcl::PointXYZINormal &pt, Vector3f translation, Vector4f quaternion)
{
  Vector4f point_in, point_out;
  point_in << pt.x, pt.y, pt.z,
      1;                  // Assign homogenous co ordinates to the fourth column
  double qx, qy, qz, qw;  // Quaternion x,y,z,w parameters
  qx = quaternion[0];
  qy = quaternion[1];
  qz = quaternion[2];
  qw = quaternion[3];
  MatrixXf transform_mat(4, 4);

  // Assign homogenous coefficients to the 4th column
  // https://www.scratchapixel.com/lessons/mathematics-physics-for-computer-graphics/geometry/transforming-points-and-vectors
  transform_mat << 1 - 2 * qz * qz - 2 * qy * qy, -2 * qz * qw + 2 * qy * qx,
      2 * qy * qw + 2 * qz * qx, translation[0], 2 * qx * qy + 2 * qw * qz,
      1 - 2 * qz * qz - 2 * qx * qx, 2 * qz * qy - 2 * qx * qw, translation[1],
      2 * qx * qz - 2 * qw * qy, 2 * qy * qz + 2 * qw * qx, 1 - 2 * qy * qy - 2 * qx * qx,
      translation[2], 0.0, 0.0, 0.0, 1;
  point_out = transform_mat * point_in;

  // Update the input PCL point
  pt.x = point_out[0];
  pt.y = point_out[1];
  pt.z = point_out[2];

  // https://www.scratchapixel.com/lessons/mathematics-physics-for-computer-graphics/geometry/transforming-normals
  Vector4f normal_in, normal_out;
  normal_in << pt.normal_x, pt.normal_y, pt.normal_z, 1;
  normal_out = (transform_mat.inverse().transpose()) * normal_in;

  // Update the input PCL point Normals
  pt.normal_x = normal_out[0];
  pt.normal_y = normal_out[1];
  pt.normal_z = normal_out[2];
}

pcl::PointXYZINormal Mapping::convertToXYZINormal(toposens_msgs::TsPoint i)
{
  // Fast calculation of inverse of square roots
  // https://en.wikipedia.org/wiki/Fast_inverse_square_root
  auto isqrtf = [](float number) {
    long i;
    float x2, y;
    const float threehalfs = 1.5F;

    x2 = number * 0.5F;
    y = number;
    i = *(long *)&y;
    i = 0x5f3759df - (i >> 1);
    y = *(float *)&i;
    y = y * (threehalfs - (x2 * y * y));
    return y;
  };

  pcl::PointXYZINormal o;

  o.x = i.location.x;
  o.y = i.location.y;
  o.z = i.location.z;
  o.intensity = i.intensity;

  // Approximation of the inverse of the length of the normal vector
  float len = isqrtf(i.location.x * i.location.x + i.location.y * i.location.y +
                     i.location.z * i.location.z);
  // Normalize and store the vector in the PCL point
  o.normal_x = -i.location.x * len;
  o.normal_y = -i.location.y * len;
  o.normal_z = -i.location.z * len;

  return o;
}

}  // namespace toposens_pointcloud