void SurfaceVectorJump<EvalT, Traits>::evaluateFields(
typename Traits::EvalData workset)
{
Intrepid2::Vector<ScalarT>
vecA(0, 0, 0), vecB(0, 0, 0), vecJump(0, 0, 0);
for (int cell = 0; cell < workset.numCells; ++cell) {
for (int pt = 0; pt < num_qps_; ++pt) {
vecA.fill(Intrepid2::ZEROS);
vecB.fill(Intrepid2::ZEROS);
for (int node = 0; node < num_plane_nodes_; ++node) {
int topNode = node + num_plane_nodes_;
vecA += Intrepid2::Vector<ScalarT>(
ref_values_(node, pt) * vector_(cell, node, 0),
ref_values_(node, pt) * vector_(cell, node, 1),
ref_values_(node, pt) * vector_(cell, node, 2));
vecB += Intrepid2::Vector<ScalarT>(
ref_values_(node, pt) * vector_(cell, topNode, 0),
ref_values_(node, pt) * vector_(cell, topNode, 1),
ref_values_(node, pt) * vector_(cell, topNode, 2));
}
vecJump = vecB - vecA;
jump_(cell, pt, 0) = vecJump(0);
jump_(cell, pt, 1) = vecJump(1);
jump_(cell, pt, 2) = vecJump(2);
}
}
}
const std::vector<T>& vec() const {
if (m_A)
return vecA();
else if (m_B)
return vecB();
else
return vecC();
}
TEST(VectorTest, DotTest)
{
Math::Vector vecA(0.8202190530968309, 0.0130926060162780, 0.2411914183883510);
Math::Vector vecB(-0.0524083951404069, 1.5564932716738220, -0.8971342631500536);
float expectedDot = -0.238988896477326;
EXPECT_TRUE(Math::IsEqual(Math::DotProduct(vecA, vecB), expectedDot, TEST_TOLERANCE));
}
// Returns cross product between two 3D vectors
Point Bezier::crossP(Point a, Point b) {
float* valueA = a.getValues();
float* valueB = b.getValues();
Eigen::Vector3f vecA(valueA[0], valueA[1], valueA[2]);
Eigen::Vector3f vecB(valueB[0], valueB[1], valueB[2]);
Eigen::Vector3f result = vecA.cross(vecB);
Point point(result[0], result[1], result[2]);
return point;
}
TEST(Copy, CopyOneElementToEmptyContainer)
{
std::vector<int> vecA = {1, 2, 3};
std::vector<int> vecB(1);
std::vector<int>::iterator it = Namespace::copy(vecA.begin()+1, vecA.begin()+2, vecB.begin());
EXPECT_EQ(vecB.end(), it);
EXPECT_TRUE(std::equal(vecB.begin(), vecB.end(), std::vector<int>({2}).begin()));
}
TEST(Copy, CopyMultipleElementsToBeginningOfContainer)
{
std::vector<int> vecA = {1, 2, 3};
std::vector<int> vecB(3);
vecB = {4, 5, 0};
std::vector<int>::iterator it = Namespace::copy(vecA.begin(), vecA.end(), vecB.begin());
EXPECT_EQ(vecB.end(), it);
EXPECT_TRUE(std::equal(vecB.begin(), vecB.end(), std::vector<int>({1, 2, 3}).begin()));
EXPECT_EQ(std::vector<int>({1, 2, 3}), vecB);
}
TEST(Copy, CopyMultipleElementsToEmptyContainer)
{
std::vector<int> vecA = {1, 2, 3};
std::vector<int> vecB(3);
std::vector<int>::iterator it = Namespace::copy(vecA.begin(), vecA.end(), vecB.begin());
EXPECT_EQ(vecB.end(), it);
std::vector<int> reference = {1, 2, 3};
EXPECT_TRUE(std::equal(vecB.begin(), vecB.end(), reference.begin()));
}
void Registration::ICPEnergyMinimization(const Point3DSet* pRef, const Point3DSet* pOrigin, const MagicMath::HomoMatrix4* pTransInit,
std::vector<int>& sampleIndex, std::vector<int>& correspondIndex, MagicMath::HomoMatrix4* pTransDelta)
{
//DebugLog << "Registration::ICPEnergyMinimization" << std::endl;
int pcNum = sampleIndex.size();
Eigen::MatrixXd matA(pcNum, 6);
Eigen::VectorXd vecB(pcNum, 1);
for (int i = 0; i < pcNum; i++)
{
MagicMath::Vector3 norRef = pRef->GetPoint(correspondIndex.at(i))->GetNormal();
MagicMath::Vector3 posRef = pRef->GetPoint(correspondIndex.at(i))->GetPosition();
MagicMath::Vector3 posPC = pTransInit->TransformPoint( pOrigin->GetPoint(sampleIndex.at(i))->GetPosition() );
vecB(i) = (posRef - posPC) * norRef;
MagicMath::Vector3 coffTemp = posPC.CrossProduct(norRef);
matA(i, 0) = coffTemp[0];
matA(i, 1) = coffTemp[1];
matA(i, 2) = coffTemp[2];
matA(i, 3) = norRef[0];
matA(i, 4) = norRef[1];
matA(i, 5) = norRef[2];
}
Eigen::MatrixXd matAT = matA.transpose();
Eigen::MatrixXd matCoefA = matAT * matA;
Eigen::MatrixXd vecCoefB = matAT * vecB;
Eigen::VectorXd res = matCoefA.ldlt().solve(vecCoefB);
pTransDelta->Unit();
pTransDelta->SetValue(0, 1, -res(2));
pTransDelta->SetValue(0, 2, res(1));
pTransDelta->SetValue(0, 3, res(3));
pTransDelta->SetValue(1, 0, res(2));
pTransDelta->SetValue(1, 2, -res(0));
pTransDelta->SetValue(1, 3, res(4));
pTransDelta->SetValue(2, 0, -res(1));
pTransDelta->SetValue(2, 1, res(0));
pTransDelta->SetValue(2, 3, res(5));
//print result
/*DebugLog << "MatA: " << std::endl;
DebugLog << 1 << " " << -res(2) << " " << res(1) << " " << res(3) << std::endl;
DebugLog << res(2) << " " << 1 << " " << -res(0) << " " << res(4) << std::endl;
DebugLog << -res(1) << " " << res(0) << " " << 1 << " " << res(5) << std::endl;*/
}
TEST(VectorTest, CrossTest)
{
Math::Vector vecA(1.37380499798567, 1.18054518384682, 1.95166361293121);
Math::Vector vecB(0.891657855926886, 0.447591335394532, -0.901604070087823);
Math::Vector expectedCross(-1.937932065431669, 2.978844370287636, -0.437739173833581);
Math::Vector expectedReverseCross = -expectedCross;
EXPECT_TRUE(Math::VectorsEqual(vecA.CrossMultiply(vecB), expectedCross, TEST_TOLERANCE));
EXPECT_TRUE(Math::VectorsEqual(vecB.CrossMultiply(vecA), expectedReverseCross, TEST_TOLERANCE));
}
/**
* Calculates bounds for a CqCurve.
*
* NOTE: This method makes the same assumptions as
* CqSurfacePatchBicubic::Bound() does about the convex-hull property of the
* curve. This is fine most of the time, but the user can specify basis
* matrices like Catmull-Rom, which are non-convex.
*
* FIXME: Make sure that all hulls which reach this method are convex!
*
* @return CqBound object containing the bounds.
*/
void CqCurve::Bound(CqBound* bound) const
{
// Get the boundary in camera space.
CqVector3D vecA( FLT_MAX, FLT_MAX, FLT_MAX );
CqVector3D vecB( -FLT_MAX, -FLT_MAX, -FLT_MAX );
TqFloat maxCameraSpaceWidth = 0;
TqUint nWidthParams = cVarying();
for ( TqUint i = 0; i < ( *P() ).Size(); i++ )
{
// expand the boundary if necessary to accomodate the
// current vertex
CqVector3D vecV = vectorCast<CqVector3D>(P()->pValue( i )[0]);
if ( vecV.x() < vecA.x() )
vecA.x( vecV.x() );
if ( vecV.y() < vecA.y() )
vecA.y( vecV.y() );
if ( vecV.x() > vecB.x() )
vecB.x( vecV.x() );
if ( vecV.y() > vecB.y() )
vecB.y( vecV.y() );
if ( vecV.z() < vecA.z() )
vecA.z( vecV.z() );
if ( vecV.z() > vecB.z() )
vecB.z( vecV.z() );
// increase the maximum camera space width of the curve if
// necessary
if ( i < nWidthParams )
{
TqFloat camSpaceWidth = width()->pValue( i )[0];
if ( camSpaceWidth > maxCameraSpaceWidth )
{
maxCameraSpaceWidth = camSpaceWidth;
}
}
}
// increase the size of the boundary by half the width of the
// curve in camera space
vecA -= ( maxCameraSpaceWidth / 2.0 );
vecB += ( maxCameraSpaceWidth / 2.0 );
bound->vecMin() = vecA;
bound->vecMax() = vecB;
AdjustBoundForTransformationMotion( bound );
}
void SceneManager::Update()
{
RenderLayerManager & renderManager = RenderLayerManager::GetRenderLayerManager();
const PVRTVec3 center = renderManager.GetCenter();
float occlusionRadius = renderManager.GetOcclusionRadius();
PVRTVec4 vecA( mLookMtx->f[12], 0.0f, mLookMtx->f[14], 1);
PVRTVec4 vecB( GLOBAL_SCALE * FRUSTUM_W, 0.0f, GLOBAL_SCALE * FRUSTUM_D, 1);
PVRTVec4 vecC( GLOBAL_SCALE * -FRUSTUM_W, 0.0f, GLOBAL_SCALE * FRUSTUM_D, 1);
vecB = *mLookMtx * vecB;
vecC = *mLookMtx * vecC;
PVRTVec2 A(vecA.x, vecA.z);
PVRTVec2 B(vecB.x, vecB.z);
PVRTVec2 C(vecC.x, vecC.z);
mToApplyCount = 0;
if (mQuadTree)
{
static QuadNode * quadNodes[256]={0};
int quadNodeCount = 0;
//mQuadTree->GetQuads(center.x, center.z, occlusionRadius, quadNodes, quadNodeCount);
mQuadTree->GetQuadsCameraFrustum(quadNodes, quadNodeCount, mLookMtx);
quadNodeCount--;
bool useFrustumCulling = true; //!!!!!!!!!!!!!!!!!!!!!
for (int quad = quadNodeCount ; quad >=0 ; quad--)
{
QuadNode * pQuadNode = quadNodes[quad];
List & dataList = pQuadNode->GetDataList();
ListIterator listIter(dataList);
while( Node * pRootNode = (Node*)listIter.GetPtr() )
{
if (!pRootNode->IsVisible())
continue;
//pRootNode->UpdateWithoutChildren();
bool useOcclusionRadius = pRootNode->GetUseOcclusionCulling();
PVRTVec3 worldPos = pRootNode->GetWorldTranslation();
if (!useFrustumCulling && useOcclusionRadius)
{
PVRTVec3 distVec = worldPos - center;
if ( distVec.lenSqr() < MM(occlusionRadius) )
{
pRootNode->SetInFrustum(true);
pRootNode->Update();
mToApply[mToApplyCount] = pRootNode;
mToApplyCount++;
}
else
{
pRootNode->SetInFrustum(false);
}
}
else if (useFrustumCulling)
{
PVRTVec2 P(worldPos.x, worldPos.z);
PVRTVec2 v0 = C - A;
PVRTVec2 v1 = B - A;
PVRTVec2 v2 = P - A;
// Compute dot products
float dot00 = v0.dot(v0);
float dot01 = v0.dot(v1);
float dot02 = v0.dot(v2);
float dot11 = v1.dot(v1);
float dot12 = v1.dot(v2);
// Compute barycentric coordinates
float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
bool addToList = false;
// Check if point is in triangle
//PVRTVec3 distVec = worldPos - center;
//if ( distVec.lenSqr() < MM(occlusionRadius) )
{
if ( (u > 0) && (v > 0) && (u + v < 1))
{
addToList = true;
}
else if ( Collision::CircleTriangleEdgeIntersection(A,B,P, pRootNode->GetRadius() ) )
{
addToList = true;
}
else if ( Collision::CircleTriangleEdgeIntersection(A,C,P, pRootNode->GetRadius() ))
{
addToList = true;
}
if (addToList)
{
pRootNode->SetInFrustum(true);
//pRootNode->Update();
mToApply[mToApplyCount] = pRootNode;
mToApplyCount++;
}
else
{
pRootNode->SetInFrustum(false);
}
}
//else
//{
// pRootNode->SetInFrustum(false);
//}
}
else
{
pRootNode->SetInFrustum(true);
//pRootNode->Update();
mToApply[mToApplyCount] = pRootNode;
mToApplyCount++;
}
}
}
}
for (int n=0;n<mNodeCount;n++)
{
Node * pRootNode = mRootNodes[n];
if (!pRootNode->IsVisible())
continue;
pRootNode->UpdateWithoutChildren();
bool useOcclusionRadius = pRootNode->GetUseOcclusionCulling();
PVRTVec3 worldPos = pRootNode->GetWorldTranslation();
PVRTVec3 distVec = worldPos - center;
if (useOcclusionRadius)
{
if ( distVec.lenSqr() < MM(occlusionRadius) )
{
PVRTVec2 P(worldPos.x, worldPos.z);
PVRTVec2 v0 = C - A;
PVRTVec2 v1 = B - A;
PVRTVec2 v2 = P - A;
// Compute dot products
float dot00 = v0.dot(v0);
float dot01 = v0.dot(v1);
float dot02 = v0.dot(v2);
float dot11 = v1.dot(v1);
float dot12 = v1.dot(v2);
// Compute barycentric coordinates
float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
bool addToList = false;
// Check if point is in triangle
//PVRTVec3 distVec = worldPos - center;
//if ( distVec.lenSqr() < MM(occlusionRadius) )
{
if ( (u > 0) && (v > 0) && (u + v < 1))
{
addToList = true;
}
else if ( Collision::CircleTriangleEdgeIntersection(A,B,P, pRootNode->GetRadius() ) )
{
addToList = true;
}
else if ( Collision::CircleTriangleEdgeIntersection(A,C,P, pRootNode->GetRadius() ))
{
addToList = true;
}
if (addToList)
{
pRootNode->SetInFrustum(true);
pRootNode->Update();
mToApply[mToApplyCount] = pRootNode;
mToApplyCount++;
}
else
{
pRootNode->SetInFrustum(false);
}
}
/*
pRootNode->SetInFrustum(true);
pRootNode->Update();
mToApply[mToApplyCount] = pRootNode;
mToApplyCount++;
*/
}
else
{
pRootNode->SetInFrustum(false);
}
}
else
{
pRootNode->SetInFrustum(true);
pRootNode->Update();
mToApply[mToApplyCount] = pRootNode;
mToApplyCount++;
}
/*
PVRTVec3 worldPos = pRootNode->GetWorldTranslation();
PVRTVec3 distVec = worldPos - center;
if (!pRootNode->GetUseOcclusionCulling())
{
pRootNode->SetInFrustum(true);
}
else if ( distVec.lenSqr() < occlusionRadius )
{
pRootNode->SetInFrustum(true);
}
else
{
pRootNode->SetInFrustum(false);
}
*/
}
}
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;
}
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;
}
