void Reconstruction::updateMesh( LaplacianMesh& resMesh )
{
const mat& paramMat = this->refMesh.GetParamMatrix();
mat matC = TraceManager::imageDatabase->getRefCamera().ReprojectPoints(reshape(c, refMesh.GetNCtrlPoints(), 2));
// mat matC = reshape(c, refMesh.GetNCtrlPoints(), 3);
// Update vertex coordinates
resMesh.SetVertexCoords(paramMat * matC);
// Resulting mesh yields a correct projection on image but it does not preserve lengths.
// So we need to compute the scale factor and multiply with matC
// Determine on which side the mesh lies: -Z or Z
double meanZ = mean(resMesh.GetVertexCoords().col(2));
int globalSign = meanZ > 0 ? 1 : -1;
const vec& resMeshEdgeLens = resMesh.ComputeEdgeLengths();
const vec& refMeshEdgeLens = refMesh.GetEdgeLengths();
// this seems no difference if do not scale the points?
double scale = globalSign * norm(refMeshEdgeLens, 2) / norm(resMeshEdgeLens, 2);
// Update vertex coordinates
resMesh.SetVertexCoords(scale * paramMat * matC);
}
void Reconstruction::ReconstructSoftConstr2D(const arma::vec& cInit, LaplacianMesh &resMesh) {
const mat& paramMat = refMesh.GetParamMatrix();
const mat& bigP = refMesh.GetBigParamMat();
if (cOptimal.is_empty()) {
cOptimal = reshape(refMesh.GetVertexCoords().rows(refMesh.GetCtrlPointIDs()), refMesh.GetNCtrlPoints()*3, 1 );
}
SoftConstrFunction2D function( bigP, this->GetBPwAP(), refMesh.GetEdges(), refMesh.GetEdgeLengths());
if ( this->usePrevFrameToInit && !isFirstFrame )
cOptimal = softConstrOptimize.OptimizeLagrange(cOptimal, function, resMesh);
else
cOptimal = softConstrOptimize.OptimizeLagrange(cInit, function, resMesh);
arma::mat cOptimalMat = arma::mat(reshape(cOptimal, refMesh.GetNCtrlPoints(), 3));
if ( mean(cOptimalMat.col(2)) < 0 ) { // Change the sign if the reconstruction is behind the camera. This happens because we take cOptimal as initial value for constrained optimization.
cOptimalMat = -cOptimalMat;
}
// Update vertex coordinates
resMesh.SetVertexCoords(paramMat*(cOptimalMat));
isFirstFrame = false;
}
void Reconstruction::ReconstructPlanarUnconstrOnce( const mat& matchesInit, LaplacianMesh& resMesh)
{
// Input check
if (matchesInit.n_rows == 0) {
return;
}
Timer timer;
double meanZ = mean(resMesh.GetVertexCoords().col(2));
// scale wr due to distance : 0 - 200
double wr = this->wrInit * (meanZ / 200.); // Currently used regularization weight
// double radius = this->radiusInit; // Currently used radius of the estimator
// vec reprojErrors; // Reprojection errors
// First, we need to build the correspondent matrix with all given matches to avoid re-computation
this->buildCorrespondenceMatrix(matchesInit);
// Then compute MPinit. Function reconstructPlanarUnconstr() will use part of MPinit w.r.t currently used matches
this->MPinit = this->Minit * this->refMesh.GetBigParamMat();
uvec matchesInitIdxs = linspace<uvec>(0, matchesInit.n_rows-1, matchesInit.n_rows);
// Currently used matches represented by their indices. Initially, use all matches: [0,1,2..n-1]
this->reconstructPlanarUnconstr(matchesInitIdxs, wr, resMesh);
}
void Reconstruction::reconstructPlanarUnconstr( const uvec& matchIdxs, double wr, LaplacianMesh& resMesh )
{
Timer timer;
const mat& paramMat = this->refMesh.GetParamMatrix(); // Parameterization matrix
// Build the matrix MPwAP = [MP; wr*AP] and compute: (MPwAP)' * (MPwAP)
this->computeCurrentMatrices( matchIdxs, wr);
// --------------- Eigen value decomposition --------------------------
mat V;
vec s;
timer.start();
// 此处最小的特征值只是其中一个解，在下面会对这个解进行Scale使得它的边长和ref mesh的边长相等
eig_sym(s, V, this->MPwAPtMPwAP);
timer.stop();
// cout << "Eigen(): " << timer.getElapsedTimeInMilliSec() << " ms"<< endl;
const vec& c = V.col(0);
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
mat matC = reshape(c, refMesh.GetNCtrlPoints(), 3);
// Update vertex coordinates
resMesh.SetVertexCoords(paramMat * matC);
// Resulting mesh yields a correct projection on image but it does not preserve lengths.
// So we need to compute the scale factor and multiply with matC
// Determine on which side the mesh lies: -Z or Z
double meanZ = mean(resMesh.GetVertexCoords().col(2));
int globalSign = meanZ > 0 ? 1 : -1;
const vec& resMeshEdgeLens = resMesh.ComputeEdgeLengths();
const vec& refMeshEdgeLens = refMesh.GetEdgeLengths();
double scale = globalSign * norm(refMeshEdgeLens, 2) / norm(resMeshEdgeLens, 2);
// Update vertex coordinates
resMesh.SetVertexCoords(scale * paramMat * matC);
}
void Reconstruction::ReconstructIneqConstr(const vec& cInit, LaplacianMesh& resMesh)
{
const mat& paramMat = refMesh.GetParamMatrix();
const mat& bigP = refMesh.GetBigParamMat();
static bool isFirstFrame = true;
static vec cOptimal = reshape(refMesh.GetVertexCoords().rows(refMesh.GetCtrlPointIDs()), refMesh.GetNCtrlPoints()*3, 1);
// Objective function: use MPwAP which was already computed in unconstrained reconstruction
ObjectiveFunction *objtFunction;
if (this->useTemporal && !isFirstFrame)
objtFunction = new ObjectiveFunction(this->GetMPwAP(), this->timeSmoothAlpha, cOptimal);
else
objtFunction = new ObjectiveFunction(this->GetMPwAP());
// Constrained function
IneqConstrFunction cstrFunction(bigP, refMesh.GetEdges(), refMesh.GetEdgeLengths());
if (this->usePrevFrameToInit && !isFirstFrame)
cOptimal = ineqConstrOptimize.OptimizeLagrange(cOptimal, *objtFunction, cstrFunction);
else
cOptimal = ineqConstrOptimize.OptimizeLagrange(cInit, *objtFunction, cstrFunction);
mat cOptimalMat = reshape(cOptimal, refMesh.GetNCtrlPoints(), 3);
if (cOptimalMat(0,2) < 0)
{
// Change the sign if the reconstruction is behind the camera.
// This happens because we take cOptimal as initial value for constrained optimization.
cOptimalMat = -cOptimalMat;
}
// Update vertex coordinates
resMesh.SetVertexCoords(paramMat*cOptimalMat);
isFirstFrame = false;
delete objtFunction;
}
void Reconstruction::ReconstructPlanarUnconstrIter( const mat& matchesInit, LaplacianMesh& resMesh, uvec& inlierMatchIdxs )
{
// Input check
if (matchesInit.n_rows == 0) {
inlierMatchIdxs.resize(0);
return;
}
Timer timer;
double wr = this->wrInit; // Currently used regularization weight
double radius = this->radiusInit; // Currently used radius of the estimator
vec reprojErrors; // Reprojection errors
// First, we need to build the correspondent matrix with all given matches to avoid re-computation
this->buildCorrespondenceMatrix(matchesInit);
// Then compute MPinit. Function reconstructPlanarUnconstr() will use part of MPinit w.r.t currently used matches
this->MPinit = this->Minit * this->refMesh.GetBigParamMat();
uvec matchesInitIdxs = linspace<uvec>(0, matchesInit.n_rows-1, matchesInit.n_rows);
// Currently used matches represented by their indices. Initially, use all matches: [0,1,2..n-1]
inlierMatchIdxs = matchesInitIdxs;
for (int i = 0; i < nUncstrIters; i++)
{
// If it is the final iteration, break and don't update "inlierMatchIdxs" or "weights", "radius"
if (i == nUncstrIters - 1) {
double meanZ = mean(resMesh.GetVertexCoords().col(2));
this->reconstructPlanarUnconstr(inlierMatchIdxs, this->wrInit * (meanZ / 200.), resMesh);
//cout << "Current radius: " << radius << endl;
//cout << "Current wr: " << wr << endl;
//Reconstruction::computeCurrentMatrices( currentMatchIdxs, 325 ); // For Fern
break;
} else {
this->reconstructPlanarUnconstr(inlierMatchIdxs, wr, resMesh);
}
// Otherwise, remove outliers
int iterTO = nUncstrIters - 2;
if (i >= iterTO)
reprojErrors = this->computeReprojectionErrors(resMesh, matchesInit, matchesInitIdxs);
else
reprojErrors = this->computeReprojectionErrors(resMesh, matchesInit, inlierMatchIdxs);
uvec idxs = find( reprojErrors < radius );
if ( idxs.n_elem == 0 )
break;
if (i >= iterTO)
inlierMatchIdxs = matchesInitIdxs.elem( idxs );
else
inlierMatchIdxs = inlierMatchIdxs.elem( idxs );
// Update parameters
wr = wr / Reconstruction::ROBUST_SCALE;
radius = radius / Reconstruction::ROBUST_SCALE;
}
}
