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

Refactored 3DTo2D and 3DTo3D motion estimations (Memory and OdometryBOW are now using the same methods)

Browse Source
This commit is contained in:
matlabbe
2015-06-27 00:08:52 -04:00
parent 6df403ed42
commit e5447be23a
8 changed files with 492 additions and 412 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/util3d_features.cpp
+11 -13
View File
@@ -351,18 +351,6 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
}
}
if(!useCameraTransformGuess)
{
cv::Mat R, T;
EpipolarGeometry::findRTFromP(P, R, T);
Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2));
cameraTransform = (cameraModel.localTransform() * t).inverse() * cameraModel.localTransform();
}
if(refGuess3D.size())
{
// scale estimation
@@ -489,7 +477,6 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
else
{
UWARN("No inliers after PnP!");
cameraTransform = Transform();
}
}
}
@@ -498,6 +485,17 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
UWARN("Cannot compute the scale, no points corresponding between the generated ref words and words guess");
}
}
else if(!useCameraTransformGuess)
{
cv::Mat R, T;
EpipolarGeometry::findRTFromP(P, R, T);
Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2));
cameraTransform = (cameraModel.localTransform() * t).inverse() * cameraModel.localTransform();
}
}
}
}