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; }