const CPoint3DWORLD CLandmark::_getOptimizedLandmarkSTEREOUV( const UIDFrame& p_uFrame, const CPoint3DWORLD& p_vecInitialGuess )
{
//ds initial values
Eigen::Matrix4d matH( Eigen::Matrix4d::Zero( ) );
Eigen::Vector4d vecB( Eigen::Vector4d::Zero( ) );
CPoint3DHomogenized vecX( CMiniVisionToolbox::getHomogeneous( p_vecInitialGuess ) );
//Eigen::Matrix2d matOmega( Eigen::Matrix2d::Identity( ) );
double dErrorSquaredTotalPixelsPREVIOUS = 0.0;
//ds iterations (break-out if convergence reached early)
for( uint32_t uIteration = 0; uIteration < CLandmark::uCapIterations; ++uIteration )
{
//ds counts
double dErrorSquaredTotalPixels = 0.0;
uint32_t uInliers = 0;
//ds initialize setup
matH.setZero( );
vecB.setZero( );
//ds do calibration over all recorded values
for( const CMeasurementLandmark* pMeasurement: m_vecMeasurements )
{
//ds apply the projection to the transformed point
const Eigen::Vector3d vecABCLEFT = pMeasurement->matProjectionWORLDtoLEFT*vecX;
const Eigen::Vector3d vecABCRIGHT = pMeasurement->matProjectionWORLDtoRIGHT*vecX;
//ds buffer c value
const double dCLEFT = vecABCLEFT.z( );
const double dCRIGHT = vecABCRIGHT.z( );
//ds compute error
const Eigen::Vector2d vecUVLEFT( vecABCLEFT.x( )/dCLEFT, vecABCLEFT.y( )/dCLEFT );
const Eigen::Vector2d vecUVRIGHT( vecABCRIGHT.x( )/dCRIGHT, vecABCRIGHT.y( )/dCRIGHT );
const Eigen::Vector4d vecError( vecUVLEFT.x( )-pMeasurement->ptUVLEFT.x,
vecUVLEFT.y( )-pMeasurement->ptUVLEFT.y,
vecUVRIGHT.x( )-pMeasurement->ptUVRIGHT.x,
vecUVRIGHT.y( )-pMeasurement->ptUVRIGHT.y );
//ds current error
const double dErrorSquaredPixels = vecError.transpose( )*vecError;
//std::printf( "[%06lu][%04u] error: %4.2f %4.2f %4.2f %4.2f (squared: %4.2f)\n", uID, uIteration, vecError(0), vecError(1), vecError(2), vecError(3) , dErrorSquaredPixels );
//ds check if outlier
double dWeight = 1.0;
if( dKernelMaximumErrorSquaredPixels < dErrorSquaredPixels )
{
dWeight = dKernelMaximumErrorSquaredPixels/dErrorSquaredPixels;
}
else
{
++uInliers;
}
dErrorSquaredTotalPixels += dWeight*dErrorSquaredPixels;
//ds jacobian of the homogeneous division
Eigen::Matrix< double, 2, 3 > matJacobianLEFT;
matJacobianLEFT << 1/dCLEFT, 0, -vecABCLEFT.x( )/( dCLEFT*dCLEFT ),
0, 1/dCLEFT, -vecABCLEFT.y( )/( dCLEFT*dCLEFT );
Eigen::Matrix< double, 2, 3 > matJacobianRIGHT;
matJacobianRIGHT << 1/dCRIGHT, 0, -vecABCRIGHT.x( )/( dCRIGHT*dCRIGHT ),
0, 1/dCRIGHT, -vecABCRIGHT.y( )/( dCRIGHT*dCRIGHT );
//ds final jacobian
Eigen::Matrix< double, 4, 4 > matJacobian;
matJacobian.setZero( );
matJacobian.block< 2,4 >(0,0) = matJacobianLEFT*pMeasurement->matProjectionWORLDtoLEFT;
matJacobian.block< 2,4 >(2,0) = matJacobianRIGHT*pMeasurement->matProjectionWORLDtoRIGHT;
//ds precompute transposed
const Eigen::Matrix< double, 4, 4 > matJacobianTransposed( matJacobian.transpose( ) );
//ds accumulate
matH += dWeight*matJacobianTransposed*matJacobian;
vecB += dWeight*matJacobianTransposed*vecError;
}
//ds solve constrained system (since dx(3) = 0.0) and update x solution
vecX.block< 3,1 >(0,0) += matH.block< 4, 3 >(0,0).householderQr( ).solve( -vecB );
//std::printf( "[%06lu][%04u]: %6.2f %6.2f %6.2f %6.2f (delta 2norm: %f inliers: %u)\n", uID, uIteration, vecX.x( ), vecX.y( ), vecX.z( ), vecX(3), vecDeltaX.squaredNorm( ), uInliers );
//std::fprintf( m_pFilePosition, "%04lu %06lu %03u %03lu %03u %6.2f\n", p_uFrame, uID, uIteration, m_vecMeasurements.size( ), uInliers, dRSSCurrent );
//ds check if we have converged
if( CLandmark::dConvergenceDelta > std::fabs( dErrorSquaredTotalPixelsPREVIOUS-dErrorSquaredTotalPixels ) )
{
//ds compute average error
const double dErrorSquaredAveragePixels = dErrorSquaredTotalPixels/m_vecMeasurements.size( );
//ds if acceptable inlier/outlier ratio
if( CLandmark::dMinimumRatioInliersToOutliers < static_cast< double >( uInliers )/m_vecMeasurements.size( ) )
{
//ds success
++uOptimizationsSuccessful;
//ds update average
dCurrentAverageSquaredError = dErrorSquaredAveragePixels;
//ds check if optimal
if( dMaximumErrorSquaredAveragePixels > dErrorSquaredAveragePixels )
{
bIsOptimal = true;
}
//std::printf( "<CLandmark>(_getOptimizedLandmarkSTEREOUV) [%06lu] converged (%2u) in %3u iterations to (%6.2f %6.2f %6.2f) from (%6.2f %6.2f %6.2f) ARSS: %6.2f (inliers: %u/%lu)\n",
// uID, uOptimizationsSuccessful, uIteration, vecX(0), vecX(1), vecX(2), p_vecInitialGuess(0), p_vecInitialGuess(1), p_vecInitialGuess(2), dCurrentAverageSquaredError, uInliers, m_vecMeasurements.size( ) );
//ds update the estimate
return vecX.block< 3,1 >(0,0);
}
else
{
++uOptimizationsFailed;
//std::printf( "<CLandmark>(_getOptimizedLandmarkSTEREOUV) landmark [%06lu] optimization failed - solution unacceptable (average error: %f, inliers: %u, iteration: %u)\n", uID, dErrorSquaredAveragePixels, uInliers, uIteration );
//ds if still here the calibration did not converge - keep the initial estimate
return p_vecInitialGuess;
}
}
else
{
//ds update error
dErrorSquaredTotalPixelsPREVIOUS = dErrorSquaredTotalPixels;
}
}
++uOptimizationsFailed;
//std::printf( "<CLandmark>(_getOptimizedLandmarkSTEREOUV) landmark [%06lu] optimization failed - system did not converge\n", uID );
//ds if still here the calibration did not converge - keep the initial estimate
return p_vecInitialGuess;
}
void Arnoldi<SCAL>::Calc (int numval, Array<Complex> & lam, int numev,
Array<shared_ptr<BaseVector>> & hevecs,
const BaseMatrix * pre) const
{
static Timer t("arnoldi");
static Timer t2("arnoldi - orthogonalize");
static Timer t3("arnoldi - compute large vectors");
RegionTimer reg(t);
auto hv = a.CreateVector();
auto hv2 = a.CreateVector();
auto hva = a.CreateVector();
auto hvm = a.CreateVector();
int n = hv.FV<SCAL>().Size();
int m = min2 (numval, n);
Matrix<SCAL> matH(m);
Array<shared_ptr<BaseVector>> abv(m);
for (int i = 0; i < m; i++)
abv[i] = a.CreateVector();
auto mat_shift = a.CreateMatrix();
mat_shift->AsVector() = a.AsVector() - shift*b.AsVector();
shared_ptr<BaseMatrix> inv;
if (!pre)
inv = mat_shift->InverseMatrix (freedofs);
else
{
auto itso = make_shared<GMRESSolver<double>> (*mat_shift, *pre);
itso->SetPrintRates(1);
itso->SetMaxSteps(2000);
inv = itso;
}
hv.SetRandom();
hv.SetParallelStatus (CUMULATED);
FlatVector<SCAL> fv = hv.FV<SCAL>();
if (freedofs)
for (int i = 0; i < hv.Size(); i++)
if (! (*freedofs)[i] ) fv(i) = 0;
t2.Start();
// matV = SCAL(0.0); why ?
matH = SCAL(0.0);
*hv2 = *hv;
SCAL len = sqrt (S_InnerProduct<SCAL> (*hv, *hv2)); // parallel
*hv /= len;
for (int i = 0; i < m; i++)
{
cout << IM(1) << "\ri = " << i << "/" << m << flush;
/*
for (int j = 0; j < n; j++)
matV(i,j) = hv.FV<SCAL>()(j);
*/
*abv[i] = *hv;
*hva = b * *hv;
*hvm = *inv * *hva;
for (int j = 0; j <= i; j++)
{
/*
SCAL sum = 0.0;
for (int k = 0; k < n; k++)
sum += hvm.FV<SCAL>()(k) * matV(j,k);
matH(j,i) = sum;
for (int k = 0; k < n; k++)
hvm.FV<SCAL>()(k) -= sum * matV(j,k);
*/
/*
SCAL sum = 0.0;
FlatVector<SCAL> abvj = abv[j] -> FV<SCAL>();
FlatVector<SCAL> fv_hvm = hvm.FV<SCAL>();
for (int k = 0; k < n; k++)
sum += fv_hvm(k) * abvj(k);
matH(j,i) = sum;
for (int k = 0; k < n; k++)
fv_hvm(k) -= sum * abvj(k);
*/
matH(j,i) = S_InnerProduct<SCAL> (*hvm, *abv[j]);
*hvm -= matH(j,i) * *abv[j];
}
*hv = *hvm;
*hv2 = *hv;
SCAL len = sqrt (S_InnerProduct<SCAL> (*hv, *hv2));
if (i<m-1) matH(i+1,i) = len;
*hv /= len;
}
t2.Stop();
t2.AddFlops (double(n)*m*m);
cout << "n = " << n << ", m = " << m << " n*m*m = " << n*m*m << endl;
cout << IM(1) << "\ri = " << m << "/" << m << endl;
Vector<Complex> lami(m);
Matrix<Complex> evecs(m);
Matrix<Complex> matHt(m);
matHt = Trans (matH);
evecs = Complex (0.0);
lami = Complex (0.0);
cout << "Solve Hessenberg evp with Lapack ... " << flush;
LapackHessenbergEP (matH.Height(), &matHt(0,0), &lami(0), &evecs(0,0));
cout << "done" << endl;
for (int i = 0; i < m; i++)
lami(i) = 1.0 / lami(i) + shift;
lam.SetSize (m);
for (int i = 0; i < m; i++)
lam[i] = lami(i);
t3.Start();
if (numev>0)
{
int nout = min2 (numev, m);
hevecs.SetSize(nout);
for (int i = 0; i< nout; i++)
{
hevecs[i] = a.CreateVector();
*hevecs[i] = 0;
for (int j = 0; j < m; j++)
*hevecs[i] += evecs(i,j) * *abv[j];
// hevecs[i]->FVComplex() = Trans(matV)*evecs.Row(i);
}
}
t3.Stop();
}
const CPoint3DWORLD CLandmark::_getOptimizedLandmarkLEFT3D( const UIDFrame& p_uFrame, const CPoint3DWORLD& p_vecInitialGuess )
{
//ds initial values
Eigen::Matrix3d matH( Eigen::Matrix3d::Zero( ) );
Eigen::Vector3d vecB( Eigen::Vector3d::Zero( ) );
const Eigen::Matrix3d matOmega( Eigen::Matrix3d::Identity( ) );
double dErrorSquaredTotalMetersPREVIOUS = 0.0;
//ds 3d point to optimize
CPoint3DWORLD vecX( p_vecInitialGuess );
//ds iterations (break-out if convergence reached early)
for( uint32_t uIteration = 0; uIteration < CLandmark::uCapIterations; ++uIteration )
{
//ds counts
double dErrorSquaredTotalMeters = 0.0;
uint32_t uInliers = 0;
//ds initialize setup
matH.setZero( );
vecB.setZero( );
//ds do calibration over all recorded values
for( const CMeasurementLandmark* pMeasurement: m_vecMeasurements )
{
//ds get error
const Eigen::Vector3d vecError( vecX-pMeasurement->vecPointXYZWORLD );
//ds current error
const double dErrorSquaredMeters = vecError.transpose( )*vecError;
//ds check if outlier
double dWeight = 1.0;
if( 0.1 < dErrorSquaredMeters )
{
dWeight = 0.1/dErrorSquaredMeters;
}
else
{
++uInliers;
}
dErrorSquaredTotalMeters += dWeight*dErrorSquaredMeters;
//ds accumulate (special case as jacobian is the identity)
matH += dWeight*matOmega;
vecB += dWeight*vecError;
}
//ds update x solution
vecX += matH.ldlt( ).solve( -vecB );
//ds check if we have converged
if( CLandmark::dConvergenceDelta > std::fabs( dErrorSquaredTotalMetersPREVIOUS-dErrorSquaredTotalMeters ) )
{
//ds compute average error
const double dErrorSquaredAverageMeters = dErrorSquaredTotalMeters/m_vecMeasurements.size( );
//ds if acceptable (don't mind about the actual number of inliers - we could be at this point with only 2 measurements -> 2 inliers -> 100%)
if( 0 < uInliers )
{
//ds success
++uOptimizationsSuccessful;
//ds update average
dCurrentAverageSquaredError = dErrorSquaredAverageMeters;
//ds check if optimal
if( 0.075 > dErrorSquaredAverageMeters )
{
bIsOptimal = true;
}
//std::printf( "<CLandmark>(_getOptimizedLandmarkWORLD) [%06lu] converged (%2u) in %3u iterations to (%6.2f %6.2f %6.2f) from (%6.2f %6.2f %6.2f) ARSS: %6.2f (inliers: %u/%lu)\n",
// uID, uOptimizationsSuccessful, uIteration, vecX(0), vecX(1), vecX(2), p_vecInitialGuess(0), p_vecInitialGuess(1), p_vecInitialGuess(2), dCurrentAverageSquaredErrorPixels, uInliers, m_vecMeasurements.size( ) );
return vecX;
}
else
{
++uOptimizationsFailed;
//std::printf( "<CLandmark>(_getOptimizedLandmarkWORLD) landmark [%06lu] optimization failed - solution unacceptable (average error: %f, inliers: %u, iteration: %u)\n", uID, dErrorSquaredAverageMeters, uInliers, uIteration );
//ds if still here the calibration did not converge - keep the initial estimate
return p_vecInitialGuess;
}
}
else
{
//ds update error
dErrorSquaredTotalMetersPREVIOUS = dErrorSquaredTotalMeters;
}
}
++uOptimizationsFailed;
//std::printf( "<CLandmark>(_getOptimizedLandmarkWORLD) landmark [%06lu] optimization failed - system did not converge\n", uID );
//ds if still here the calibration did not converge - keep the initial estimate
return p_vecInitialGuess;
}