// 先从历史数据中选出 K 个最近参考帧 + ALL 关键帧进行 match
// 选取一个帧ptr 序列
int TrackRunner::runKeyStep() {
//printf("////////////////////////////////////////////\n");
printf("//////////// idxImgCur: %06d ////////////\n",idxImgCur);
//printf("////////////////////////////////////////////\n");
TIME_BEGIN("MainLoop");
TIME_BEGIN("FeatureDetect");
/** TODO: 可以考虑 多线程并行计算,然后在这里放个 wait
* 另外那个线程算好一个放个 signal
*/
// 读取当前帧数据
FeatureState* ptrCurFeature = new FeatureState(idxImgCur);
ptrCurFeature->detect(CFG_iMaxFeatures);
TIME_END("FeatureDetect");
//showImage(ptrCurFeature);
// 确定当前帧需要比较的 历史帧集合
std::vector<FeatureState*> vecKeyList;
/** TODO: 可能需要靠 视觉磁带模型预估closure */
vecKeyList = selectKeySequence(idxImgCur);
// 对每一对历史-当前帧 进行运动估算
std::vector<MotionState> vecCurMotions( vecKeyList.size() );
// 调用 FrameParser 分别处理 参考帧<->当前帧 的 matR和MatT
TIME_BEGIN(cv::format("Match&Motion[%d]", vecKeyList.size()));
#pragma omp parallel for
for (int idx = 0; idx < vecKeyList.size();idx++) {
FeatureState* ptrFeature = vecKeyList[idx];
/** TODO: 考虑对不同情况设置不同过滤规则 */
// 初始化 帧间处理器
FrameParser fparser(ptrFeature, ptrCurFeature);
fparser.match(CFG_dOpticalFlowThreshold);
// 对极几何 计算运动matR和matT
bool motionStatus = fparser.computeMotion(vecCurMotions[idx]);
vecEnableIdxs[idxImgCur] = cntRunOk;
// 运动参数计算失败
if (motionStatus == false) {
printf("运动参数计算失败\n");
//TODO: 解不出来默认 运动,然后 强制一个默认尺度
// 以上这句话只能用在 iDequeNumber == 1的时候，否则会有问题
//vecCurMotions[idx].setInited(true);
////按照CFG设置
//vecCurMotions[idx].setScale(1.65f / CFG_dScaleRatioErrorDefault, true);
//vecCurMotions[idx].errType.set(0);
continue;
}
// 位移旋转限值
if (CFG_bIsLimitRotationDiff && limitRotationDiff(vecCurMotions[idx],CFG_dRotationDiffLimit) == false) {
printf("旋转跳\n");
vecCurMotions[idx].setInited(false);
continue;
}
// 尺度估算开启
ScaleEstimator sEstimator;
sEstimator.updateMotion(&vecCurMotions[idx]);
double curScale = sEstimator.computeScaleTransform();
if (curScale <= 0) {
printf("尺度没算出来\n");
curScale = 200;
}
// 尺度增量限制
// 如果被跳帧了，跳帧后出现一个多间隔值，在这个间隔值之后，再允许任意一个。
// 如果连跳多帧，那
if (limitScaleDiff(vecCurMotions[idx], curScale, CFG_dScaleInvIncreaseDiffLimit) == false) {
vecCurMotions[idx].setInited( false );
continue;
}
int idxDelta = vecCurMotions[idx].getIdxImg(1) - vecCurMotions[idx].getIdxImg(0);
if (CFG_iPreAverageFilter > 0) {
auto& qDist = cDrawer.qDist;
auto iter = qDist.rbegin();
double aver = 0.0f;
int cnt = 0;
for (; cnt < CFG_iPreAverageFilter && iter != qDist.rend(); iter++, cnt++) {
printf("%d %f\n", idxImgCur, *iter);
aver += *iter;
}
aver += 1.65f / curScale / idxDelta;
aver /= cnt + 1;
//可以做限制 只限制上升或者只限制下降
//if (aver < 1.65f / curScale / idxDelta) {
// curScale = 1.65 / aver / idxDelta;
//}
}
/** TODO: 这里需要一个合适的尺度评价 */
if ((curScale*idxDelta < CFG_dScaleRatioLimitBottom /*/ idxDelta*/ || curScale*idxDelta > CFG_dScaleRatioLimitTop)) {
printf("Scale LIMIT %d-%d:%f %f\n", vecCurMotions[idx].getIdxImg(0), vecCurMotions[idx].getIdxImg(1), curScale, 1.65f / curScale);
if (curScale*idxDelta < CFG_dScaleRatioLimitBottom) {
curScale = CFG_dScaleRatioLimitBottom / idxDelta;
vecCurMotions[idx].errType.set(Const::Error::LimitSCALEPEAK);
}
else {
curScale = CFG_dScaleRatioLimitTop / idxDelta;
vecCurMotions[idx].errType.set(Const::Error::LimitSCALEPEAK);
}
}
// 目前使用 GroundTruth的 运动尺度，因此目前误差只在旋转角度上
if (CFG_bIsUseGroundTruthDistance) {
vecCurMotions[idx].setScale(1.65f / cv::norm(pHelper.getPosition(vecCurMotions[idx].getIdxImg(1), vecCurMotions[idx].getIdxImg(0))));
//vecCurMotions[idx].scale = 1.65f / cv::norm(pVisio.getPosition(vecCurMotions[idx].idxImg[1], vecCurMotions[idx].idxImg[0]));
}
else {
vecCurMotions[idx].setScale(curScale);
}
//vecCurMotions[idx]._matR_ = vecCurMotions[idx].matR * vecPoses[vecCurMotions[idx].idxImg[0]].dir3;
}
TIME_END(cv::format("Match&Motion[%d]", vecKeyList.size()));
if ( idxImgCur <= 2 || vecEnableIdxs[idxImgCur-1] > 0) {
curError.set(0);
}
else {
printf("Pre Error: %d\n", curError.get());
}
for (auto& motion : vecCurMotions) {
curError.set(motion.errType, false);
}
//按照一定规则过滤 vecCurMotions,目前先过滤掉 inited为false的
int cntValid = filterMotions(vecCurMotions,0);
// 如果过滤完没有了, 就跳帧
if (cntValid < 1) {
delete ptrCurFeature;
vecEnableIdxs[idxImgCur] = 0;
}
else {
/** TODO: 对算出来的这么多有效的, 1是要判定closure; 2是要综合(取平均哈哈哈)运动状态 */
TIME_BEGIN("PoseEstimate");
// 对于计算出来的每对Motion, 应用该运动
std::vector<PoseState> vecEstiPoses;
for (int idx = 0; idx < vecCurMotions.size(); idx++) {
MotionState& curMotion = vecCurMotions[idx];
if (curMotion.getInited() == true) {
// 对vecPose的对应位姿,应用Motion得到 vecEstiPoses新位姿
vecEstiPoses.push_back(vecPoses[curMotion.getIdxImg(0)].move(curMotion));
//加入骨架
vecMotionLinks[curMotion.getIdxImg(1)].insert(std::make_pair(curMotion.getIdxImg(0), curMotion));
}
}
/** TODO: 新的多位姿选择方法 */
PoseState curPoseState = PoseState::calcAverage(vecEstiPoses);
curPoseState.errType.set(curError);
// 更新队列和关键帧
updateKeyList(ptrCurFeature);
cntRunOk++;
vecPoses[idxImgCur] = curPoseState;
TIME_END("PoseEstimate");
//if (CFG_bIsLogGlobal)
std::cout << vecPoses[idxImgCur] << std::endl;
// 画布画出当前位姿, 以及GroundTruth实际路径
// 内部修改 vecPoses 的数据，不能用 curPoseState 了
cDrawer.drawCanvas(vecPoses[idxImgCur]);
/*
* 本来用于测试 梯度下降优化方法，在计算完50个点之后执行
* 现废弃
*/
if (false && idxImgCur == 50) {
printf("开始调整\n");
Skeleton skl;
skl.initData(vecPoses, vecMotionLinks);
double err = skl.calcError();
double preErr = err;
printf("i=%d err=%f\n", -1, err);
if (CFG_bIsLogGlobal)
std::cout << skl.vecX[0] << std::endl;
if (CFG_bIsLogGlobal)
std::cout << skl.vecX[1] << std::endl;
for (int i = 0; i < 1000; i++) {
skl.calcDiff();
skl.merge(1e-3);
skl.fixMatrix();
err = skl.calcError();
printf("i=%d err=%f\n", i, err);
if (CFG_bIsLogGlobal)
std::cout << skl.vecX[0] << std::endl;
if (CFG_bIsLogGlobal)
std::cout << skl.vecX[1] << std::endl;
if (preErr< err) {
break;
}
preErr = err;
}
for (int i = idxImgBegin + 1; i < idxImgCur; i++) {
if ( skl.vecX[i].rows) {
PoseState t(i);
t.setInited( true );
t.pos.x = skl.vecX[i].at<double>(0, 0);
t.pos.y = skl.vecX[i].at<double>(1, 0);
t.pos.z = skl.vecX[i].at<double>(2, 0);
if (CFG_bIsLogGlobal)
std::cout << t.pos << std::endl;
}
}
cv::waitKey();
printf("结束调整\n");
}
}
TIME_END("MainLoop");
return ++idxImgCur;
}
