void ImageViewer::layoutImage()
{
if(hasPicture()) {
calcScaleRatio();
calcTopLeft();
calcShift();
}
updateCursor();
}
double ScaleEstimator::computeScaleTransform() {
bool _isLogData = true ;
bool _isWriteInfomation = false;
if (CFG_bIsLogGlobal)
printf("=========Scale Compute=========\n");
//Need at least 3 Frames' Data
// Means 2 Motions' Data
if (ptrMotion[0] == NULL) return 0;
int pair2Cnt = ptrMotion[0]->getMapPairPointsRef().size();
if (pair2Cnt < 10) return 0;
//////////////////////////////////////////////////////////////////////////
getPairPoints2();
cv::Mat transMask;
transMask = transformIn2Coord(pair2Cnt, 0,1);
double retScale = calcScaleRatio(0);
// 1.65/scale 变化量修正（速度变化修正）
//double ScaleDelta = 1.65 / retScale - 1.65 / preTransScale;
//if (preTransScale > 0 && retScale > 0 && std::abs(ScaleDelta) > CFG_dScaleInvIncreaseDiffLimit) {
// retScale = 1.65 / (1.65 / preTransScale + CFG_ScaleInvIncreaseDiffLimit*(ScaleDelta > 0 ? 1.0f : -1.0f));
//}
if (_isWriteInfomation) {
std::fstream fs;
int i0 = ptrMotion[0]->getIdxImg(0), i1 = ptrMotion[0]->getIdxImg(1);
fs.open(cv::format("./Velocity/m%06d_%06d.m", i0, i1), std::ios_base::out);
fs << cv::format("matR_%06d_%06d=", i0, i1) << ptrMotion[0]->getMatRRef() << ";" << std::endl;
fs << cv::format("matT_%06d%_%06d=", i0, i1) << ptrMotion[0]->getMatTRef() << ";" << std::endl;
fs << cv::format("mInter_%06d%_%06d=", i0, i1) << matIntersection << ";" << std::endl;
fs << "plot3(" << cv::format("mInter_%06d%_%06d", i0, i1) << "(1,:)'," << cv::format("mInter_%06d%_%06d", i0, i1) << "(2,:)'," << cv::format("mInter_%06d%_%06d", i0, i1) << "(3,:)','r.');" << std::endl;
fs.close();
}
return retScale;
}
double ScaleEstimator::calcScaleRatio(int flag) {
double retScale=0.0f;
int sizePoint = matIntersection.cols;
std::function<bool(const cv::Point3d&, const cv::Point3d&)> sortY =
[](const cv::Point3d& a, const cv::Point3d& b)->
bool { return a.y > b.y; };
if (flag == 0) {
//std::fstream fs;
//fs.open(cv::format("./Output/m%s_%d_%d.m", CFG_sDataName.c_str(), ptrMotion[0]->idxImg[0], ptrMotion[0]->idxImg[1]), std::ios_base::out);
//fs << "m=" << matIntersection.t() << ";" << std::endl;
//fs << " plot3(m(:,1)',m(:,2)',m(:,3)','r.'); hold on;" << std::endl;
//最普通的方法, 取y最大
retScale = -100.0f;
std::vector<cv::Point3d> vecPoints;
for (int idxPoint = 0; idxPoint < sizePoint; idxPoint++) {
cv::Point3d vecTmp((cv::Vec<double, 3>)matIntersection.col(idxPoint));
//约束路面宽度在 7.0以内
if (vecTmp.y > 0 && std::abs(vecTmp.x) < 6.0f )
vecPoints.push_back(vecTmp);
}
sizePoint = vecPoints.size();
std::sort(vecPoints.begin(), vecPoints.end(), sortY);
/* printf("========Y list========\ng=[");
for (int i = 0; i < sizePoint - 2; i++) {
printf("%f,%f,%f;\n", vecPoints[i].x, vecPoints[i].y, vecPoints[i].z);
}
printf("]\nplot3(g(:,1),g(:,2),g(:,3),'r.');\n");
printf("========Y list========END\n");*/
for (int i = 0; i < sizePoint - 2; i++) {
if (std::abs(vecPoints[i].y - vecPoints[i + 1].y) / vecPoints[i].y < 0.06f &&
std::abs(vecPoints[i+1].y - vecPoints[i + 2].y) / vecPoints[i+1].y < 0.06f
) {
cv::Point3d normal = vecPoints[i].cross(vecPoints[i + 1]);
normal.x /= cv::norm(normal);
normal.y /= cv::norm(normal);
normal.z /= cv::norm(normal);
normal = normal.y < 0 ? -normal : normal;
retScale = std::abs(vecPoints[i].dot(normal));
break;
}
}
//if (retScale < 0)
for (int i = 0; i < sizePoint - 1; i++) {
if (std::abs(vecPoints[i].y - vecPoints[i + 1].y) / vecPoints[i].y < 0.1f
) {
retScale = vecPoints[i].y;
break;
}
}
}
else {
//采用libviso2 的 bsetPlane方法，
double motion_threshold = 1.0f;
//然后再 选一个 median
std::vector<double> distSort;
for (int idx = 0; idx < sizePoint; idx++) {
double _dist = 0;
for (int j = 0; j < 3; j++)
_dist += std::abs(matIntersection.at<double>(j, idx));
distSort.push_back(_dist);
}
std::sort(distSort.begin(), distSort.end());
int lenMedian = distSort.size() / 2;
double median = distSort[lenMedian-1];
double sigma = median / 50;
double weight = 1.0 / (2.0*sigma*sigma);
double best_sum = 0;
int best_idx = -1;
for (int idx = 0; idx < sizePoint; idx++) {
if (matIntersection.at<double>(1, idx) > 0) {
double sum = 0;
for (int jdx = 0; jdx < sizePoint; jdx++) {
double dist = matIntersection.at<double>(1, idx) - matIntersection.at<double>(1, jdx);
sum += std::exp(-dist*dist*weight);
}
if (sum > best_sum) {
best_sum = sum;
best_idx = idx;
}
}
}
//std::cout << matIntersection.t() << std::endl;
std::fstream fs;
fs.open(cv::format("./Output/m%d_%d.m", ptrMotion[0]->getIdxImg(0), ptrMotion[0]->getIdxImg(1)), std::ios_base::out);
fs << "m=" << matIntersection.t() << ";" <<std::endl;
fs << " plot3(m(:,1)',m(:,2)',m(:,3)','r.'); hold on;" << std::endl;
fs << cv::format("plot3(m(%d,1),m(%d,2),m(%d,3),'b*');", best_idx+1, best_idx+1, best_idx+1) << std::endl;
if (best_idx > -1) {
retScale = matIntersection.at<double>(1, best_idx);
double m2 = calcScaleRatio(0);
if (m2 > CFG_dScaleRatioLimitBottom)
retScale = (m2 + retScale) / 2;
printf("SCALE[%d]=%f ::: %f\n", best_idx, retScale, m2 );
return retScale;
}
else {
printf("计算失败\n");
return calcScaleRatio(0);
}
}
//else {
// //换一种，先按照y排序，然后三个三个 算平面，选择比较平的那些平面.
// // 三个点算 法向量 ，y轴取负
// //考虑当前的 matDir,选择与其 点乘 的double作为 map.first
// // 按照 map< double点乘 方向, vec<Point>> 存储
// //map从小到大排序，计算 点到平面距离，依次验证容忍度，
// // 还要加入对跳帧的容忍范围,单帧容忍 1.65/x 后的 0.2, 多帧 0.2+0.1*(n-1)
// std::vector<cv::Point3d> vecPoints;
//
// for (int idxPoint = 0; idxPoint < sizePoint; idxPoint++) {
// cv::Point3d vecTmp((cv::Vec<double, 3>)matIntersection.col(idxPoint));
// if (vecTmp.y > 0)
// vecPoints.push_back(vecTmp);
// }
// sizePoint = vecPoints.size();
// //排序y轴
//
// std::sort(vecPoints.begin(), vecPoints.end(), sortY);
// //printf("=====输出所有点=====\n");
// //for (auto&p : vecPoints) std::cout << p << std::endl;
// cv::Point3d pointDir((cv::Vec<double, 3>)matDir);
// std::map<double, double> mapDotDist;
// //std::map<double, std::vector<cv::Point3d>> mapDotPoint;
// //计算点乘结果, 并插入map
// //std::deque<cv::Point3d> cachePoint;
// for (int idxPoint = 0; idxPoint < sizePoint - 2; idxPoint++) {
// cv::Point3d normal = (vecPoints[idxPoint] - vecPoints[idxPoint + 1]).cross(vecPoints[idxPoint] - vecPoints[idxPoint+2]);
// //法向量取负
// normal = normal.y < 0.0f ? -normal : normal;
// normal.x /= cv::norm(normal);
// normal.y /= cv::norm(normal);
// normal.z /= cv::norm(normal);
// //printf("点%d-%d-%d\n法向量\n", idxPoint, idxPoint + 1, idxPoint + 2);
// //std::cout << normal << std::endl;
// double dotResult = normal.dot(pointDir);
// double dist = vecPoints[idxPoint].dot(normal);
// mapDotDist.insert(std::make_pair(dist, dotResult));
// //printf("dist=%f, dot=%f\n", dist, dotResult);
// //vecTmp.push
// /*while (cachePoint.size() >= 3){
// cachePoint.pop_front();
// }
// while (cachePoint.size() < 3){
// cachePoint.push_back(vecPoints[idxPoint + cachePoint.size()]);
// }
// mapDotPoint.insert(std::make_pair(dotResult, std::vector<cv::Point3d>(cachePoint.begin(), cachePoint.end())));*/
// }
// //map从大到小
// printf("Map 遍历\n");
// for (auto riter = mapDotDist.rbegin(); riter != mapDotDist.rend(); riter++) {
// double dist = riter->first,
// dot = riter->second;
// //printf("dist=%f, dot=%f\n", dist, dot);
// if (std::abs(dot) < 1 - std::cos(acos(-1) / 180.0f*5.0f)) {
// retScale = dist;
// return retScale;
// }
// }
// for (auto riter = mapDotDist.rbegin(); riter != mapDotDist.rend(); riter++) {
// double dist = riter->first,
// dot = riter->second;
// //printf("dist=%f, dot=%f\n", dist, dot);
// if (std::abs(dot) < 1 - std::cos(acos(-1) / 180.0f*10.0f)) {
// retScale = dist;
// break;
// }
// }
//}
return retScale;
}
