string utf16_to_utf8( const wstring& strSrc )
{
string strTmp;
const int nUtf8Len = WideCharToMultiByte(CP_UTF8, 0, strSrc.c_str(), static_cast<int>(strSrc.size()), NULL, 0, NULL, NULL);
vector<char> vecTmp(nUtf8Len + 1);
WideCharToMultiByte(CP_UTF8, 0, strSrc.c_str(), static_cast<int>(strSrc.size()), &vecTmp[0], nUtf8Len, NULL, NULL);
strTmp.assign(vecTmp.begin(), vecTmp.end() - 1);
return strTmp;
}
wstring utf8_to_utf16( const string& strSrc )
{
wstring strTmp;
const int nWcsLen = MultiByteToWideChar(CP_UTF8, 0, strSrc.c_str(), static_cast<int>(strSrc.size()), NULL, 0);
vector<wchar_t> vecTmp(nWcsLen + 1);
MultiByteToWideChar(CP_UTF8, 0, strSrc.c_str(), static_cast<int>(strSrc.size()), &vecTmp[0], nWcsLen);
strTmp.assign(vecTmp.begin(), vecTmp.end() - 1);
return strTmp;
}
//-----------------------------------------------------------------------------
// Purpose:
//-----------------------------------------------------------------------------
void CVRADDispColl::CalcSampleRadius2AndBox( dface_t *pFace )
{
// Get the luxel sample size.
texinfo_t *pTexInfo = &texinfo[pFace->texinfo];
Assert ( pTexInfo );
if ( !pTexInfo )
return;
// Todo: Width = Height now, should change all the code to look at one value.
Vector vecTmp( pTexInfo->lightmapVecsLuxelsPerWorldUnits[0][0],
pTexInfo->lightmapVecsLuxelsPerWorldUnits[0][1],
pTexInfo->lightmapVecsLuxelsPerWorldUnits[0][2] );
float flWidth = 1.0f / VectorLength( vecTmp );
float flHeight = flWidth;
// Save off the sample width and height.
m_flSampleWidth = flWidth;
m_flSampleHeight = flHeight;
// Calculate the sample radius squared.
float flSampleRadius = sqrt( ( ( flWidth * flWidth ) + ( flHeight * flHeight ) ) ) * 2.2f;//RADIALDIST2;
if ( flSampleRadius > g_flMaxDispSampleSize )
{
flSampleRadius = g_flMaxDispSampleSize;
}
m_flSampleRadius2 = flSampleRadius * flSampleRadius;
// Calculate the patch radius - the max sample edge length * the number of luxels per edge "chop."
float flSampleSize = MAX( m_flSampleWidth, m_flSampleHeight );
float flPatchSampleRadius = flSampleSize * dispchop * 2.2f;
if ( flPatchSampleRadius > g_MaxDispPatchRadius )
{
flPatchSampleRadius = g_MaxDispPatchRadius;
Warning( "Patch Sample Radius Clamped!\n" );
}
m_flPatchSampleRadius2 = flPatchSampleRadius * flPatchSampleRadius;
}
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;
}
// Setup a CCoreDispInfo given a mapdispinfo_t.
// If pFace is non-NULL, then lightmap texture coordinates will be generated.
void DispMapToCoreDispInfo( mapdispinfo_t *pMapDisp, CCoreDispInfo *pCoreDispInfo, dface_t *pFace, int *pSwappedTexInfos )
{
winding_t *pWinding = pMapDisp->face.originalface->winding;
Assert( pWinding->numpoints == 4 );
//
// set initial surface data
//
CCoreDispSurface *pSurf = pCoreDispInfo->GetSurface();
texinfo_t *pTexInfo = &texinfo[ pMapDisp->face.texinfo ];
Assert( pTexInfo != NULL );
// init material contents
pMapDisp->contents = pMapDisp->face.contents;
if (!(pMapDisp->contents & (ALL_VISIBLE_CONTENTS | CONTENTS_PLAYERCLIP|CONTENTS_MONSTERCLIP) ) )
{
pMapDisp->contents |= CONTENTS_SOLID;
}
pSurf->SetContents( pMapDisp->contents );
// Calculate the lightmap coordinates.
Vector2D tCoords[4] = {Vector2D(0,0),Vector2D(0,1),Vector2D(1,0),Vector2D(1,1)};
if( pFace )
{
Assert( pFace->numedges == 4 );
Vector pt[4];
for( int i=0; i < 4; i++ )
pt[i] = pWinding->p[i];
int zeroOffset[2] = {0,0};
CalcTextureCoordsAtPoints(
pTexInfo->textureVecsTexelsPerWorldUnits,
zeroOffset,
pt,
4,
tCoords );
}
//
// set face point data ...
//
pSurf->SetPointCount( 4 );
for( int i = 0; i < 4; i++ )
{
// position
pSurf->SetPoint( i, pWinding->p[i] );
pSurf->SetTexCoord( i, tCoords[i] );
}
// reset surface given start info
pSurf->SetPointStart( pMapDisp->startPosition );
pSurf->FindSurfPointStartIndex();
pSurf->AdjustSurfPointData();
// Set the luxel coordinates on the base displacement surface.
Vector vecTmp( pTexInfo->lightmapVecsLuxelsPerWorldUnits[0][0],
pTexInfo->lightmapVecsLuxelsPerWorldUnits[0][1],
pTexInfo->lightmapVecsLuxelsPerWorldUnits[0][2] );
int nLuxelsPerWorldUnit = static_cast<int>( 1.0f / VectorLength( vecTmp ) );
Vector vecU( pTexInfo->lightmapVecsLuxelsPerWorldUnits[0][0],
pTexInfo->lightmapVecsLuxelsPerWorldUnits[0][1],
pTexInfo->lightmapVecsLuxelsPerWorldUnits[0][2] );
Vector vecV( pTexInfo->lightmapVecsLuxelsPerWorldUnits[1][0],
pTexInfo->lightmapVecsLuxelsPerWorldUnits[1][1],
pTexInfo->lightmapVecsLuxelsPerWorldUnits[1][2] );
bool bSwap = pSurf->CalcLuxelCoords( nLuxelsPerWorldUnit, false, vecU, vecV );
// Set the face m_LightmapExtents
if ( pFace )
{
pFace->m_LightmapTextureSizeInLuxels[0] = pSurf->GetLuxelU();
pFace->m_LightmapTextureSizeInLuxels[1] = pSurf->GetLuxelV();
if ( bSwap )
{
if ( pSwappedTexInfos[ pMapDisp->face.texinfo ] < 0 )
{
// Create a new texinfo to hold the swapped data.
// We must do this because other surfaces may want the non-swapped data
// This fixes a lighting bug in d2_prison_08 where many non-displacement surfaces
// were pitch black, in addition to bugs in other maps I bet.
// NOTE: Copy here because adding a texinfo could realloc.
texinfo_t temp = *pTexInfo;
memcpy( temp.lightmapVecsLuxelsPerWorldUnits[0], pTexInfo->lightmapVecsLuxelsPerWorldUnits[1], 4 * sizeof(float) );
memcpy( temp.lightmapVecsLuxelsPerWorldUnits[1], pTexInfo->lightmapVecsLuxelsPerWorldUnits[0], 4 * sizeof(float) );
temp.lightmapVecsLuxelsPerWorldUnits[1][0] *= -1.0f;
temp.lightmapVecsLuxelsPerWorldUnits[1][1] *= -1.0f;
temp.lightmapVecsLuxelsPerWorldUnits[1][2] *= -1.0f;
temp.lightmapVecsLuxelsPerWorldUnits[1][3] *= -1.0f;
pSwappedTexInfos[ pMapDisp->face.texinfo ] = texinfo.AddToTail( temp );
}
pMapDisp->face.texinfo = pSwappedTexInfos[ pMapDisp->face.texinfo ];
}
// NOTE: This is here to help future-proof code, since there are codepaths where
// pTexInfo can be made invalid (texinfo.AddToTail above).
pTexInfo = NULL;
}
// Setup the displacement vectors and offsets.
int size = ( ( ( 1 << pMapDisp->power ) + 1 ) * ( ( 1 << pMapDisp->power ) + 1 ) );
Vector vectorDisps[2048];
float dispDists[2048];
Assert( size < sizeof(vectorDisps)/sizeof(vectorDisps[0]) );
for( int j = 0; j < size; j++ )
{
Vector v;
float dist;
VectorScale( pMapDisp->vectorDisps[j], pMapDisp->dispDists[j], v );
VectorAdd( v, pMapDisp->vectorOffsets[j], v );
dist = VectorLength( v );
VectorNormalize( v );
vectorDisps[j] = v;
dispDists[j] = dist;
}
// Use CCoreDispInfo to setup the actual vertex positions.
pCoreDispInfo->InitDispInfo( pMapDisp->power, pMapDisp->minTess, pMapDisp->smoothingAngle,
pMapDisp->alphaValues, vectorDisps, dispDists );
pCoreDispInfo->Create();
}
