34 #include <opencv2/imgproc/types_c.h>
37 #include <viso_stereo.h>
39 double computeFeatureFlow(
const std::vector<Matcher::p_match>& matches)
41 double total_flow = 0.0;
42 for (
size_t i = 0;
i <
matches.size(); ++
i)
46 total_flow +=
sqrt(x_diff * x_diff + y_diff * y_diff);
48 return total_flow /
matches.size();
58 ref_frame_change_method_(0),
59 ref_frame_inlier_threshold_(
Parameters::defaultOdomVisKeyFrameThr()),
60 ref_frame_motion_threshold_(5.0),
62 keep_reference_frame_(
false),
64 reference_motion_(
Transform::getIdentity())
67 Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), ref_frame_inlier_threshold_);
106 if(!
data.depthRaw().empty())
108 UERROR(
"viso2 odometry doesn't support RGB-D data, only stereo. Aborting odometry update...");
112 if(
data.imageRaw().empty() ||
113 data.imageRaw().rows !=
data.rightRaw().rows ||
114 data.imageRaw().cols !=
data.rightRaw().cols)
116 UERROR(
"Not compatible left (%dx%d) or right (%dx%d) image.",
117 data.imageRaw().rows,
118 data.imageRaw().cols,
119 data.rightRaw().rows,
120 data.rightRaw().cols);
124 if(!(
data.stereoCameraModels().size() == 1 &&
125 data.stereoCameraModels()[0].isValidForProjection()))
127 UERROR(
"Invalid stereo camera model!");
132 if(
data.imageRaw().type() == CV_8UC3)
134 cv::cvtColor(
data.imageRaw(), leftGray, CV_BGR2GRAY);
136 else if(
data.imageRaw().type() == CV_8UC1)
138 leftGray =
data.imageRaw();
142 UFATAL(
"Not supported color type!");
145 if(
data.rightRaw().type() == CV_8UC3)
147 cv::cvtColor(
data.rightRaw(), rightGray, CV_BGR2GRAY);
149 else if(
data.rightRaw().type() == CV_8UC1)
151 rightGray =
data.rightRaw();
155 UFATAL(
"Not supported color type!");
158 int32_t dims[] = {leftGray.cols, leftGray.rows, leftGray.cols};
162 VisualOdometryStereo::parameters
params;
164 params.calib.cu =
params.match.cu =
data.stereoCameraModels()[0].left().cx();
165 params.calib.cv =
params.match.cv =
data.stereoCameraModels()[0].left().cy();
179 bool multistage = Parameters::defaultOdomViso2MatchMultiStage();
180 bool halfResolution = Parameters::defaultOdomViso2MatchHalfResolution();
183 params.match.multi_stage = multistage?1:0;
184 params.match.half_resolution = halfResolution?1:0;
191 viso2_ =
new VisualOdometryStereo(
params);
193 viso2_->process(leftGray.data, rightGray.data, dims);
195 covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
199 bool success = viso2_->process(leftGray.data, rightGray.data, dims, lost_ || keep_reference_frame_);
202 Matrix motionViso = Matrix::inv(viso2_->getMotion());
203 Transform motion(motionViso.val[0][0], motionViso.val[0][1], motionViso.val[0][2],motionViso.val[0][3],
204 motionViso.val[1][0], motionViso.val[1][1], motionViso.val[1][2],motionViso.val[1][3],
205 motionViso.val[2][0], motionViso.val[2][1], motionViso.val[2][2], motionViso.val[2][3]);
208 if(lost_ || keep_reference_frame_)
214 camera_motion = motion;
221 covariance = cv::Mat::eye(6,6, CV_64FC1);
222 covariance.at<
double>(0,0) = 0.002;
223 covariance.at<
double>(1,1) = 0.002;
224 covariance.at<
double>(2,2) = 0.05;
225 covariance.at<
double>(3,3) = 0.09;
226 covariance.at<
double>(4,4) = 0.09;
227 covariance.at<
double>(5,5) = 0.09;
233 covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
240 if(ref_frame_change_method_==1)
243 double feature_flow = computeFeatureFlow(viso2_->getMatches());
244 keep_reference_frame_ = (feature_flow < ref_frame_motion_threshold_);
248 keep_reference_frame_ = ref_frame_inlier_threshold_==0 || viso2_->getNumberOfInliers() > ref_frame_inlier_threshold_;
253 keep_reference_frame_ =
false;
257 const Transform & localTransform =
data.stereoCameraModels()[0].localTransform();
258 if(!
t.isNull() && !
t.isIdentity() && !localTransform.
isIdentity() && !localTransform.
isNull())
267 t = localTransform *
t * localTransform.
inverse();
275 info->keyFrameAdded = !keep_reference_frame_;
276 info->reg.matches = viso2_->getNumberOfMatches();
277 info->reg.inliers = viso2_->getNumberOfInliers();
278 if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
280 info->reg.covariance = covariance;
285 std::vector<Matcher::p_match>
matches = viso2_->getMatches();
295 info->cornerInliers[
i] =
i;
303 UERROR(
"RTAB-Map is not built with VISO2 support! Select another visual odometry approach.");