MultiSense SL Manual : ROS Driver

// Create two TF transforms for the Camera to Spindle calibration and the Laser
// to Spindle calibration

void rawLaserCalCallback(const multisense_ros::RawLidarCal::ConstPtr& msg)
{
    //Get Transform Matrices from flattened 16 element Homogenous arrays in
    // row-major order

    _laserToSpindle = makeTransform(msg->laserToSpindle);
    _motorToCamera  = makeTransform(msg->cameraToSpindleFixed);
}

template <typename T> tf::Transform makeTransform(T data)
{
    // Manually create the rotation matrix
    tf::Matrix3x3 rot = tf::Matrix3x3(data[0],
                                      data[1],
                                      data[2],
                                      data[4],
                                      data[5],
                                      data[6],
                                      data[8],
                                      data[9],
                                      data[10]);

    // Maually create the translation vector
    tf::Vector3 trans = tf::Vector3(data[3], data[7], data[11]);

    return tf::Transform(rot, trans);
}

// Transforms Laser Data using onboard calibration
void rawLaserDataCallback(const multisense_ros::RawLidarData::ConstPtr& msg)
{
    // For Visualization
    makePointCloudHeader(msg->distance.size());
    _point_cloud.header.stamp   = msg->time_start;
    uint8_t         *cloudP     = reinterpret_cast<uint8_t*>(&_point_cloud.data[0]);
    const uint32_t  pointSize   = 3 * sizeof(float);

    // Scan angle range of the laser
    const double fullScanAngle = 270 * M_PI / 180.;

    // Laser start at -135 degrees spinning counterclockwise
    const double startScanAngle = -fullScanAngle / 2.;

    // Get the spindle angles for this scan. angle_start and angle_end are in micro-radians
    // Use ros package angles to normalize them to -PI to +PI
    const double spindleAngleStart = angles::normalize_angle(1e-6 * static_cast<double>(msg->angle_start));
    const double spindleAngleEnd   = angles::normalize_angle(1e-6 * static_cast<double>(msg->angle_end));
    const double spindleAngleRange = angles::normalize_angle(spindleAngleEnd - spindleAngleStart);

    // Loop over all the range data
    for ( unsigned int i = 0; i < msg->distance.size(); i++, cloudP += _point_cloud.point_step){

        // Percent through the scan. Used for linear interpolation
        const double percent = static_cast<double>(i) / static_cast<double>(msg->distance.size() -1);

        // Linearly interpolate the laser scan angle from -135 to 135
        const double mirrorAngle = startScanAngle + (percent * fullScanAngle);

        // Linearly interpolate the spindle angle
        const double spindleAngle = spindleAngleStart + (percent * spindleAngleRange);

        // Generate the Homogeneous Transform for the Motor to Spindle Transform
        const double cosSpindle = std::cos(spindleAngle);
        const double sinSpindle = std::sin(spindleAngle);
        _spindleToMotor = tf::Transform(tf::Matrix3x3(cosSpindle, -sinSpindle, 0,
                                                      sinSpindle,  cosSpindle, 0,
                                                      0         ,           0, 1),
                                                      tf::Vector3(0, 0, 0)      );

        // Convert range point to meters and project into the X-Z laser plane
        const double        rangeMeters = static_cast<double>(1e-3 * msg->distance[i]);
        const tf::Vector3   rangePoint  = tf::Vector3(rangeMeters * std::sin(mirrorAngle),
                                                     0,
                                                     rangeMeters * std::cos(mirrorAngle));

        // Transform point into the left camera frame
        const tf::Vector3 pointInCamera = _motorToCamera
                                         * (_spindleToMotor
                                         * (_laserToSpindle * rangePoint));

        // Copy data to point cloud structure
        const float xyz[3] = {static_cast<float>(pointInCamera.getX()),
                              static_cast<float>(pointInCamera.getY()),
                              static_cast<float>(pointInCamera.getZ())};

        memcpy(cloudP, &(xyz[0]), pointSize);
        memcpy((cloudP + pointSize), &(msg->intensity[i]), sizeof(uint32_t));
    }
    _point_pub.publish(_point_cloud);
}

void makePointCloudHeader(const unsigned int pointCount)
{
    const uint32_t cloud_step = 16;
    _point_cloud.data.resize(cloud_step * pointCount);
    _point_cloud.is_bigendian     = (htonl(1) == 1);
    _point_cloud.is_dense         = true;
    _point_cloud.point_step       = cloud_step;
    _point_cloud.height           = 1;
    _point_cloud.header.frame_id  = "/multisense/left_camera_optical_frame";

    _point_cloud.fields.resize(4);
    _point_cloud.fields[0].name     = "x";
    _point_cloud.fields[0].offset   = 0;
    _point_cloud.fields[0].count    = 1;
    _point_cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32;

    _point_cloud.fields[1].name     = "y";
    _point_cloud.fields[1].offset   = 4;
    _point_cloud.fields[1].count    = 1;
    _point_cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32;

    _point_cloud.fields[2].name     = "z";
    _point_cloud.fields[2].offset   = 8;
    _point_cloud.fields[2].count    = 1;
    _point_cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32;

    _point_cloud.fields[3].name     = "intensity";
    _point_cloud.fields[3].offset   = 12;
    _point_cloud.fields[3].count    = 1;
    _point_cloud.fields[3].datatype = sensor_msgs::PointField::UINT32;

    _point_cloud.row_step           = pointCount;
    _point_cloud.width              = pointCount;
}