Creating a point cloud with a TF-Luna LiDAR

Keywords

Raspberry Pi, TF-Luna, LiDAR, Pan-Tilt, FreeCAD, Rust, Rerun

Note

The full code for this project/experiment can be found in the following repository: rpi-tfluna-lidar

Introduction

Figure 1: Picture of the setup. A TF-Luna mounted on the pan-tilt mechanism and connected to a Raspberry PI

In my previous post about the TF-Luna LiDAR distance sensor, I showed what it is and how to use it with a Raspberry Pi.

After publishing it, I took some time to develop and publish the embedded-tfluna crate specifically for it. It is platform agnostic and works in no-std environments. However, it only supports I2C communication for now because that’s what I used.

I still wanted to play around a bit more with the sensor, so I created, designed and printed a pan-tilt mechanism that uses 2 servo motors in order to rotate the sensor around and capture the distance of different points in space in order to create a point cloud (Figure 1).

Before diving into the details, I would like to first describe what a point cloud is and why I would need one.

A point cloud is a discrete set of data points in space. The points may represent a 3D shape or object. Each point position has its set of Cartesian coordinates (\(x\), \(y\), \(z\)). Points may contain data other than position such as RGB colors, normals, timestamps and others. Point clouds are generally produced by 3D scanners or by photogrammetry software, which measure many points on the external surfaces of objects around them.

They are used for many purposes; the one I care about personally is autonomous vehicles. I want to make a mobile robot that navigates autonomously around rooms with the use of a LiDAR sensor and SLAM (Simultaneous Localization And Mapping).

Pan-Tilt Mechanism

Getting to the final pan-til mechanism was not an easy task. I tried a few different solutions before comming up with the final one.

I first started by ordering a cheap pan-tilt mechanism from AliExpress (Figure 2) hoping that it would be enough given that the sensor itself is very light. Unfortunately, the two micro servos that came with it were either broken or defect and wouldn’t hold their position. They instead kept rotating continuously. On top of that, the pan-tilt mechanism was pretty flimsy.

Figure 2: Example cheap AliExpress Pan-Tilt Mechanism with servo motors

So I gave up on that and decided to look for a better mechanism with better servo motors. I ordered a second that I found on a website whose name I forgot. That one unfortunately came with a continuous rotation servo which I couldn’t use.

In the end, I decided to build my own mechanism and used 2 servo motors that I had ordered for another project.

It took so long to finalize this project that I learned to use FreeCAD, bought a 3D printer (a Centauri Carbon, for those that are curious) and ended up modeling the pan-tilt mechanism myself (Figure 3) (I actually reused an assembly that I created for another project of mine FreeCAD-Assembly2MuJoCo about which I will write a blog post hopefully after this one).

Figure 3: TFLuna Pan Tilt Mechanism in FreeCAD

I printed it with Elegoo’s Galaxy Blue PLA and it came out looking great (Figure 4).

Figure 4: Picture of the TF-Luna mounted on the pan-tilt mechanism

Once that was done, I connected the 2 servos to a PCA9685 module in order to control them using I2C just like for the TF-Luna sensor.

Figure 5: Wiring diagram of TFLuna, servo motors and Raspberry Pi made with Cirkit Designer

Just like for my previous post, I wrote the code in Rust, and used Rerun to store the data. I cross-compiled it on my laptop and copied and ran it on a Raspberry Pi using SSH with a Rerun server running on my laptop.

Point Cloud Creation

To create a point cloud, we have to rotate the sensor around, take distance measurements and convert each measurement into Cartesian coordinates.

In this mechanism, we control the angles of both servo motors (yaw (\(\theta\)) for the bottom one and pitch (\(\phi\)) for the top one) and can measure distance (\(d\)) to a specific point in space.

Figure 6: Diagram representing the angles, distance measurement and point position

In order to get a point’s position from these parameters and measurements, we use the following formulas:

\[ \left[ \begin{matrix}x \\ y \\ z \end{matrix}\right] = \left[ \begin{matrix} d * \cos(\phi) * \sin(\theta) \\ d * \cos(\phi) * \cos(\theta) \\ d * \sin(\phi) \end{matrix}\right] \]

Where we consider the center of the coordinate system \((0, 0, 0)\) to coincide with the position of the sensor.

Experiment

Experimental Setup

Before using the mechanism to create point clouds, I wanted to see how fast and accurate it can be. For that, I put the mechanism on a table and pointed it at a wall at a distance of 20 centimeters and limited the yaw angle (left and right movement) to the range \([-30^\circ, 30^\circ]\) inclusive and the pitch angle (up and down movement) to the range \([0^\circ, 30^\circ]\) inclusive. The pitch angle only goes down to \(0^\circ\) because otherwise it would measure the distance to the table on which it was put.

Since I know that the wall is flat and that it is at a distance of 20 centimeters from the sensor, we can represent it as a plane in 3D space with the equation:

\[ y = 20 \]

I am assuming that the y-axis runs from the mechanism to the wall, the z-axis runs from the mechanism to the ceiling and finally the x-axis runs from the mechanism to the right when facing the wall.

Plane Detection

The standard general form of a plane in 3D is given by the equation: \[ ax + by + cz + d = 0 \]

Where \(a, b, c, d \in \mathbb{R}\) are the coefficients that define the plane and \(x, y, z \in \mathbb{R}\) are the coordinates of points in 3D space.

To avoid the degenerate solution \(a = 0, b = 0, c = 0, d = 0\) and since we know that \(d \neq 0\), we divide everything by \(d\) to get new coefficients:

\[ a^\prime = \frac{a}{d}, b^\prime = \frac{b}{d}, c^\prime = \frac{c}{d} \]

The equation then becomes:

\[ a^\prime x + b^\prime y + c^\prime z + 1 = 0 \]

If we take multiple measurements of different points, we obtain a system of equations that can be represented as:

\[ A\Theta=b \]

Where \(A = \left[ \begin{matrix} x_1 & y_1 & z_1\\ x_2 & y_2 & z_2\\ & \dots\\ x_n & y_n & z_n \end{matrix} \right]\), \(b = \left[ \begin{matrix} -1\\ -1\\ \dots\\ -1 \end{matrix} \right]\), \(\Theta = \left[ \begin{matrix} a^\prime\\ b^\prime\\ c^\prime \end{matrix} \right]\)

To solve this system of equations we use SVD (Singular Value Decomposition). We first decompose the matrix \(A\) as:

\[ A = U \Sigma V^T \]

Where:

• \(U\) is an \(m \times n\) orthogonal matrix.
• \(\Sigma\) is an \(n \times n\) diagonal matrix.
• \(V\) is an \(n \times n\) orthogonal matrix.

The solution is then given:

\[ \Theta = V \Sigma^{+} U^T b \]

Where \(\Sigma^{+}\) represents the pseudo-inverse (Moore-Penrose inverse).

Optimization and Results

There are two main parameters that determine the speed of measurements:

• angle step size: The step size in degrees of the rotation of each servo motor.
• servo motor delay: The delay in milliseconds to wait after issuing a servo motor movement command before taking a measurement.

I tried different combinations of these parameters, collected the data, computed the parameters and a few metrics and plotted the results:

metrics_data = FileAttachment("metrics.csv").csv({ typed: true })

unique_angle_step = [...new Set(metrics_data.map(d => d.angle_step))];
unique_servo_delay = [...new Set(metrics_data.map(d => d.servo_motor_delay))];

filtered_metrics_data = metrics_data.filter(function(row) {
    return (
        servo_delays.includes(row.servo_motor_delay)
        && angle_steps.includes(row.angle_step)
    );
})

viewof angle_steps = Inputs.checkbox(
    unique_angle_step,
    {
        value: unique_angle_step, 
        label: "Angle Step:"
    }
)
viewof servo_delays = Inputs.checkbox(
    unique_servo_delay,
    {
        value: unique_servo_delay, 
        label: "Servo Motor Delay:"
    }
)

Plot

import { Plot } from "@observablehq/plot";

{
    const plot = Plot.plot({
        width: 800,
        height: 600,
        marginLeft: 60,
        marginBottom: 50,
        style: {
            background: "transparent",
        },
        x: {
            label: "Mean Y-Intercept Error (mm)",
            grid: true,
            type: "log",
        },
        y: {
            label: "Mean Angle Error (deg)",
            grid: true,
            type: "log",
        },
        color: {
            type: "linear",
            scheme: "Cool",
            legend: true,
            label: "Total Time (s)",
        },
        marks: [
            Plot.dot(filtered_metrics_data, {
                x: "y_intercept_error",
                y: "angle_error_deg",
                fill: "total_time_s",
                r: 6,
                title: (d) =>
                    `Angle: ${d.angle_step.toFixed(2)}deg\nServo: ${d.servo_motor_delay.toFixed(2)}ms\nTotal Time: ${d.total_time_s.toFixed(2)}s`,
            }),
        ],
    });

    // Fix the white background on the legend ramp
    const ramp = plot.querySelector('svg[class*="-ramp"]');
    if (ramp) {
        const style = document.createElement("style");
        style.textContent = `
            svg[class*="-ramp"] {
                background: transparent !important;
            }
        `;
        plot.appendChild(style);
    }

    return plot;
}

Data

Inputs.table(filtered_metrics_data)

As we can see from the plot, the best compromise between speed and accuracy comes from the two combinations of parameters:

• angle step size: 1 degree, servo motor delay: 50 milliseconds.
• angle step size: 5 degrees, servo motor delay: 50 milliseconds.

Example Point Cloud

After the experiment, I pointed the mechanism at something more interesting—specifically a bookshelf—and generated a point cloud. To ensure high detail, I maintained the settings from the best test case: an angle step size of 1 degree and a servo motor delay of 50 ms.

d3 = require("d3@7")

point_cloud_data2 = FileAttachment("point_cloud_data_1deg_50ms.csv").csv({ typed: true })

Plot

{
  // Make canvas responsive
  const container = this ?? document.body;
  const width = Math.min(800, container.clientWidth || 800);
  const height = Math.min(680, width * 0.85);

  // Create wrapper div for canvas and button
  const wrapper = document.createElement("div");
  wrapper.style.position = "relative";
  wrapper.style.width = "100%";
  
  // Create canvas
  const canvas = document.createElement("canvas");
  canvas.width = width;
  canvas.height = height;
  canvas.style.width = "100%";
  canvas.style.height = "auto";
  canvas.style.cursor = "grab";
  canvas.style.display = "block";
  const ctx = canvas.getContext("2d");

  // Get Quarto CSS variables
  const getComputedCSSStyle = (varName) => {
    return getComputedStyle(document.documentElement).getPropertyValue(varName).trim();
  };

  // Get all CSS variables
  const getStyle = () => {
    return {
      canvasFillColor: getComputedCSSStyle('--quarto-body-bg'),
      bgColor: '#ffffff01',
      textColor: getComputedCSSStyle('--quarto-body-color'),
      mutedColor: getComputedCSSStyle('--quarto-text-muted'),
      borderColor: getComputedCSSStyle('--quarto-border-color'),
      fontFamily: getComputedCSSStyle('--bs-body-font-family'),
      fontSize: getComputedCSSStyle('--bs-body-font-size'),
    }
  }
  const style = getStyle();

  // Default starting angles
  const defaultAngleX = 2.0;
  const defaultAngleY = 0.03;
  const defaultAngleZ = 0.0;

  // Create reset button
  const resetBtn = document.createElement("button");
  resetBtn.textContent = "Reset View";
  resetBtn.style.position = "absolute";
  resetBtn.style.bottom = "30px";
  resetBtn.style.left = "8px";
  resetBtn.style.padding = "6px 12px";
  resetBtn.style.fontSize = "12px";
  resetBtn.style.cursor = "pointer";
  resetBtn.style.backgroundColor = style.bgColor;
  resetBtn.style.color = style.textColor;
  resetBtn.style.border = `1px solid ${style.borderColor}`;
  resetBtn.style.borderRadius = "4px";
  resetBtn.style.zIndex = "10";
  
  // Button hover effect
  resetBtn.addEventListener("mouseenter", () => {
    const style = getStyle();
    resetBtn.style.backgroundColor = style.borderColor;
  });
  resetBtn.addEventListener("mouseleave", () => {
    const style = getStyle();
    resetBtn.style.backgroundColor = style.bgColor;
  });
  
  // Reset functionality
  resetBtn.addEventListener("click", () => {
    angleX = defaultAngleX;
    angleY = defaultAngleY;
    angleZ = defaultAngleZ;
    render();
  });

  // 3D rotation state
  const zoom = 2.0;
  let angleX = 2.0;
  let angleY = 0.03;
  let angleZ = 0.0;
  let isDragging = false;

  // Scales - adjust ranges based on canvas size
  const scale = Math.min(width, height) / 3;
  const xScale = d3.scaleLinear()
    .domain(d3.extent(point_cloud_data2, d => d.x))
    .range([-scale * 0.8, scale * 0.8]);
  const yScale = d3.scaleLinear()
    .domain(d3.extent(point_cloud_data2, d => d.y))
    .range([-scale * 0.7, scale * 0.7]);
  const zScale = d3.scaleLinear()
    .domain(d3.extent(point_cloud_data2, d => d.z))
    .range([-scale * 0.7, scale * 0.7]);

  // Color scale
  const colorScale = d3.scaleSequential(d3.interpolateCool)
    .domain(d3.extent(point_cloud_data2, d => d.distance));

  // 3D projection with perspective
  const project = (x, y, z) => {
    // Apply zoom
    const zx = x * zoom;
    const zy = y * zoom;
    const zz = z * zoom;

    // Rotate around Z axis
    const cosZ = Math.cos(angleZ);
    const sinZ = Math.sin(angleZ);
    const x1 = zx * cosZ + zy * sinZ;
    const y1 = - zx * sinZ + zy * cosZ;
    const z1 = zz;

    // Rotate around Y axis
    const cosY = Math.cos(angleY);
    const sinY = Math.sin(angleY);
    const x2 = x1 * cosY - z1 * sinY;
    const y2 = y1;
    const z2 = x1 * sinY + z1 * cosY;
    
    // Rotate around X axis
    const cosX = Math.cos(angleX);
    const sinX = Math.sin(angleX);
    const x3 = x2;
    const y3 = y2 * cosX - z2 * sinX;
    const z3 = y2 * sinX + z2 * cosX;
    
    // Perspective
    const perspective = 1000;
    const scale = perspective / (perspective + z3);
    
    return {
      x: x3 * scale + width / 2,
      y: y3 * scale + height / 2,
      z: z3,
      scale: scale
    };
  };

  // Draw color legend
  const drawLegend = () => {
    const style = getStyle();
    const legendWidth = 20;
    const legendHeight = 200;
    const legendX = width - 60;
    const legendY = 50;
    
    // Draw gradient
    const gradient = ctx.createLinearGradient(0, legendY, 0, legendY + legendHeight);
    const steps = 10;
    for (let i = 0; i <= steps; i++) {
      const t = i / steps;
      const value = d3.min(point_cloud_data2, d => d.distance) + 
                   t * (d3.max(point_cloud_data2, d => d.distance) - d3.min(point_cloud_data2, d => d.distance));
      gradient.addColorStop(1 - t, colorScale(value));
    }
    
    ctx.fillStyle = gradient;
    ctx.fillRect(legendX, legendY, legendWidth, legendHeight);
    
    // Draw border using Quarto border color
    ctx.strokeStyle = style.borderColor;
    ctx.lineWidth = 1;
    ctx.strokeRect(legendX, legendY, legendWidth, legendHeight);
    
    // Draw labels using Quarto muted color
    ctx.fillStyle = style.mutedColor;
    ctx.font = `${style.fontSize} ${style.fontFamily}`;
    ctx.textAlign = "left";
    
    const maxVal = d3.max(point_cloud_data2, d => d.distance);
    const minVal = d3.min(point_cloud_data2, d => d.distance);
    
    ctx.fillText(maxVal.toFixed(0), legendX + legendWidth + 5, legendY + 5);
    ctx.fillText(minVal.toFixed(0), legendX + legendWidth + 5, legendY + legendHeight + 5);
    
    // Title in regular text color
    ctx.fillStyle = style.textColor;
    ctx.fillText("Distance", legendX - 5, legendY - 20);
  };

  const render = () => {
    const style = getStyle();
    // Clear canvas
    ctx.fillStyle = style.canvasFillColor;
    ctx.fillRect(0, 0, width, height);
    
    // Project and sort by depth
    const projected = point_cloud_data2.map(d => {
      const proj = project(xScale(d.x), yScale(d.y), zScale(d.z));
      return {
        ...d,
        ...proj
      };
    }).sort((a, b) => a.z - b.z); // Draw far to near
    
    // Draw points
    projected.forEach(d => {
      const radius = 2.5 * d.scale; // Size based on depth
      ctx.beginPath();
      ctx.arc(d.x, d.y, radius, 0, 2 * Math.PI);
      ctx.fillStyle = colorScale(d.distance);
      ctx.fill();
      ctx.strokeStyle = style.mutedColor;
      ctx.lineWidth = 0.5;
      ctx.stroke();
    });

    // Draw legend
    drawLegend();
    
    // Draw axes labels
    ctx.fillStyle = style.mutedColor;
    ctx.font = `${style.fontSize} ${style.fontFamily}`;
    ctx.textAlign = "left";
    ctx.fillText("Drag to rotate", 10, 20);
  };

  // Update button colors when theme changes
  const updateButtonColors = () => {
    const style = getStyle();
    resetBtn.style.backgroundColor = style.bgColor;
    resetBtn.style.color = style.textColor;
    resetBtn.style.border = `1px solid ${style.borderColor}`;
  };
  
  render();

  // Mouse interaction
  let lastX, lastY;

  // Helper to get coordinates
  const getEventCoordinates = (event) => {
    if (event.touches && event.touches.length > 0) {
        return { x: event.touches[0].clientX, y: event.touches[0].clientY };
    }
    return { x: event.clientX, y: event.clientY };
  }

  /// Event Handlers

  const handle_start = (event) => {
    // Prevent default browser behaviors (like zooming or scrolling)
    event.preventDefault();

    isDragging = true;
    const { x, y } = getEventCoordinates(event);
    lastX = x;
    lastY = y;
    canvas.style.cursor = "grabbing";
  };

  const handle_move = (event) => {
    if (!isDragging) return;
    // Prevent default browser behaviors (like zooming or scrolling)
    event.preventDefault();

    const { x, y } = getEventCoordinates(event);
    const dx = x - lastX;
    const dy = y - lastY;
    
    angleZ += dx * 0.005;
    angleX += dy * 0.005;
  
    lastX = x;
    lastY = y;
    render();
  };
  
  const handle_end = (event) => {
    isDragging = false;
    canvas.style.cursor = "grab";
  };

  // Attach event listeners
  // Mouse
  canvas.addEventListener("mousedown", handle_start);
  canvas.addEventListener("mousemove", handle_move);
  canvas.addEventListener("mouseup", handle_end);
  canvas.addEventListener("mouseleave", handle_end);
  // Touch
  canvas.addEventListener("touchstart", handle_start, { passive: false });
  canvas.addEventListener("touchmove", handle_move, { passive: false });
  canvas.addEventListener("touchup", handle_end);
  canvas.addEventListener("touchcancel", handle_end);
  
  // Watch for theme changes
  const observer = new MutationObserver(() => {
    updateButtonColors();
    render();
  });
  observer.observe(document.body, { 
    attributes: true, 
    attributeFilter: ['class'] 
  });

  // Assemble wrapper
  wrapper.appendChild(canvas);
  wrapper.appendChild(resetBtn);
  
  return wrapper;
}

Data

Inputs.table(point_cloud_data2)

The resulting cloud successfully reconstructs the structure of the bookshelf and highlights how the mechanism captures some of the fine details. While it is slow, it has a sufficient resolution to distinguish between the bookshelf, its shelves and the wall.

Conclusion

It is clear that the pan-tilt TF-Luna assembly has its limits when it comes to speed, which means it likely won’t be suitable for high-speed autonomous navigation. However, building this mechanism and creating the point clouds was a great learning experience for me in many aspects.

I may try again to push the mechanism to its limits, perhaps by limiting the rotation to the z-axis to make it faster.

For now, that concludes my exploration of the TF-Luna sensor. I will go back to some of the projects that I started or pick up a new one from my backlog.