i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +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
corelib/src/Features2d.cpp
+1 -1
View File
@@ -217,7 +217,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints)
{
UASSERT_MSG((int)keypoints.size() == descriptors.rows || descriptors.rows == 0, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
UASSERT_MSG((int)keypoints.size() == keypoints3D.size() || keypoints3D.size() == 0, uFormat("keypoints=%d keypoints3D=%d", (int)keypoints.size(), (int)keypoints3D.size()).c_str());
UASSERT_MSG(keypoints.size() == keypoints3D.size() || keypoints3D.size() == 0, uFormat("keypoints=%d keypoints3D=%d", (int)keypoints.size(), (int)keypoints3D.size()).c_str());
if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
{
UTimer timer;
guilib/src/MainWindow.cpp
+18 -12
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,