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

fixed fatal error with OdometryMono and rgb color only is selected for a RGB-D driver

Browse Source
This commit is contained in:
matlabbe
2015-07-30 10:04:24 -04:00
parent eab4a68838
commit 38807bf12e
1 changed files with 33 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/OdometryMono.cpp
+33 -30
View File
@@ -416,7 +416,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
UDEBUG("cameraTransform guess= %s (norm^2=%f)", cameraTransform.prettyPrint().c_str(), cameraTransform.getNormSquared()); UDEBUG("cameraTransform guess= %s (norm^2=%f)", cameraTransform.prettyPrint().c_str(), cameraTransform.getNormSquared());
if(cameraTransform.getNorm() < minTranslation_) if(cameraTransform.getNorm() < minTranslation_)
{ {
UWARN("Translation with the nearest frame is too small (%f<%f) to add new points to local map", UINFO("Translation with the nearest frame is too small (%f<%f) to add new points to local map",
cameraTransform.getNorm(), minTranslation_); cameraTransform.getNorm(), minTranslation_);
} }
else else
@@ -730,36 +730,39 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
if(refDepthOrRight_.type() == CV_8UC1) if(!refDepthOrRight_.empty())
{ {
newCorners3D = util3d::generateKeypoints3DStereo( if(refDepthOrRight_.type() == CV_8UC1)
refCorners, {
refS->sensorData().imageRaw(), newCorners3D = util3d::generateKeypoints3DStereo(
refDepthOrRight_, refCorners,
cameraModel.fx(), refS->sensorData().imageRaw(),
data.stereoCameraModel().baseline(), refDepthOrRight_,
cameraModel.cx(), cameraModel.fx(),
cameraModel.cy(), data.stereoCameraModel().baseline(),
Transform::getIdentity(), cameraModel.cx(),
stereoWinSize_, cameraModel.cy(),
stereoMaxLevel_, Transform::getIdentity(),
stereoIterations_, stereoWinSize_,
stereoEps_, stereoMaxLevel_,
stereoMaxSlope_ ); stereoIterations_,
} stereoEps_,
else if(refDepthOrRight_.type() == CV_32FC1 || refDepthOrRight_.type() == CV_16UC1) stereoMaxSlope_ );
{ }
std::vector<cv::KeyPoint> tmpKpts; else if(refDepthOrRight_.type() == CV_32FC1 || refDepthOrRight_.type() == CV_16UC1)
cv::KeyPoint::convert(refCorners, tmpKpts); {
CameraModel m(cameraModel.fx(), cameraModel.fy(), cameraModel.cx(), cameraModel.cy()); std::vector<cv::KeyPoint> tmpKpts;
newCorners3D = util3d::generateKeypoints3DDepth( cv::KeyPoint::convert(refCorners, tmpKpts);
tmpKpts, CameraModel m(cameraModel.fx(), cameraModel.fy(), cameraModel.cx(), cameraModel.cy());
refDepthOrRight_, newCorners3D = util3d::generateKeypoints3DDepth(
m); tmpKpts,
} refDepthOrRight_,
else if(!refDepthOrRight_.empty()) m);
{ }
UWARN("Depth or right image type not supported: %d", refDepthOrRight_.type()); else
{
UWARN("Depth or right image type not supported: %d", refDepthOrRight_.type());
}
} }
for(unsigned int i=0; i<cloud->size(); ++i) for(unsigned int i=0; i<cloud->size(); ++i)