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

Fixed multi-kinects keypoint projection

Browse Source
This commit is contained in:
matlabbe
2016-03-11 23:27:41 -05:00
parent 3cb3524aa2
commit 6c15cb8b25
3 changed files with 7 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/Features2d.cpp
View File

@@ -579,7 +579,7 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
const std::vector<cv::KeyPoint> & keypoints) const const std::vector<cv::KeyPoint> & keypoints) const
{ {
std::vector<cv::Point3f> keypoints3D; std::vector<cv::Point3f> keypoints3D;
if(!data.depthOrRightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection()) if(!data.rightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection())
{ {
//stereo //stereo
cv::Mat imageMono; cv::Mat imageMono;

2
corelib/src/RegistrationVis.cpp
View File

@@ -180,7 +180,7 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar); UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError); UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags); UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach?1:0); UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize); UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations); UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
UDEBUG("%s=%f", Parameters::kVisCorFlowEps().c_str(), _flowEps); UDEBUG("%s=%f", Parameters::kVisCorFlowEps().c_str(), _flowEps);

9
corelib/src/util3d_features.cpp
View File

@@ -72,18 +72,19 @@ std::vector<cv::Point3f> generateKeypoints3DDepth(
UASSERT(int((depth.cols/cameraModels.size())*cameraModels.size()) == depth.cols); UASSERT(int((depth.cols/cameraModels.size())*cameraModels.size()) == depth.cols);
float subImageWidth = depth.cols/cameraModels.size(); float subImageWidth = depth.cols/cameraModels.size();
keypoints3d.resize(keypoints.size()); keypoints3d.resize(keypoints.size());
float rgbToDepthFactorX = 1.0f/(cameraModels[0].imageWidth()>0?cameraModels[0].imageWidth()/depth.cols:1); float rgbToDepthFactorX = 1.0f/(cameraModels[0].imageWidth()>0?cameraModels[0].imageWidth()/subImageWidth:1);
float rgbToDepthFactorY = 1.0f/(cameraModels[0].imageHeight()>0?cameraModels[0].imageHeight()/depth.rows:1); float rgbToDepthFactorY = 1.0f/(cameraModels[0].imageHeight()>0?cameraModels[0].imageHeight()/depth.rows:1);
for(unsigned int i=0; i!=keypoints.size(); ++i) for(unsigned int i=0; i<keypoints.size(); ++i)
{ {
float x = keypoints[i].pt.x*rgbToDepthFactorX; float x = keypoints[i].pt.x*rgbToDepthFactorX;
float y = keypoints[i].pt.y*rgbToDepthFactorY; float y = keypoints[i].pt.y*rgbToDepthFactorY;
int cameraIndex = int(x / subImageWidth); int cameraIndex = int(x / subImageWidth);
UASSERT_MSG(cameraIndex < (int)cameraModels.size(), UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)cameraModels.size(),
uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)", uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
cameraIndex, (int)cameraModels.size(), keypoints[i].pt.x, subImageWidth, cameraModels[0].imageWidth()).c_str()); cameraIndex, (int)cameraModels.size(), keypoints[i].pt.x, subImageWidth, cameraModels[0].imageWidth()).c_str());
pcl::PointXYZ ptXYZ = util3d::projectDepthTo3D( pcl::PointXYZ ptXYZ = util3d::projectDepthTo3D(
depth, cameraModels.size()==1?depth:cv::Mat(depth, cv::Range::all(), cv::Range(subImageWidth*cameraIndex,subImageWidth*(cameraIndex+1))),
x-subImageWidth*cameraIndex, x-subImageWidth*cameraIndex,
y, y,
cameraModels.at(cameraIndex).cx()*rgbToDepthFactorX, cameraModels.at(cameraIndex).cx()*rgbToDepthFactorX,