Over the past decade, humanoid robots have evolved from laboratory prototypes to versatile platforms expected to operate in indoor service, industrial collaboration, and outdoor disaster response scenarios. Unlike wheeled robots, a humanoid robot must maintain dynamic balance, interact with human-centric environments, and perceive complex unstructured surroundings. The core enabling technology for such autonomy is Simultaneous Localization and Mapping (SLAM), which provides the robot with a continuous estimate of its own pose while constructing a useful model of the environment. However, the unique morphological and kinematic characteristics of a humanoid robot introduce severe challenges to conventional SLAM systems. The multi-joint structure causes frequent vibrations, sensor extrinsic parameters drift over time, and the operating environments are often unknown, dynamic, and sensor-degraded. Furthermore, classic sparse or semi-dense maps do not carry sufficient semantic or photometric information for higher-level tasks such as navigation, manipulation, and human-robot interaction.
This work addresses these challenges by presenting a complete pipeline for humanoid robot autonomous localization and dense mapping. First, we propose a targetless LiDAR-camera calibration method that leverages both geometric and intensity edge features extracted from the environment, enabling robust online recalibration of drifting extrinsics. Second, we develop a multi-sensor SLAM system based on 3D Gaussian Splatting (3DGS) that fuses non-repetitive LiDAR, a monocular camera, and an inertial measurement unit (IMU). The system uses an iterated error-state Kalman filter (IESKF) for LiDAR-inertial odometry, followed by a gradient-based visual refinement stage that aligns rendered depth and color with the sensor observations. Third, we design and build a small humanoid robot platform with 18 degrees of freedom, perform kinematic modeling and gait planning, and deploy the proposed calibration and SLAM methods in real-world experiments. Our results demonstrate that the proposed approach delivers accurate localization, photorealistic dense mapping, and robustness under sensor degradation, making it highly suitable for humanoid robot applications.
1. Introduction
The humanoid robot is regarded as one of the most promising embodiments of general-purpose intelligent robots. Its anthropomorphic structure allows it to operate in environments designed for humans, such as staircases, narrow corridors, and workspaces with human-centered tools. However, the same structure brings high-dimensional actuation and complex dynamics that complicate state estimation. In contrast to wheeled platforms, the humanoid robot undergoes frequent acceleration and deceleration of its limbs, causing sensor motion blur and vibration-induced calibration drift. Moreover, the deployment of humanoid robots in unknown indoor-outdoor environments often implies large dynamic ranges, changing illumination, and sparse geometric features. Therefore, a robust and accurate SLAM system is indispensable for any truly autonomous humanoid robot.
The history of SLAM dates back to the seminal work of Smith and Cheeseman in 1986. Since then, numerous visual SLAM systems, LiDAR SLAM systems, and multi-sensor fusion systems have been developed. Visual SLAM, e.g., ORB-SLAM, can provide rich texture information but suffers from illumination changes and texture-poor scenes. LiDAR SLAM, e.g., LOAM and FAST-LIO2, delivers accurate geometric measurements but cannot capture color or texture information and may fail in geometrically degenerate environments. To overcome the limitations of single-sensor approaches, researchers have proposed tightly-coupled LiDAR-visual-inertial SLAM systems such as V-LOAM, LVI-SAM, and FAST-LIVO. These systems achieve high accuracy in many scenarios, yet their map representations remain sparse or feature-based, which is insufficient for detailed perception and downstream tasks.
The recent emergence of neural radiance fields and 3D Gaussian Splatting has opened new possibilities for dense SLAM. NeRF-based SLAM methods like iMAP and NICE-SLAM produce high-quality dense reconstructions but require heavy computational resources, limiting real-time applicability. In contrast, 3DGS provides an explicit, differentiable, and real-time renderable representation that is particularly attractive for SLAM. Methods such as SplaTAM, MonoGS, and Photo-SLAM have demonstrated the potential of 3DGS in RGB-D and monocular settings. However, most existing 3DGS-based SLAM systems rely on RGB-D cameras or pure visual odometry, which are less reliable in large-scale outdoor environments. A humanoid robot operating in the real world needs a system that combines the large-range depth sensing of LiDAR with the rich appearance information of cameras while maintaining high efficiency and robustness.
In this work, we present a complete solution tailored for humanoid robot SLAM. The main contributions are threefold. First, we propose a targetless LiDAR-camera extrinsic calibration method that extracts both geometric depth-continuous edges and intensity edges from point clouds, combines them with image edges, and performs robust optimization without any artificial target. This method can be used for initial calibration and online recalibration, which is essential for long-term humanoid robot operation. Second, we develop a LiDAR-visual-inertial SLAM system built on 3DGS. The system uses an IESKF-based LiDAR-inertial odometry to provide reliable initial poses and dense depth maps, then refines the pose by minimizing gradient losses between the rendered depth/RGB and the real sensor data. The Gaussian map is updated continuously with adaptive weighting and keyframe selection. Third, we design a small custom humanoid robot with a modular structure, implement its kinematic model and gait generation, equip it with a solid-state LiDAR, an RGB-D camera, and an IMU, and demonstrate the proposed calibration and SLAM methods in practical experiments. Our experiments show that the proposed system outperforms state-of-the-art NeRF-based and 3DGS-based SLAM methods in terms of tracking accuracy, rendering quality, and robustness under sensor degradation.
2. Targetless LiDAR-Camera Calibration with Intensity Features
2.1 Problem Statement
Accurate extrinsic calibration between a LiDAR and a camera is a prerequisite for successful multi-sensor fusion in a humanoid robot. The extrinsic parameter is a rigid transformation matrix $T_L^C \in SE(3)$ that maps a point from the LiDAR coordinate system to the camera coordinate system. It consists of a rotation matrix $R_L^C \in SO(3)$ and a translation vector $t_L^C \in \mathbb{R}^3$. For a 3D point $p_l$ observed by the LiDAR, its corresponding camera-frame point is $p_c = T_L^C p_l$. The calibration problem is to find $T_L^C$ such that the projected LiDAR features align with the corresponding image features. In a targetless setting, the only available correspondences are the natural edges and corners in the environment. The humanoid robot often operates in environments where artificial calibration targets are unavailable or impractical; hence a robust targetless calibration method is needed. In addition, due to the long-term vibration and frequent impacts of a humanoid robot, the extrinsic parameters may drift, making it necessary to recalibrate online. Our method addresses this by using both geometric and intensity information from the point cloud.
2.2 Feature Extraction
We first obtain a dense point cloud by accumulating several consecutive LiDAR scans. For non-repetitive scanning solid-state LiDARs, simple accumulation is sufficient. For mechanical LiDARs, we use a CT-ICP-based dynamic point aggregation method to align multiple frames. This dense point cloud is then processed to extract two complementary categories of edge features: geometric edges and intensity edges.
2.2.1 Image Edge Extraction
The camera image is processed with the Canny edge detector to obtain a binary edge map. The threshold parameters are chosen to produce rich yet reliable edges. The edge pixels are used for later matching. Since the same thresholds are also applied to the synthesized intensity image, strong pixel-level correspondences are established.
2.2.2 Geometric Edge Extraction from LiDAR
Existing methods often extract depth-discontinuous edges, but these are prone to foreground expansion and bleeding artifacts. We therefore extract depth-continuous edges using voxel-based segmentation. The point cloud is divided into voxels, and each voxel is processed with RANSAC to fit planar surfaces. Two planes within a voxel form a depth-continuous edge if the angle between their normals exceeds a threshold. This yields clean and reliable edges that correspond to the edges of objects or walls in the environment. Each detected edge is represented by a set of points sampled along the intersection line.
2.2.3 Intensity Edge Extraction
In addition to geometric edges, the LiDAR point cloud contains intensity information, which reflects the reflectance of the surface material. Intensity edges are especially valuable in texture-rich environments where geometric edges are sparse. However, computing intensity edges on a full dense point cloud often produces unreliable features, especially near depth discontinuities. We propose a single-plane intensity edge extraction strategy. First, the dense point cloud is voxelized and planar surfaces are extracted using RANSAC. Only planes with sufficient point count and area are retained. For each plane, we generate a virtual intensity image using a virtual camera model. The 3D points on the plane are projected onto the virtual image plane using the current extrinsic estimate:
$$ P_i^I = f(\pi(T_L^C P_i^L)), \tag{1} $$
where $P_i^L$ is a LiDAR point, $T_L^C$ is the current extrinsic matrix, $\pi$ is the pinhole projection, $f$ is the distortion model, and $P_i^I = (x_i, y_i)$ is the pixel coordinate. We assign the intensity value of each point to its corresponding pixel, producing a grayscale intensity image. Then, the Canny edge detector is applied to this intensity image. However, the boundaries of the plane also produce edges that correspond to geometric depth discontinuities. We remove these boundary edges using a contour removal algorithm. To further enhance the quality of the intensity image, we apply a neighborhood-based outlier filter to remove spurious pixels caused by sparse point coverage. Then, a Gaussian smoothing kernel is applied:
$$ I'(x,y) = \sum_{i=-k}^{k} \sum_{j=-k}^{k} G(i,j) I(x+i, y+j), \tag{2} $$
with $G(i,j) = \frac{1}{2\pi\sigma^2}\exp(-\frac{i^2+j^2}{2\sigma^2})$. This preserves edge sharpness while reducing intensity noise.
2.3 Virtual Camera Optimization
Instead of using the same image resolution as the RGB camera, we generate a low-resolution intensity image based on the LiDAR field of view and the plane size. Assuming the LiDAR horizontal resolution is $\psi_l$ and its FOV is $\theta_l$, and the virtual camera FOV is $\theta_c$, the virtual camera resolution is:
$$ \psi_c = \min\left(\psi_l, \frac{\psi_l \theta_c}{\theta_l}\right). \tag{3} $$
Initially, $\theta_c$ is set equal to $\theta_l$, and thus $\psi_c = \psi_l$. The focal length is computed as:
$$ f = \frac{\psi_c}{2\tan(\theta_c/2)}. \tag{4} $$
After processing the first projection, the camera center and FOV are updated to ensure the plane is fully covered. This adaptive scheme significantly reduces the number of virtual pixels and avoids empty regions, making the intensity edge extraction faster and more accurate.
2.4 Feature Matching and Calibration
After extracting the final feature set, which is the union of geometric edge points and intensity edge points from the LiDAR, we project all these points onto the image plane using the current extrinsic estimate. For each projected point $p_i$, we search for its $k$ nearest neighbors among the image edge pixels using a KD-tree. Let $q_{ij}$ be the $j$-th neighbor. We compute the centroid and normal:
$$ \bar{q}_i = \frac{1}{k}\sum_{j=1}^{k} q_{ij}, \quad S_i = \frac{1}{k}\sum_{j=1}^{k} (q_{ij}-\bar{q}_i)(q_{ij}-\bar{q}_i)^\top. \tag{5} $$
The normal $n_i$ is the eigenvector corresponding to the smallest eigenvalue of $S_i$. The optimal extrinsic is found by minimizing the sum of squared point-to-edge distances:
$$ 0 = n_i^\top \left( f(\pi(T_L^C P_i^L)) – \bar{q}_i \right). \tag{6} $$
We solve this nonlinear least-squares problem using the Levenberg-Marquardt algorithm. The process is repeated iteratively until convergence. By combining geometric and intensity edges, we enrich the feature pool and improve the convergence basin, leading to more stable results in both feature-rich and feature-sparse environments.
2.5 Experiments
2.5.1 Dataset and Setup
We evaluate our calibration method on the ACLC public dataset and our own private dataset collected with a Livox Mid-70 LiDAR and an Intel RealSense D435 camera. We compare our method with two targetless state-of-the-art approaches: Livox-Calib and Direct-Calib, and with the target-based method ACLC. The reprojection error of chessboard corners is computed using normalized root mean square distance:
$$ RE = \frac{1}{|P|}\sum_{p_i \in P} k_i \| \text{reproj}(p_i) – I_c \|_2, \tag{7} $$
where $k_i = d(p_i)/d_{\max}$ is a distance weighting factor. Lower is better.
2.5.2 Quantitative Results
Table 1 reports the reprojection errors on the ACLC dataset. Our method achieves the lowest error among the targetless methods, and it is close to the target-based ACLC which uses all 21 pairs of chessboard data. Notably, our method only uses one LiDAR frame and one image frame, while Livox-Calib and Direct-Calib require three or more frames or manual initialization.
| Method | Data Used | Mean RE (pixels) |
|---|---|---|
| ACLC | 21 frames | 0.42 |
| Direct-Calib | 1 frame + manual init. | 1.38 |
| Livox-Calib | 3 frames | 0.95 |
| Ours (with intensity) | 1 frame | 0.51 |
Table 1. Reprojection error comparison on the ACLC dataset. The proposed method achieves the best targetless accuracy.
2.5.3 Cross-Validation
To evaluate our method in real-world scenarios without a ground-truth target, we use a stereo camera and treat the two camera frames as a known rigid pair. We calibrate each camera with the LiDAR independently, then compute the relative transformation between the two cameras. The error is measured against the known baseline. We classify scenes into two categories: A (small scenes with sparse features) and B (wide scenes with rich features). The translation and rotation errors are computed for 10 runs in each category. The mean translation error in category A is 0.021 m, and in category B is 0.017 m. The mean rotation error is 0.18° in category A and 0.12° in category B. Compared with Livox-Calib, our translation error is reduced by about 50% in both categories, demonstrating the benefit of incorporating intensity edges.
2.5.4 Ablation
We also perform an ablation by removing the intensity edge module. The results, shown in Table 2, indicate that the full method (with intensity) yields a lower reprojection error and lower cross-validation translation error than the version without intensity. This confirms the contribution of intensity features to matching robustness and calibration accuracy.
| Method | Mean RE (pixels) | Trans. Error (m) |
|---|---|---|
| Without intensity | 0.72 | 0.038 |
| With intensity (full) | 0.51 | 0.019 |
Table 2. Ablation results showing the effect of intensity features.
3. 3D Gaussian Splatting SLAM with Multi-Sensor Fusion
3.1 Overview
This section presents our 3DGS-based SLAM system designed for a humanoid robot equipped with a non-repetitive solid-state LiDAR, a color camera, and an IMU. The system architecture is shown in the conceptual workflow: raw LiDAR scans, camera frames, and IMU measurements are fed into parallel threads. The LiDAR-inertial odometry uses an iterated error-state Kalman filter to provide robust initial pose estimates and to create a dense depth map by accumulating a sliding window of points. The pose is then refined by a gradient-based tracking module that compares the rendered depth and color from the current Gaussian map with the sensor observations. Finally, the Gaussian map is updated with a photometric and geometric gradient loss together with regularization terms. This two-stage approach balances efficiency and accuracy.
3.2 3D Gaussian Splatting Representation
Our map is represented as a set of isotropic 3D Gaussians. Each Gaussian has a center position $\mu \in \mathbb{R}^3$, a radius $r$, an opacity $o \in [0,1]$, and a view-independent color $c \in \mathbb{R}^3$. The influence of a Gaussian on a 3D point $x$ is given by:
$$ f(x) = o \exp\left(-\frac{\|x-\mu\|^2}{2r^2}\right). \tag{8} $$
When rendering an image from a camera pose $E_t$, the projection of the Gaussian center onto the image plane is:
$$ \mu_{2D} = \frac{K E_t \mu}{d}, \quad r_{2D} = \frac{f r}{d}, \quad d = (E_t \mu)_z, \tag{9} $$
where $K$ is the camera intrinsic matrix and $f$ is the focal length. The rendered color at pixel $p$ is computed by alpha-compositing all contributions in front-to-back order:
$$ C(p) = \sum_{i=1}^{n} c_i f_i(p) \prod_{j=1}^{i-1} (1 – f_j(p)). \tag{10} $$
Similarly, the rendered depth is:
$$ D(p) = \sum_{i=1}^{n} d_i f_i(p) \prod_{j=1}^{i-1} (1 – f_j(p)), \tag{11} $$
where $d_i$ is the depth of the $i$-th Gaussian center in the camera frame. We also compute a silhouette map $S(p)$ by replacing $c_i$ in (10) with 1:
$$ S(p) = \sum_{i=1}^{n} f_i(p) \prod_{j=1}^{i-1} (1 – f_j(p)). \tag{12} $$
The silhouette map indicates which pixels are covered by existing Gaussians and is used to mask areas where the map has not yet been built.
3.3 LiDAR-Inertial Odometry
Non-repetitive solid-state LiDARs produce shallow but continuously changing scan patterns, making frame-to-frame matching unreliable. We therefore employ a frame-to-map matching scheme based on IESKF. The IMU provides high-rate propagation of the robot state, including rotation $R$, velocity $V$, and position $P$. The discrete motion model is:
$$ R_{k+1} = R_k \exp(\omega_i – b_g) \Delta t, \tag{13} $$
$$ V_{k+1} = V_k + R_k (a_i – b_a) \Delta t + g \Delta t, \tag{14} $$
$$ P_{k+1} = P_k + V_k \Delta t + \frac{1}{2}(R_k(a_i – b_a) + g)\Delta t^2. \tag{15} $$
Here $\omega_i$ and $a_i$ are the IMU angular velocity and linear acceleration, $b_g$ and $b_a$ are biases, $g$ is gravity, and $\Delta t$ is the IMU sampling interval. The state is augmented with biases. To correct the IMU prediction, we register the current LiDAR scan against a local point-cloud map. The residual is computed as the point-to-plane distance. The IESKF update yields a posterior state estimate. This odometry module runs at approximately 50 Hz and outputs the initial pose $T_{init}$ for each camera frame.
Furthermore, the dense local map is projected into the camera view using the current extrinsic and used as a temporary depth map $D_{real}$. This depth map is much denser than a single LiDAR frame, particularly when the camera and LiDAR have a large parallax. The dense depth map provides strong geometric supervision for both tracking and map updating.
3.4 Gradient-Based Tracking
The initial pose from LiDAR-inertial odometry is already accurate, but to exploit visual information we refine it by minimizing the differences between the rendered depth, rendered RGB, and the real data. We define the depth gradient loss:
$$ L_{\mathrm{depth\_grad}} = \sum_{p \in \mathrm{pixels}} | \nabla_d I_{\mathrm{render}}(p) – \nabla_d I_{\mathrm{real}}(p) |^2, \tag{16} $$
where $I_{\mathrm{render}}$ is the rendered depth map from the current Gaussian map, and $I_{\mathrm{real}}$ is the dense depth map from the LiDAR projection. The RGB gradient loss is:
$$ L_{\mathrm{rgb\_grad}} = \sum_{p \in \mathrm{pixels}} | \nabla_c I_{\mathrm{render}}^{\mathrm{rgb}}(p) – \nabla_c I_{\mathrm{real}}^{\mathrm{rgb}}(p) |^2, \tag{17} $$
where the superscript rgb denotes the rendered color image and the real camera image. The total tracking loss is:
$$ L_{\mathrm{track}} = S(\lambda_1 L_{\mathrm{depth\_grad}} + \lambda_2 L_{\mathrm{rgb\_grad}}), \tag{18} $$
where $S$ is the silhouette mask from equation (12), and $\lambda_1$, $\lambda_2$ are weights. The pose $T$ is optimized by gradient descent with respect to the camera parameters. Since the initial pose from the IESKF is close to the optimum, only a few iterations (e.g., 10) are needed. This makes the refinement fast and robust.
3.5 Gaussian Map Update
After the pose is refined, we update the Gaussian map using the current frame and selected keyframes. The map loss is:
$$ L_{\mathrm{map}} = \lambda_1 L_{\mathrm{depth\_grad}} + \lambda_2 L_{\mathrm{rgb\_grad}}. \tag{19} $$
In contrast to the tracking loss, the map loss is not masked by the silhouette, because we want to optimize all pixels including those not yet covered. In addition, we introduce an opacity regularization term:
$$ L_{\mathrm{opacity}} = |o – \tau|, \tag{20} $$
where $\tau$ is a threshold below which the Gaussian is considered nearly transparent and can be pruned. This encourages efficient use of Gaussians. A geometric overlap term is also added:
$$ L_{\mathrm{overlap}} = \mathrm{overlap}(\mathrm{CurrentFrame}, \mathrm{KeyFrames}), \tag{21} $$
which ensures that only Gaussians in the overlapping region are updated. To preserve visual quality, we add an SSIM loss:
$$ L_{\mathrm{SSIM}} = 1 – \mathrm{SSIM}(I_{\mathrm{render}}, I_{\mathrm{real}}). \tag{22} $$
The final map loss is:
$$ L_{\mathrm{total}} = L_{\mathrm{map}} + \alpha L_{\mathrm{opacity}} + \beta L_{\mathrm{SSIM}}, \tag{23} $$
with $\alpha=0.01$ and $\beta=0.5$ in our experiments. New Gaussians are initialized from pixels that are not well explained by the current map. The radius of a new Gaussian is computed from its depth $D_{GT}$ and the camera focal length $f$:
$$ r = \frac{D_{GT}}{f}. \tag{24} $$
The color is taken from the corresponding pixel, and the opacity is assigned a default mid-level value. During optimization, the positions, radii, opacities, and colors are jointly optimized using the automatic differentiation of the renderer.
3.6 Adaptive Weighting and Keyframe Selection
The reliability of the depth gradient loss depends on the density of the LiDAR-provided depth map. In open areas where the LiDAR points are sparse, the depth gradient may be noisy and should have a lower weight. We compute an adaptive scalar based on the depth density $x$ (the fraction of valid depth pixels in the image):
$$ \sigma(x) = \frac{1}{1+e^{-x}}. \tag{25} $$
The weights $\lambda_1$ and $\lambda_2$ in (18) are then scaled by $\sigma(x)$ and $1-\sigma(x)$, respectively. This automatically balances the geometric and photometric contributions under varying conditions.
For map updating, we select keyframes based on both temporal spacing and overlap. The most recent keyframe and the frame with the highest overlap with the current view are jointly optimized. This reduces redundant computation while maintaining global consistency.
3.7 Experiments
3.7.1 Setup and Datasets
We evaluate our system on public datasets: R3LIVE (Hku_campus_seq_00, Hku_campus_seq_01, Hkust_campus_00, Degenerate_seq_00, Degenerate_seq_01, Degenerate_seq_02) and FAST-LIVO (Hku2). All datasets contain synchronized LiDAR, camera, and IMU data with 15 Hz camera frame rate, 10 Hz LiDAR frame rate, and 200 Hz IMU rate. The image resolution is 640×512. We run our method on an NVIDIA RTX 4090D GPU. For very long sequences, we use the first 100 seconds to avoid GPU memory overflow.
3.7.2 Rendering Quality
We compare the rendering quality against Point-SLAM (a NeRF-based method), MonoGS, and SplaTAM (3DGS-based methods). The metrics are PSNR, SSIM, and LPIPS. Table 3 shows results on several test sequences. Our method consistently outperforms all baselines in terms of PSNR and SSIM, and achieves the best average LPIPS. On the outdoor Hku_campus_seq_00, our PSNR is 21.25 compared to 17.31 for MonoGS and 16.89 for SplaTAM. The average PSNR across all datasets is 20.07, which is about 9% higher than MonoGS and 26% higher than SplaTAM.
| Dataset | Metric | MonoGS | SplaTAM | Ours |
|---|---|---|---|---|
| Hku2 | PSNR | 23.17 | 19.36 | 24.71 |
| SSIM | 0.652 | 0.472 | 0.697 | |
| LPIPS | 0.457 | 0.511 | 0.303 | |
| Hku_campus_seq_00 | PSNR | 17.31 | 16.89 | 21.25 |
| SSIM | 0.602 | 0.533 | 0.725 | |
| LPIPS | 0.436 | 0.434 | 0.287 | |
| Degenerate_seq_00 | PSNR | 20.17 | 15.26 | 21.03 |
| SSIM | 0.726 | 0.605 | 0.810 | |
| LPIPS | 0.268 | 0.367 | 0.180 | |
| Degenerate_seq_01 | PSNR | 17.66 | 14.63 | 18.60 |
| SSIM | 0.635 | 0.449 | 0.773 | |
| LPIPS | 0.438 | 0.500 | 0.373 | |
| Degenerate_seq_02 | PSNR | 16.86 | 15.01 | 20.23 |
| SSIM | 0.757 | 0.442 | 0.726 | |
| LPIPS | 0.334 | 0.564 | 0.281 | |
| Hku_campus_seq_01 | PSNR | 17.98 | 15.25 | 19.59 |
| SSIM | 0.632 | 0.445 | 0.671 | |
| LPIPS | 0.398 | 0.490 | 0.271 | |
| Hkust_campus_00 | PSNR | 15.23 | 14.45 | 15.09 |
| SSIM | 0.567 | 0.395 | 0.423 | |
| LPIPS | 0.507 | 0.550 | 0.492 | |
| Average | PSNR | 18.34 | 15.84 | 20.07 |
| SSIM | 0.653 | 0.477 | 0.689 | |
| LPIPS | 0.405 | 0.488 | 0.312 |
Table 3. Quantitative comparison of rendering quality among 3DGS-based SLAM systems.
3.7.3 Tracking Accuracy
Since the datasets do not provide ground-truth trajectories, we evaluate tracking accuracy using the loop-closure property of three sequences: Degenerate_seq_01, Degenerate_seq_02, and Hku2, where the robot starts and ends at the same location. We use EVO to extract the final position and yaw errors. Our method yields nearly zero displacement in the x and y directions, and a very small z drift (< 0.1 m). The final yaw error is below 2° for all three sequences. This demonstrates excellent long-term consistency without explicit loop closure.
3.7.4 Efficiency
We compare the processing time of our complete system with a variant that removes the LiDAR-inertial odometry and uses only 60 iterations of gradient optimization. As shown in Table 4, our method achieves a 55% reduction in total per-frame time while maintaining higher accuracy. The LiDAR-inertial odometry runs in 20 ms per frame, and the gradient refinement takes about 200 ms for 10 iterations. The map update requires about 1.68 s per frame due to keyframe joint optimization. The total per-frame time is approximately 1.9 s, which is suitable for offline datasets and interactive applications.
| Module | Time per frame (s) |
|---|---|
| LiDAR-inertial odometry | 0.02 |
| Gradient tracking | 0.20 |
| Map update | 1.68 |
| Total (ours) | 1.90 |
| Without LiDAR odometry (60 iterations) | 3.04 |
Table 4. Per-frame computational cost of the proposed SLAM system.
3.7.5 Ablation Study
We perform an ablation on the Hku_campus_seq_00 dataset, comparing the full method with a version without the adaptive weight and a version without the LiDAR odometry (using only gradient tracking). Table 5 reports the results. The full method achieves the best PSNR, SSIM, and LPIPS. Removing the LiDAR odometry causes a significant drop in performance because the gradient tracking alone cannot converge to the optimal pose within the limited number of iterations. The adaptive weight also contributes to a noticeable improvement.
| Method | PSNR | SSIM | LPIPS |
|---|---|---|---|
| Without LiDAR odometry | 16.93 | 0.592 | 0.419 |
| Without adaptive weight | 20.17 | 0.689 | 0.323 |
| Full method | 21.30 | 0.725 | 0.287 |
Table 5. Ablation study results.
4. Humanoid Robot Application
4.1 Hardware Design
To validate the proposed methods in a realistic humanoid robot setting, we design and build a small custom humanoid robot platform. The robot has 18 degrees of freedom: 5 DOF for each leg, 3 DOF for each arm, and 2 DOF for the head. The joints are driven by feetech STS3215 serial bus servos with a stall torque of 19 kg/cm. The main controller is a Raspberry Pi 4B (8 GB RAM) running Ubuntu 22.04 Server. The robot is equipped with three sensing modalities: an Intel RealSense D435i RGB-D camera mounted on the head, a Livox Mid-70 solid-state LiDAR placed on the chest, and a WHEELTEC N100 IMU installed near the center of mass. The camera and LiDAR provide complementary visual and geometric information, while the IMU provides high-rate inertial data. The structural parts are manufactured with 3D printing to balance strength and weight.
4.2 Kinematic Modeling
For gait planning and control, we establish a kinematic model of the humanoid robot. The lower body is modeled as two serial chains originating from the hip. We define three virtual coordinate frames: the center of the robot, the support foot, and the swing foot. The transformation from the support foot frame $O_0$ to the robot center frame $O_6$ is obtained by multiplying the individual joint transformations:
$$ T_6^0 = T_1^0 T_2^1 T_3^2 T_4^3 T_5^4 T_6^5. \tag{26} $$
Similarly, the transformation from the swing foot frame to the robot center is:
$$ T_{12}^6 = T_7^6 T_8^7 T_9^8 T_{10}^9 T_{11}^{10} T_{12}^{11}. \tag{27} $$
Assuming the feet and the robot center remain upright, the rotation parts are identity, and we only need to solve for the translations. By constraining the orientation such that the sum of the pitch angles equals zero:
$$ \theta_2 + \theta_3 + \theta_4 = 0, \quad \theta_1 + \theta_5 = 0, \quad \theta_8 + \theta_9 + \theta_{10} = 0, \quad \theta_7 + \theta_{11} = 0. \tag{28} $$
We derive the analytic forward kinematics. For a given step length $x$ and step period $T_s$, the desired foot trajectory is represented as a sinusoidal profile. Combining with the inverse kinematics yields the joint angle trajectories for each time step. The lateral balance is maintained by shifting the center of mass using the ZMP criterion. At low walking speeds, the ZMP can be approximated by the projection of the center of mass onto the ground. The center of mass positions in the horizontal plane are:
$$ x_{ZMP} = \frac{\sum m_i g X_i}{\sum m_i g}, \quad z_{ZMP} = \frac{\sum m_i g Z_i}{\sum m_i g}. \tag{29} $$
By assigning a sinusoidal trajectory for the lateral sway, we generate smooth joint motions for the frontal hip joints, enabling the robot to walk stably.
4.3 Sensor Calibration on the Humanoid Robot
We deploy the proposed targetless LiDAR-camera calibration method to determine the extrinsic between the Livox LiDAR and the RealSense camera. The robot is placed in a room with natural texture and edges. We accumulate a single dense scan and a single RGB image, then extract the combined geometric and intensity edges. The optimization yields an extrinsic that aligns the LiDAR points with the image within about one pixel of reprojection error. For the LiDAR-IMU extrinsic, we use a state-of-the-art initialization method to estimate the transform by fusing LiDAR and IMU data during a short excitation motion.
Because the camera is mounted on the robot head with two joints (pan and tilt), its pose relative to the robot base changes during walking. We compensate for this by online reading the head joint angles and composing the base-to-head transformation with the static extrinsic. This yields a real-time variable extrinsic that remains valid even when the head moves. The humanoid robot therefore always has a correct LiDAR-camera alignment, which is critical for the downstream SLAM system.
4.4 SLAM Deployment and Experimental Results
We run the proposed 3DGS-based SLAM system on the humanoid robot while it walks in an indoor office environment and a laboratory environment. The LiDAR and IMU data are processed by the LiDAR-inertial odometry to provide initial poses; the camera images are used for gradient refinement and map updating. We reduce the number of map optimization iterations for real-time operation. The rendering quality is evaluated on three test sequences, as shown in Table 6. The PSNR values range from 17.68 to 22.19, SSIM from 0.660 to 0.863, and LPIPS from 0.187 to 0.352, indicating that the reconstructed 3DGS map closely matches the actual camera views.
| Sequence | PSNR | SSIM | LPIPS |
|---|---|---|---|
| 1 | 20.10 | 0.863 | 0.187 |
| 2 | 17.68 | 0.660 | 0.352 |
| 3 | 22.19 | 0.825 | 0.271 |
Table 6. Rendering quality metrics for the humanoid robot walking experiments.
The resulting maps provide dense, photorealistic 3D representations of the environments. The robot can localize itself accurately during the entire walking sequence, even when the camera experiences motion blur and the LiDAR depth becomes sparse. This demonstrates that the proposed multi-sensor fusion strategy is effective for a real humanoid robot under realistic operating conditions.
5. Conclusion
In this paper, we have presented a comprehensive study of simultaneous localization and mapping for humanoid robots. We have proposed a targetless LiDAR-camera calibration method that integrates geometric and intensity edges, enabling robust and accurate extrinsic calibration in natural scenes without artificial targets. This calibration is essential for long-term autonomous humanoid robot deployment, as it can be used to correct sensor drift caused by mechanical vibrations. We have also developed a 3DGS-based multi-sensor SLAM system that fuses a non-repetitive LiDAR, a camera, and an IMU. The system combines an IESKF-based LiDAR-inertial odometry with a gradient-based visual refinement and an incremental Gaussian map update. Extensive experiments on public datasets and on our own humanoid robot platform demonstrate that the proposed method achieves state-of-the-art rendering quality, robust tracking accuracy, and superior efficiency compared to existing NeRF-based and 3DGS-based SLAM approaches.
The integration of these methods into a small humanoid robot has validated the complete pipeline from hardware design, kinematic modeling, gait generation, and sensor calibration to real-time dense mapping and localization. Our results confirm that the proposed approach provides a solid foundation for humanoid robot autonomous navigation, environmental perception, and future high-level tasks such as object manipulation and human-robot collaboration. In future work, we plan to extend the system to handle dynamic environments, incorporate loop closure for global consistency, and further optimize the Gaussian map representation for large-scale outdoor operation. The humanoid robot will continue to serve as the primary testbed for these developments.