i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 17:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed camera local transform when used with lidar and odomSensor

Browse Source
This commit is contained in:
matlabbe
2026-07-04 11:37:55 -07:00
parent c0a4eb561d
commit 4e03039934
1 changed files with 20 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/SensorCaptureThread.cpp
+20 -11
View File
@@ -114,7 +114,6 @@ SensorCaptureThread::SensorCaptureThread(
_camera(camera), _camera(camera),
_odomSensor(odomSensor), _odomSensor(odomSensor),
_lidar(lidar), _lidar(lidar),
_extrinsicsOdomToCamera(extrinsics * CameraModel::opticalRotation()),
_odomAsGt(false), _odomAsGt(false),
_poseTimeOffset(poseTimeOffset), _poseTimeOffset(poseTimeOffset),
_poseScaleFactor(poseScaleFactor), _poseScaleFactor(poseScaleFactor),
@@ -153,10 +152,14 @@ SensorCaptureThread::SensorCaptureThread(
{ {
if(_camera) if(_camera)
{ {
if(_odomSensor == _camera && _extrinsicsOdomToCamera.isNull()) if(_odomSensor == _camera && extrinsics.isNull())
{ {
_extrinsicsOdomToCamera.setIdentity(); _extrinsicsOdomToCamera.setIdentity();
} }
else
{
_extrinsicsOdomToCamera = extrinsics * CameraModel::opticalRotation();
}
UASSERT(!_extrinsicsOdomToCamera.isNull()); UASSERT(!_extrinsicsOdomToCamera.isNull());
UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str()); UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str());
} }
@@ -492,20 +495,26 @@ void SensorCaptureThread::mainLoop()
} }
} }
// Adjust local transform of the camera based on the pose frame // Adjust local transform of the camera(s) based on the pose frame. The correction
// and odom->camera extrinsics are frame-level, so apply the same prefix to each
// camera while keeping its own local transform (multi-camera supported).
if(!data.cameraModels().empty()) if(!data.cameraModels().empty())
{ {
UASSERT(data.cameraModels().size()==1); std::vector<CameraModel> models = data.cameraModels();
CameraModel model = data.cameraModels()[0]; for(size_t i=0; i<models.size(); ++i)
model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera); {
data.setCameraModel(model); models[i].setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera*models[i].localTransform());
}
data.setCameraModels(models);
} }
else if(!data.stereoCameraModels().empty()) else if(!data.stereoCameraModels().empty())
{ {
UASSERT(data.stereoCameraModels().size()==1); std::vector<StereoCameraModel> models = data.stereoCameraModels();
StereoCameraModel model = data.stereoCameraModels()[0]; for(size_t i=0; i<models.size(); ++i)
model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera); {
data.setStereoCameraModel(model); models[i].setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera*models[i].localTransform());
}
data.setStereoCameraModels(models);
} }
} }