luigifreda / plvs

PLVS is a real-time SLAM system with points, lines, volumetric mapping and 3D unsupervised incremental segmentation.
GNU General Public License v3.0
453 stars 67 forks source link

Scaling Problem in 3D Reconstruction Using PLVS on KITTI Sequence 00 #33

Closed Ayoub-adil closed 3 months ago

Ayoub-adil commented 3 months ago

Hey Luigi, I am testing PLVS to perform 3D reconstruction from a sequence of images. During the evaluation of the odometry, I noticed that the algorithm gives distances that are significantly smaller than the actual world distances, even though the camera calibration is accurate.

Observations

I tested PLVS on sequence 00 of the KITTI dataset and compared the results with the ground truth provided by KITTI. It appears that there is a scaling issue: PLVS produces a 3D trajectory that is much smaller than the real-world trajectory.

Screenshot from 2024-05-28 16-09-06

Detailed Findings

This discrepancy suggests that the problem is not simply a matter of unit conversion (e.g., meters to millimeters). The distance computed by the PLVS is drastically smaller than the actual distance.

Request for Assistance

I am seeking help to understand and resolve this scaling issue. Any insights into potential causes or suggestions for debugging this problem would be greatly appreciated.

Thank you!

luigifreda commented 3 months ago

Hi, thank you for the feedback.

To properly understand your points, would you mind to share how you tested PLVS:

This discrepancy suggests that the problem is not simply a matter of unit conversion (e.g., meters to millimeters). The distance computed by the PLVS is drastically smaller than the actual distance.

I am not sure to follow this comment. Would you mind further elaborating on it?

the problem is not simply a matter of unit conversion

In particular, could you provide more evidence and details on this point? What do you exactly mean?

Ayoub-adil commented 3 months ago

Hi, Thank you for your response.

To address your questions, here are the details of my testing setup and methodology:

YAML Settings File config_mono_0_2.yaml


#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
File.version: "1.0"

Camera.type: "PinHole"

Camera.width: 1241
Camera.height: 376
Camera.fps: 10
Camera.RGB: 1

Camera1.fx: 718.856
Camera1.fy: 718.856
Camera1.cx: 607.1928
Camera1.cy: 185.2157

Camera1.k1: 0.0
Camera1.k2: 0.0
Camera1.p1: 0.0
Camera1.p2: 0.0

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------
ORBextractor.nFeatures: 2000
ORBextractor.scaleFactor: 1.2
ORBextractor.nLevels: 8
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.on: 1
Viewer.KeyFrameSize: 0.1
Viewer.KeyFrameLineWidth: 1.0
Viewer.GraphLineWidth: 1.0
Viewer.PointSize: 2.0
Viewer.CameraSize: 0.15
Viewer.CameraLineWidth: 2.0
Viewer.ViewpointX: 0.0
Viewer.ViewpointY: -10.0
Viewer.ViewpointZ: -0.1
Viewer.ViewpointF: 2000.0
Viewer.imageViewScale: 0.5

#--------------------------------------------------------------------------------------------
# Sparse Mapping
#--------------------------------------------------------------------------------------------
SparseMapping.filename: "sparse_kitti_mono.atlas"
SparseMapping.reuseMap: 0
SparseMapping.saveMap: 1 
SparseMapping.forceRelocalization: 1

#--------------------------------------------------------------------------------------------
# Depth Noise Model
#--------------------------------------------------------------------------------------------
Depth.sigmaZfactor: 6

#--------------------------------------------------------------------------------------------
# PointCloud Mapping
#--------------------------------------------------------------------------------------------
PointCloudMapping.on: 1
PointCloudMapping.type: "voxblox"
PointCloudMapping.resolution: 0.03
PointCloudMapping.numKeyframesToQueueBeforeProcessing: 1
PointCloudMapping.downSampleStep: 2
PointCloudMapping.minDepth: 0.1
PointCloudMapping.maxDepth: 5
PointCloudMapping.removeUnstablePoints: 1
PointCloudMapping.resetOnSparseMapChange: 1
PointCloudMapping.cloudDeformationOnSparseMapChange: 0
PointCloudMapping.pointCounterThreshold: 3
PointCloudMapping.voxbloxIntegrationMethod: "fast"
PointCloudMapping.useCarving: 0
PointCloudMapping.filterDepth.on: 0
PointCloudMapping.filterDepth.diameter: 7
PointCloudMapping.filterDepth.sigmaDepth: 0.02
PointCloudMapping.filterDepth.sigmaSpace: 5

PointCloudMapping.loadMap: 0
PointCloudMapping.loadFilename: "volumetric_map_0.ply"

Command or Script for Launching

DATA_FOLDER="../DATA/KITTI"
APP_FOLDER="../APP/"
TOOLS_FOLDER="../TOOLS/"
APP="kitti_mono"

SEQUENCE="00"
CONFIG="config_mono_0_2.yaml"   

PATH_TO_SEQUENCE_FOLDER="$DATA_FOLDER/$SEQUENCE"
PATH_TO_CONFIG="$DATA_FOLDER/$CONFIG"

VOC="$APP_FOLDER/ORBvoc.txt"

$APP_FOLDER$APP \
    $VOC \
    $PATH_TO_CONFIG \
    $PATH_TO_SEQUENCE_FOLDER \

# evaluate the results
python3 $TOOLS_FOLDER/A_kitti.py $PATH_TO_SEQUENCE_FOLDER/$SEQUENCE.txt ./KeyFrameTrajectory.txt 

Code Changes

No significant changes were made to the original codebase.

Clarification on the Scaling Issue

Regarding the scaling issue, here is a more detailed explanation:

To ensure accuracy, I calculated the total distance traveled using both the ground truth data and the PLVS results. The distance obtained from PLVS is significantly smaller than the ground truth distance, indicating a scaling factor issue. The factor is 15, which rules out a simple unit conversion problem (e.g., kilometers to meters: factor!=1000 ). I also tested other data sequences and found that this scaling factor varies between sequences (e.g., 15 for KITTI-00 and 8 for another sequence).

I hope this clarifies the issue. Please let me know if you need any more information or specific details.

Thank you Luigi!

luigifreda commented 3 months ago

You are using monocular SLAM settings. That means the reconstruction is up to scale. If you want a metric trajectory estimation then you need to use stereo settings. See my scripts in the Scripts folder.: run_kitti_stereo.sh for KITTI datasets