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

Handling intial odometry pose in all odometry approaches. For VIO approaches, gravity initialization is handled too. (#298)

Browse Source
This commit is contained in:
matlabbe
2018-07-19 14:10:36 -04:00
parent 9ae47b79f9
commit 173bd49a26
12 changed files with 124 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Memory.cpp
+20 -1
View File
@@ -2538,7 +2538,26 @@ Transform Memory::computeTransform(
{
int id = iter->first;
const Signature * s = this->getSignature(id);
CameraModel model = s->sensorData().cameraModels()[0];
CameraModel model;
if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
{
model = s->sensorData().cameraModels()[0];
}
else if(s->sensorData().stereoCameraModel().isValidForProjection())
{
model = s->sensorData().stereoCameraModel().left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-s->sensorData().stereoCameraModel().baseline()*model.fx());
}
else
{
UFATAL("no valid camera model to use local bundle adjustment on loop closure!");
}
bundleModels.insert(std::make_pair(id, model));
Transform invLocalTransform = model.localTransform().inverse();
if(iter->second.isValid())