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/OdometryViso2.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_VISO2
00036 #include <viso_stereo.h>
00037
00038 double computeFeatureFlow(const std::vector<Matcher::p_match>& matches)
00039 {
00040 double total_flow = 0.0;
00041 for (size_t i = 0; i < matches.size(); ++i)
00042 {
00043 double x_diff = matches[i].u1c - matches[i].u1p;
00044 double y_diff = matches[i].v1c - matches[i].v1p;
00045 total_flow += sqrt(x_diff * x_diff + y_diff * y_diff);
00046 }
00047 return total_flow / matches.size();
00048 }
00049 #endif
00050
00051 namespace rtabmap {
00052
00053 OdometryViso2::OdometryViso2(const ParametersMap & parameters) :
00054 Odometry(parameters),
00055 #ifdef RTABMAP_VISO2
00056 viso2_(0),
00057 ref_frame_change_method_(0),
00058 ref_frame_inlier_threshold_(Parameters::defaultOdomVisKeyFrameThr()),
00059 ref_frame_motion_threshold_(5.0),
00060 lost_(false),
00061 keep_reference_frame_(false),
00062 #endif
00063 reference_motion_(Transform::getIdentity())
00064 {
00065 #ifdef RTABMAP_VISO2
00066 Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), ref_frame_inlier_threshold_);
00067 #endif
00068 viso2Parameters_ = Parameters::filterParameters(parameters, "OdomViso2");
00069 }
00070
00071 OdometryViso2::~OdometryViso2()
00072 {
00073 #ifdef RTABMAP_VISO2
00074 delete viso2_;
00075 #endif
00076 }
00077
00078 void OdometryViso2::reset(const Transform & initialPose)
00079 {
00080 Odometry::reset(initialPose);
00081 #ifdef RTABMAP_VISO2
00082 if(viso2_)
00083 {
00084 delete viso2_;
00085 viso2_ = 0;
00086 }
00087 lost_ = false;
00088 reference_motion_.setIdentity();
00089 previousLocalTransform_.setNull();
00090 #endif
00091 }
00092
00093
00094 Transform OdometryViso2::computeTransform(
00095 SensorData & data,
00096 const Transform & guess,
00097 OdometryInfo * info)
00098 {
00099 Transform t;
00100 #ifdef RTABMAP_VISO2
00101
00102
00103 UTimer timer;
00104
00105 if(!data.depthRaw().empty())
00106 {
00107 UERROR("viso2 odometry doesn't support RGB-D data, only stereo. Aborting odometry update...");
00108 return t;
00109 }
00110
00111 if(data.imageRaw().empty() ||
00112 data.imageRaw().rows != data.rightRaw().rows ||
00113 data.imageRaw().cols != data.rightRaw().cols)
00114 {
00115 UERROR("Not compatible left (%dx%d) or right (%dx%d) image.",
00116 data.imageRaw().rows,
00117 data.imageRaw().cols,
00118 data.rightRaw().rows,
00119 data.rightRaw().cols);
00120 return t;
00121 }
00122
00123 if(!(data.stereoCameraModel().isValidForProjection() &&
00124 data.stereoCameraModel().left().isValidForReprojection() &&
00125 data.stereoCameraModel().right().isValidForReprojection()))
00126 {
00127 UERROR("Invalid stereo camera model!");
00128 return t;
00129 }
00130
00131 cv::Mat leftGray;
00132 if(data.imageRaw().type() == CV_8UC3)
00133 {
00134 cv::cvtColor(data.imageRaw(), leftGray, CV_BGR2GRAY);
00135 }
00136 else if(data.imageRaw().type() == CV_8UC1)
00137 {
00138 leftGray = data.imageRaw();
00139 }
00140 else
00141 {
00142 UFATAL("Not supported color type!");
00143 }
00144 cv::Mat rightGray;
00145 if(data.rightRaw().type() == CV_8UC3)
00146 {
00147 cv::cvtColor(data.rightRaw(), rightGray, CV_BGR2GRAY);
00148 }
00149 else if(data.rightRaw().type() == CV_8UC1)
00150 {
00151 rightGray = data.rightRaw();
00152 }
00153 else
00154 {
00155 UFATAL("Not supported color type!");
00156 }
00157
00158 int32_t dims[] = {leftGray.cols, leftGray.rows, leftGray.cols};
00159 cv::Mat covariance;
00160 if(viso2_ == 0)
00161 {
00162 VisualOdometryStereo::parameters params;
00163 params.base = params.match.base = data.stereoCameraModel().baseline();
00164 params.calib.cu = params.match.cu = data.stereoCameraModel().left().cx();
00165 params.calib.cv = params.match.cv = data.stereoCameraModel().left().cy();
00166 params.calib.f = params.match.f = data.stereoCameraModel().left().fx();
00167
00168 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2RansacIters(), params.ransac_iters);
00169 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2InlierThreshold(), params.inlier_threshold);
00170 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2Reweighting(), params.reweighting);
00171
00172 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchNmsN(), params.match.nms_n);
00173 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchNmsTau(), params.match.nms_tau);
00174 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchBinsize(), params.match.match_binsize);
00175 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchRadius(), params.match.match_radius);
00176 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchDispTolerance(), params.match.match_disp_tolerance);
00177 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchOutlierDispTolerance(), params.match.outlier_disp_tolerance);
00178 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchOutlierFlowTolerance(), params.match.outlier_flow_tolerance);
00179 bool multistage = Parameters::defaultOdomViso2MatchMultiStage();
00180 bool halfResolution = Parameters::defaultOdomViso2MatchHalfResolution();
00181 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchMultiStage(), multistage);
00182 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchHalfResolution() , halfResolution);
00183 params.match.multi_stage = multistage?1:0;
00184 params.match.half_resolution = halfResolution?1:0;
00185 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchRefinement(), params.match.refinement);
00186
00187 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2BucketMaxFeatures(), params.bucket.max_features);
00188 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2BucketWidth(), params.bucket.bucket_width);
00189 Parameters::parse(viso2Parameters_, Parameters::kOdomViso2BucketHeight(), params.bucket.bucket_height);
00190
00191 viso2_ = new VisualOdometryStereo(params);
00192
00193 viso2_->process(leftGray.data, rightGray.data, dims);
00194 t.setIdentity();
00195 covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
00196 }
00197 else
00198 {
00199 bool success = viso2_->process(leftGray.data, rightGray.data, dims, lost_ || keep_reference_frame_);
00200 if (success)
00201 {
00202 Matrix motionViso = Matrix::inv(viso2_->getMotion());
00203 Transform motion(motionViso.val[0][0], motionViso.val[0][1], motionViso.val[0][2],motionViso.val[0][3],
00204 motionViso.val[1][0], motionViso.val[1][1], motionViso.val[1][2],motionViso.val[1][3],
00205 motionViso.val[2][0], motionViso.val[2][1], motionViso.val[2][2], motionViso.val[2][3]);
00206 Transform camera_motion;
00207
00208 if(lost_ || keep_reference_frame_)
00209 {
00210 camera_motion = reference_motion_.inverse() * motion;
00211 }
00212 else
00213 {
00214 camera_motion = motion;
00215 }
00216 reference_motion_ = motion;
00217
00218 t=camera_motion;
00219
00220
00221 covariance = cv::Mat::eye(6,6, CV_64FC1);
00222 covariance.at<double>(0,0) = 0.002;
00223 covariance.at<double>(1,1) = 0.002;
00224 covariance.at<double>(2,2) = 0.05;
00225 covariance.at<double>(3,3) = 0.09;
00226 covariance.at<double>(4,4) = 0.09;
00227 covariance.at<double>(5,5) = 0.09;
00228
00229 lost_=false;
00230 }
00231 else
00232 {
00233 covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
00234 lost_ = true;
00235 }
00236
00237 if(success)
00238 {
00239
00240 if(ref_frame_change_method_==1)
00241 {
00242
00243 double feature_flow = computeFeatureFlow(viso2_->getMatches());
00244 keep_reference_frame_ = (feature_flow < ref_frame_motion_threshold_);
00245 }
00246 else
00247 {
00248 keep_reference_frame_ = ref_frame_inlier_threshold_==0 || viso2_->getNumberOfInliers() > ref_frame_inlier_threshold_;
00249 }
00250 }
00251 else
00252 {
00253 keep_reference_frame_ = false;
00254 }
00255 }
00256
00257 const Transform & localTransform = data.stereoCameraModel().localTransform();
00258 if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
00259 {
00260
00261 if(!previousLocalTransform_.isNull())
00262 {
00263 t = previousLocalTransform_ * t * localTransform.inverse();
00264 }
00265 else
00266 {
00267 t = localTransform * t * localTransform.inverse();
00268 }
00269 previousLocalTransform_ = localTransform;
00270 }
00271
00272 if(info)
00273 {
00274 info->type = (int)kTypeViso2;
00275 info->keyFrameAdded = !keep_reference_frame_;
00276 info->reg.matches = viso2_->getNumberOfMatches();
00277 info->reg.inliers = viso2_->getNumberOfInliers();
00278 if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
00279 {
00280 info->reg.covariance = covariance;
00281 }
00282
00283 if(this->isInfoDataFilled())
00284 {
00285 std::vector<Matcher::p_match> matches = viso2_->getMatches();
00286 info->refCorners.resize(matches.size());
00287 info->newCorners.resize(matches.size());
00288 info->cornerInliers.resize(matches.size());
00289 for (size_t i = 0; i < matches.size(); ++i)
00290 {
00291 info->refCorners[i].x = matches[i].u1p;
00292 info->refCorners[i].y = matches[i].v1p;
00293 info->newCorners[i].x = matches[i].u1c;
00294 info->newCorners[i].y = matches[i].v1c;
00295 info->cornerInliers[i] = i;
00296 }
00297 }
00298 }
00299
00300 UINFO("Odom update time = %fs lost=%s", timer.elapsed(), lost_?"true":"false");
00301
00302 #else
00303 UERROR("RTAB-Map is not built with VISO2 support! Select another visual odometry approach.");
00304 #endif
00305 return t;
00306 }
00307
00308 }