41 keyFrameThr_(
Parameters::defaultOdomKeyFrameThr()),
42 visKeyFrameThr_(
Parameters::defaultOdomVisKeyFrameThr()),
43 scanKeyFrameThr_(
Parameters::defaultOdomScanKeyFrameThr())
76 if(!
data.rightRaw().empty() &&
77 (
data.stereoCameraModels().empty() || !
data.stereoCameraModels()[0].isValidForProjection()))
79 UERROR(
"Calibrated stereo camera required.");
82 if(!
data.depthRaw().empty() &&
83 (
data.cameraModels().empty() || !
data.cameraModels()[0].isValidForProjection()))
85 UERROR(
"Calibrated camera required.");
89 bool addKeyFrame =
false;
102 float maxCorrespondenceDistance = 0.0f;
103 float outlierRatio = 0.0f;
107 this->framesProcessed() < 2)
110 maxCorrespondenceDistance = Parameters::defaultIcpMaxCorrespondenceDistance();
111 outlierRatio = Parameters::defaultIcpOutlierRatio();
128 if(maxCorrespondenceDistance>0.0
f)
142 UWARN(
"Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.
prettyPrint().c_str());
144 int visCorTypeBackup = Parameters::defaultVisCorType();
146 if(visCorTypeBackup == 1)
159 if(visCorTypeBackup == 1)
168 UWARN(
"Trial with no guess still fail.");
172 UWARN(
"Trial with no guess succeeded.");
178 std::list<std::pair<int, std::pair<int, int> > > pairs;
180 info->refCorners.resize(pairs.size());
181 info->newCorners.resize(pairs.size());
182 std::map<int, int> idToIndex;
184 for(std::list<std::pair<
int, std::pair<int, int> > >::
iterator iter=pairs.begin();
190 idToIndex.insert(std::make_pair(
iter->first,
i));
227 regInfo.
covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
232 output = motionSinceLastKeyFrame.
inverse() * output;
242 UDEBUG(
"Update key frame");
281 if(features < registrationPipeline_->getMinVisualCorrespondences())
283 UWARN(
"Too low 2D features (%d), keeping last key frame...", features);
309 info->keyFrameAdded = addKeyFrame;
320 UINFO(
"Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",
322 output.
isNull()?
"true":
"false",
325 !output.
isNull()?
"true":
"false");