i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed bundler exportation not using local transform

Browse Source
This commit is contained in:
matlabbe
2017-01-04 17:49:59 -05:00
parent 91f78b54d8
commit dfb965cfa9
2 changed files with 19 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
guilib/src/MainWindow.cpp
View File

@@ -6056,21 +6056,27 @@ void MainWindow::exportBundlerFormat()
localTransform = _cachedSignatures[iter->first].sensorData().stereoCameraModel().left().localTransform();
}
Transform rotation(0,-1,0,0,
0,0,1,0,
-1,0,0,0);
static const Transform opengl_world_T_rtabmap_world(
0.0f, -1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
-1.0f, 0.0f, 0.0f, 0.0f);
Transform R = rotation*iter->second.rotation().inverse();
static const Transform optical_rotation_inv(
0.0f, -1.0f, 0.0f, 0.0f,
0.0f, 0.0f, -1.0f, 0.0f,
1.0f, 0.0f, 0.0f, 0.0f);
out << R.r11() << " " << R.r12() << " " << R.r13() << "\n";
out << R.r21() << " " << R.r22() << " " << R.r23() << "\n";
out << R.r31() << " " << R.r32() << " " << R.r33() << "\n";
Transform pose = iter->second;
if(!localTransform.isNull())
{
pose*=localTransform*optical_rotation_inv;
}
Transform poseGL = opengl_world_T_rtabmap_world*pose.inverse();
Transform t = R * iter->second.translation();
t.x() *= -1.0f;
t.y() *= -1.0f;
t.z() *= -1.0f;
out << t.x() << " " << t.y() << " " << t.z() << "\n";
out << poseGL.r11() << " " << poseGL.r12() << " " << poseGL.r13() << "\n";
out << poseGL.r21() << " " << poseGL.r22() << " " << poseGL.r23() << "\n";
out << poseGL.r31() << " " << poseGL.r32() << " " << poseGL.r33() << "\n";
out << poseGL.x() << " " << poseGL.y() << " " << poseGL.z() << "\n";
}
QMessageBox::question(this,