00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/core/OdometryFovis.h"
00029 #include "rtabmap/core/OdometryInfo.h"
00030 #include "rtabmap/core/util2d.h"
00031 #include "rtabmap/utilite/ULogger.h"
00032 #include "rtabmap/utilite/UTimer.h"
00033 #include "rtabmap/utilite/UStl.h"
00034
00035 #ifdef RTABMAP_FOVIS
00036 #include <libfovis/fovis.hpp>
00037 #endif
00038
00039 namespace rtabmap {
00040
00041 OdometryFovis::OdometryFovis(const ParametersMap & parameters) :
00042 Odometry(parameters)
00043 #ifdef RTABMAP_FOVIS
00044 ,
00045 fovis_(0),
00046 rect_(0),
00047 stereoCalib_(0),
00048 depthImage_(0),
00049 stereoDepth_(0),
00050 lost_(false)
00051 #endif
00052 {
00053 fovisParameters_ = Parameters::filterParameters(parameters, "OdomFovis");
00054 if(parameters.find(Parameters::kOdomVisKeyFrameThr()) != parameters.end())
00055 {
00056 fovisParameters_.insert(*parameters.find(Parameters::kOdomVisKeyFrameThr()));
00057 }
00058 }
00059
00060 OdometryFovis::~OdometryFovis()
00061 {
00062 #ifdef RTABMAP_FOVIS
00063 delete fovis_;
00064 delete rect_;
00065 delete stereoCalib_;
00066 delete depthImage_;
00067 delete stereoDepth_;
00068 #endif
00069 }
00070
00071 void OdometryFovis::reset(const Transform & initialPose)
00072 {
00073 Odometry::reset(initialPose);
00074 #ifdef RTABMAP_FOVIS
00075 if(fovis_)
00076 {
00077 delete fovis_;
00078 fovis_ = 0;
00079 }
00080 if(rect_)
00081 {
00082 delete rect_;
00083 rect_ = 0;
00084 }
00085 if(stereoCalib_)
00086 {
00087 delete stereoCalib_;
00088 stereoCalib_ = 0;
00089 }
00090 if(depthImage_)
00091 {
00092 delete depthImage_;
00093 depthImage_ = 0;
00094 }
00095 if(stereoDepth_)
00096 {
00097 delete stereoDepth_;
00098 stereoDepth_ = 0;
00099 }
00100 lost_ = false;
00101 previousLocalTransform_.setNull();
00102 #endif
00103 }
00104
00105
00106 Transform OdometryFovis::computeTransform(
00107 SensorData & data,
00108 const Transform & guess,
00109 OdometryInfo * info)
00110 {
00111 UDEBUG("");
00112 Transform t;
00113
00114 #ifdef RTABMAP_FOVIS
00115 UTimer timer;
00116
00117 if(data.imageRaw().empty() ||
00118 data.imageRaw().rows != data.depthOrRightRaw().rows ||
00119 data.imageRaw().cols != data.depthOrRightRaw().cols)
00120 {
00121 UERROR("Not supported input!");
00122 return t;
00123 }
00124
00125 if(!((data.cameraModels().size() == 1 &&
00126 data.cameraModels()[0].isValidForReprojection()) ||
00127 (data.stereoCameraModel().isValidForProjection() &&
00128 data.stereoCameraModel().left().isValidForReprojection() &&
00129 data.stereoCameraModel().right().isValidForReprojection())))
00130 {
00131 UERROR("Invalid camera model! Mono cameras=%d (reproj=%d), Stereo camera=%d (reproj=%d|%d)",
00132 (int)data.cameraModels().size(),
00133 data.cameraModels().size() && data.cameraModels()[0].isValidForReprojection()?1:0,
00134 data.stereoCameraModel().isValidForProjection()?1:0,
00135 data.stereoCameraModel().left().isValidForReprojection()?1:0,
00136 data.stereoCameraModel().right().isValidForReprojection()?1:0);
00137 return t;
00138 }
00139
00140 cv::Mat gray;
00141 if(data.imageRaw().type() == CV_8UC3)
00142 {
00143 cv::cvtColor(data.imageRaw(), gray, CV_BGR2GRAY);
00144 }
00145 else if(data.imageRaw().type() == CV_8UC1)
00146 {
00147 gray = data.imageRaw();
00148 }
00149 else
00150 {
00151 UFATAL("Not supported color type!");
00152 }
00153
00154 fovis::VisualOdometryOptions options;
00155 if(fovis_ == 0 || (data.cameraModels().size() != 1 && stereoDepth_ == 0))
00156 {
00157 options = fovis::VisualOdometry::getDefaultOptions();
00158
00159 ParametersMap defaults = Parameters::getDefaultParameters("OdomFovis");
00160 options["feature-window-size"] = uValue(fovisParameters_, Parameters::kOdomFovisFeatureWindowSize(), defaults.at(Parameters::kOdomFovisFeatureWindowSize()));
00161 options["max-pyramid-level"] = uValue(fovisParameters_, Parameters::kOdomFovisMaxPyramidLevel(), defaults.at(Parameters::kOdomFovisMaxPyramidLevel()));
00162 options["min-pyramid-level"] = uValue(fovisParameters_, Parameters::kOdomFovisMinPyramidLevel(), defaults.at(Parameters::kOdomFovisMinPyramidLevel()));
00163 options["target-pixels-per-feature"] = uValue(fovisParameters_, Parameters::kOdomFovisTargetPixelsPerFeature(), defaults.at(Parameters::kOdomFovisTargetPixelsPerFeature()));
00164 options["fast-threshold"] = uValue(fovisParameters_, Parameters::kOdomFovisFastThreshold(), defaults.at(Parameters::kOdomFovisFastThreshold()));
00165 options["use-adaptive-threshold"] = uValue(fovisParameters_, Parameters::kOdomFovisUseAdaptiveThreshold(), defaults.at(Parameters::kOdomFovisUseAdaptiveThreshold()));
00166 options["fast-threshold-adaptive-gain"] = uValue(fovisParameters_, Parameters::kOdomFovisFastThresholdAdaptiveGain(), defaults.at(Parameters::kOdomFovisFastThresholdAdaptiveGain()));
00167 options["use-homography-initialization"] = uValue(fovisParameters_, Parameters::kOdomFovisUseHomographyInitialization(), defaults.at(Parameters::kOdomFovisUseHomographyInitialization()));
00168 options["ref-frame-change-threshold"] = uValue(fovisParameters_, Parameters::kOdomVisKeyFrameThr(), uNumber2Str(Parameters::defaultOdomVisKeyFrameThr()));
00169
00170
00171 options["use-bucketing"] = uValue(fovisParameters_, Parameters::kOdomFovisUseBucketing(), defaults.at(Parameters::kOdomFovisUseBucketing()));
00172 options["bucket-width"] = uValue(fovisParameters_, Parameters::kOdomFovisBucketWidth(), defaults.at(Parameters::kOdomFovisBucketWidth()));
00173 options["bucket-height"] = uValue(fovisParameters_, Parameters::kOdomFovisBucketHeight(), defaults.at(Parameters::kOdomFovisBucketHeight()));
00174 options["max-keypoints-per-bucket"] = uValue(fovisParameters_, Parameters::kOdomFovisMaxKeypointsPerBucket(), defaults.at(Parameters::kOdomFovisMaxKeypointsPerBucket()));
00175 options["use-image-normalization"] = uValue(fovisParameters_, Parameters::kOdomFovisUseImageNormalization(), defaults.at(Parameters::kOdomFovisUseImageNormalization()));
00176
00177
00178 options["inlier-max-reprojection-error"] = uValue(fovisParameters_, Parameters::kOdomFovisInlierMaxReprojectionError(), defaults.at(Parameters::kOdomFovisInlierMaxReprojectionError()));
00179 options["clique-inlier-threshold"] = uValue(fovisParameters_, Parameters::kOdomFovisCliqueInlierThreshold(), defaults.at(Parameters::kOdomFovisCliqueInlierThreshold()));
00180 options["min-features-for-estimate"] = uValue(fovisParameters_, Parameters::kOdomFovisMinFeaturesForEstimate(), defaults.at(Parameters::kOdomFovisMinFeaturesForEstimate()));
00181 options["max-mean-reprojection-error"] = uValue(fovisParameters_, Parameters::kOdomFovisMaxMeanReprojectionError(), defaults.at(Parameters::kOdomFovisMaxMeanReprojectionError()));
00182 options["use-subpixel-refinement"] = uValue(fovisParameters_, Parameters::kOdomFovisUseSubpixelRefinement(), defaults.at(Parameters::kOdomFovisUseSubpixelRefinement()));
00183 options["feature-search-window"] = uValue(fovisParameters_, Parameters::kOdomFovisFeatureSearchWindow(), defaults.at(Parameters::kOdomFovisFeatureSearchWindow()));
00184 options["update-target-features-with-refined"] = uValue(fovisParameters_, Parameters::kOdomFovisUpdateTargetFeaturesWithRefined(), defaults.at(Parameters::kOdomFovisUpdateTargetFeaturesWithRefined()));
00185
00186
00187 options["stereo-require-mutual-match"] = uValue(fovisParameters_, Parameters::kOdomFovisStereoRequireMutualMatch(), defaults.at(Parameters::kOdomFovisStereoRequireMutualMatch()));
00188 options["stereo-max-dist-epipolar-line"] = uValue(fovisParameters_, Parameters::kOdomFovisStereoMaxDistEpipolarLine(), defaults.at(Parameters::kOdomFovisStereoMaxDistEpipolarLine()));
00189 options["stereo-max-refinement-displacement"] = uValue(fovisParameters_, Parameters::kOdomFovisStereoMaxRefinementDisplacement(), defaults.at(Parameters::kOdomFovisStereoMaxRefinementDisplacement()));
00190 options["stereo-max-disparity"] = uValue(fovisParameters_, Parameters::kOdomFovisStereoMaxDisparity(), defaults.at(Parameters::kOdomFovisStereoMaxDisparity()));
00191 }
00192
00193 fovis::DepthSource * depthSource = 0;
00194 cv::Mat depth;
00195 cv::Mat right;
00196 Transform localTransform = Transform::getIdentity();
00197 if(data.cameraModels().size() == 1)
00198 {
00199 UDEBUG("");
00200 fovis::CameraIntrinsicsParameters rgb_params;
00201 memset(&rgb_params, 0, sizeof(fovis::CameraIntrinsicsParameters));
00202 rgb_params.width = data.cameraModels()[0].imageWidth();
00203 rgb_params.height = data.cameraModels()[0].imageHeight();
00204
00205 rgb_params.fx = data.cameraModels()[0].fx();
00206 rgb_params.fy = data.cameraModels()[0].fy();
00207 rgb_params.cx = data.cameraModels()[0].cx()==0.0?double(rgb_params.width) / 2.0:data.cameraModels()[0].cx();
00208 rgb_params.cy = data.cameraModels()[0].cy()==0.0?double(rgb_params.height) / 2.0:data.cameraModels()[0].cy();
00209 localTransform = data.cameraModels()[0].localTransform();
00210
00211 if(rect_ == 0)
00212 {
00213 UINFO("Init rgbd fovis: %dx%d fx=%f fy=%f cx=%f cy=%f", rgb_params.width, rgb_params.height, rgb_params.fx, rgb_params.fy, rgb_params.cx, rgb_params.cy);
00214 rect_ = new fovis::Rectification(rgb_params);
00215 }
00216
00217 if(depthImage_ == 0)
00218 {
00219 depthImage_ = new fovis::DepthImage(rgb_params, rgb_params.width, rgb_params.height);
00220 }
00221
00222 if(data.depthRaw().type() == CV_32FC1)
00223 {
00224 depth = data.depthRaw();
00225 for(int i=0; i<depth.rows; ++i)
00226 {
00227 for(int j=0; j<depth.cols; ++j)
00228 {
00229 float & d = depth.at<float>(i,j);
00230 if(d == 0.0f)
00231 {
00232 d = NAN;
00233 }
00234 }
00235 }
00236 }
00237 else if(data.depthRaw().type() == CV_16UC1)
00238 {
00239 depth = cv::Mat(data.depthRaw().size(), CV_32FC1);
00240 for(int i=0; i<data.depthRaw().rows; ++i)
00241 {
00242 for(int j=0; j<data.depthRaw().cols; ++j)
00243 {
00244 float d = float(data.depthRaw().at<unsigned short>(i,j))/1000.0f;
00245 depth.at<float>(i, j) = d==0.0f?NAN:d;
00246 }
00247 }
00248 }
00249 else
00250 {
00251 UFATAL("Unknown depth format!");
00252 }
00253 depthImage_->setDepthImage((float*)depth.data);
00254 depthSource = depthImage_;
00255 }
00256 else
00257 {
00258 UDEBUG("");
00259
00260 fovis::CameraIntrinsicsParameters left_parameters;
00261 left_parameters.width = data.stereoCameraModel().left().imageWidth();
00262 left_parameters.height = data.stereoCameraModel().left().imageHeight();
00263 left_parameters.fx = data.stereoCameraModel().left().fx();
00264 left_parameters.fy = data.stereoCameraModel().left().fy();
00265 left_parameters.cx = data.stereoCameraModel().left().cx()==0.0?double(left_parameters.width) / 2.0:data.stereoCameraModel().left().cx();
00266 left_parameters.cy = data.stereoCameraModel().left().cy()==0.0?double(left_parameters.height) / 2.0:data.stereoCameraModel().left().cy();
00267 localTransform = data.stereoCameraModel().localTransform();
00268
00269 if(rect_ == 0)
00270 {
00271 UINFO("Init stereo fovis: %dx%d fx=%f fy=%f cx=%f cy=%f", left_parameters.width, left_parameters.height, left_parameters.fx, left_parameters.fy, left_parameters.cx, left_parameters.cy);
00272 rect_ = new fovis::Rectification(left_parameters);
00273 }
00274
00275 if(stereoCalib_ == 0)
00276 {
00277
00278 fovis::CameraIntrinsicsParameters right_parameters;
00279 right_parameters.width = data.stereoCameraModel().right().imageWidth();
00280 right_parameters.height = data.stereoCameraModel().right().imageHeight();
00281 right_parameters.fx = data.stereoCameraModel().right().fx();
00282 right_parameters.fy = data.stereoCameraModel().right().fy();
00283 right_parameters.cx = data.stereoCameraModel().right().cx()==0.0?double(right_parameters.width) / 2.0:data.stereoCameraModel().right().cx();
00284 right_parameters.cy = data.stereoCameraModel().right().cy()==0.0?double(right_parameters.height) / 2.0:data.stereoCameraModel().right().cy();
00285
00286
00287
00288 fovis::StereoCalibrationParameters stereo_parameters;
00289 stereo_parameters.left_parameters = left_parameters;
00290 stereo_parameters.right_parameters = right_parameters;
00291 stereo_parameters.right_to_left_rotation[0] = 1.0;
00292 stereo_parameters.right_to_left_rotation[1] = 0.0;
00293 stereo_parameters.right_to_left_rotation[2] = 0.0;
00294 stereo_parameters.right_to_left_rotation[3] = 0.0;
00295 stereo_parameters.right_to_left_translation[0] = -data.stereoCameraModel().baseline();
00296 stereo_parameters.right_to_left_translation[1] = 0.0;
00297 stereo_parameters.right_to_left_translation[2] = 0.0;
00298
00299 stereoCalib_ = new fovis::StereoCalibration(stereo_parameters);
00300 }
00301
00302 if(stereoDepth_ == 0)
00303 {
00304 stereoDepth_ = new fovis::StereoDepth(stereoCalib_, options);
00305 }
00306 if(data.rightRaw().type() == CV_8UC3)
00307 {
00308 cv::cvtColor(data.rightRaw(), right, CV_BGR2GRAY);
00309 }
00310 else if(data.rightRaw().type() == CV_8UC1)
00311 {
00312 right = data.rightRaw();
00313 }
00314 else
00315 {
00316 UFATAL("Not supported color type!");
00317 }
00318 stereoDepth_->setRightImage(right.data);
00319 depthSource = stereoDepth_;
00320 }
00321
00322 if(fovis_ == 0)
00323 {
00324 fovis_ = new fovis::VisualOdometry(rect_, options);
00325 }
00326
00327 UDEBUG("");
00328 fovis_->processFrame(gray.data, depthSource);
00329
00330
00331 t = Transform::fromEigen3d(fovis_->getMotionEstimate());
00332
00333 cv::Mat covariance;
00334 fovis::MotionEstimateStatusCode statusCode = fovis_->getMotionEstimator()->getMotionEstimateStatus();
00335 if(statusCode > fovis::SUCCESS)
00336 {
00337 UWARN("Fovis error status: %s", fovis::MotionEstimateStatusCodeStrings[statusCode]);
00338 t.setNull();
00339 lost_ = true;
00340 covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
00341 previousLocalTransform_.setNull();
00342 }
00343 else if(lost_)
00344 {
00345 lost_ = false;
00346
00347 covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
00348 previousLocalTransform_.setNull();
00349 }
00350 else
00351 {
00352 const Eigen::MatrixXd& cov = fovis_->getMotionEstimator()->getMotionEstimateCov();
00353 if(cov.cols() == 6 && cov.rows() == 6 && cov(0,0) > 0.0)
00354 {
00355 covariance = cv::Mat::eye(6,6, CV_64FC1);
00356 memcpy(covariance.data, cov.data(), 36*sizeof(double));
00357 covariance *= 100.0;
00358 }
00359 }
00360
00361 if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
00362 {
00363
00364 if(!previousLocalTransform_.isNull())
00365 {
00366 t = previousLocalTransform_ * t * localTransform.inverse();
00367 }
00368 else
00369 {
00370 t = localTransform * t * localTransform.inverse();
00371 }
00372 previousLocalTransform_ = localTransform;
00373 }
00374
00375 if(info)
00376 {
00377 info->type = (int)kTypeFovis;
00378 info->keyFrameAdded = fovis_->getChangeReferenceFrames();
00379 info->features = fovis_->getTargetFrame()->getNumDetectedKeypoints();
00380 info->reg.matches = fovis_->getMotionEstimator()->getNumMatches();
00381 info->reg.inliers = fovis_->getMotionEstimator()->getNumInliers();
00382 info->reg.covariance = covariance;
00383
00384 if(this->isInfoDataFilled())
00385 {
00386 const fovis::FeatureMatch * matches = fovis_->getMotionEstimator()->getMatches();
00387 int numMatches = fovis_->getMotionEstimator()->getNumMatches();
00388 if(matches && numMatches>0)
00389 {
00390 info->refCorners.resize(numMatches);
00391 info->newCorners.resize(numMatches);
00392 info->cornerInliers.resize(numMatches);
00393 int oi=0;
00394 for (int i = 0; i < numMatches; ++i)
00395 {
00396 info->refCorners[i].x = matches[i].ref_keypoint->base_uv[0];
00397 info->refCorners[i].y = matches[i].ref_keypoint->base_uv[1];
00398 info->newCorners[i].x = matches[i].target_keypoint->base_uv[0];
00399 info->newCorners[i].y = matches[i].target_keypoint->base_uv[1];
00400 info->cornerInliers[oi++] = i;
00401 }
00402 info->cornerInliers.resize(oi);
00403 }
00404 }
00405 }
00406
00407 UINFO("Odom update time = %fs status=%s", timer.elapsed(), fovis::MotionEstimateStatusCodeStrings[statusCode]);
00408
00409 #else
00410 UERROR("RTAB-Map is not built with FOVIS support! Select another visual odometry approach.");
00411 #endif
00412 return t;
00413 }
00414
00415 }