void CRenWin3D::DrawAxis()
{
glDisable(GL_LIGHTING);
glClear(GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glOrtho(-2.0f, 2.0f, -1.5f, 1.5f, 0.1f, 100.0f);
glm::mat4 rotMx = glm::lookAt(glm::vec3(0.0f, 0.0f, 0.0f), m_front, m_up);
glm::vec4 vecX(0.1f, 0.0f, 0.0f, 1.0f);
glm::vec4 vecY(0.0f, 0.1f, 0.0f, 1.0f);
glm::vec4 vecZ(0.0f, 0.0f, 0.1f, 1.0f);
vecX = rotMx * vecX;
vecY = rotMx * vecY;
vecZ = rotMx * vecZ;
glTranslatef(-1.9f, -1.4f, -20.0f);
glColor3f(1.0f, 0.0f, 0.0f);
glBegin(GL_LINES);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(vecX.x, vecX.y, vecX.z);
glEnd();
glColor3f(0.0f, 1.0f, 0.0f);
glBegin(GL_LINES);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(vecY.x, vecY.y, vecY.z);
glEnd();
glColor3f(0.0f, 0.0f, 1.0f);
glBegin(GL_LINES);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(vecZ.x, vecZ.y, vecZ.z);
glEnd();
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
if(m_lighting)
{
glEnable(GL_LIGHTING);
}
glColor3f(1.0f, 1.0f, 1.0f);
}
void popblas::testBlas::test_gemv() {
float alpha = 2;
pop::F32 dataA[] = {0, 3, 2,
1, -2, 3,
5, 6, 2,
0, 1, 2,
3, 6, 1};
pop::F32 dataX[] = {1, 2, 3};
float beta = 1;
pop::F32 dataY[] = {1,1,1,1,1};
pop::MatN<2, pop::F32> vecX(pop::VecN<2, int>(3, 1), dataX);
pop::MatN<2, pop::F32> vecY(pop::VecN<2, int>(5, 1), dataY);
pop::MatN<2, pop::F32> matA(pop::VecN<2, int>(5, 3), dataA);
pop::MatN<2, pop::F32> opMatA = matA.transpose();
blas::gemv(alpha, opMatA, 'T', vecX, beta, vecY);
std::cout << vecY << std::endl;
}
void customAttrManip::updateManipLocations()
//
// Description
// This method places the manip in the scene according to the information
// obtained from the attached transform node. The position and orientation
// of the distance manip is determined.
//
{
MVector trans = nodeTranslation();
MQuaternion q = nodeRotation();
MFnDistanceManip freePointManipFn(fManip);
MVector vecX(1.0, 0.0, 0.0);
freePointManipFn.setDirection(vecX);
freePointManipFn.rotateBy(q);
freePointManipFn.setTranslation(trans, MSpace::kWorld);
}
void popblas::testBlas::test_ger() {
pop::F32 dataX[] = {2,3,4};
pop::F32 datamatX[] = {2, 3, 4,
1, 5, 3,
2, 1, 0};
pop::F32 dataY[] = {1,3,5,7,2};
pop::F32 alpha = 2;
pop::MatN<2, pop::F32> vecX(pop::VecN<2, int>(1, 3), dataX);
pop::MatN<2, pop::F32> matX(pop::VecN<2, int>(3, 3), datamatX);
pop::MatN<2, pop::F32> vecY(pop::VecN<2, int>(1, 5), dataY);
pop::MatN<2, pop::F32> matA(3, 5);
pop::MatN<2, pop::F32> matB(3, 5);
matA = 0; matB = 0;
blas::ger(alpha, vecX, vecY, matA);
pop::MatN<2, pop::F32> vecmatX = matX.selectRow(0);
blas::ger(alpha, vecmatX, vecY, matB);
std::cout << matA << std::endl;
std::cout << matB << std::endl;
}
bool DecalManager::clipDecal( DecalInstance *decal, Vector<Point3F> *edgeVerts, const Point2F *clipDepth )
{
PROFILE_SCOPE( DecalManager_clipDecal );
// Free old verts and indices.
_freeBuffers( decal );
ClippedPolyList clipper;
clipper.mNormal.set( Point3F( 0, 0, 0 ) );
clipper.mPlaneList.setSize(6);
F32 halfSize = decal->mSize * 0.5f;
// Ugly hack for ProjectedShadow!
F32 halfSizeZ = clipDepth ? clipDepth->x : halfSize;
F32 negHalfSize = clipDepth ? clipDepth->y : halfSize;
Point3F decalHalfSize( halfSize, halfSize, halfSize );
Point3F decalHalfSizeZ( halfSizeZ, halfSizeZ, halfSizeZ );
MatrixF projMat( true );
decal->getWorldMatrix( &projMat );
const VectorF &crossVec = decal->mNormal;
const Point3F &decalPos = decal->mPosition;
VectorF newFwd, newRight;
projMat.getColumn( 0, &newRight );
projMat.getColumn( 1, &newFwd );
VectorF objRight( 1.0f, 0, 0 );
VectorF objFwd( 0, 1.0f, 0 );
VectorF objUp( 0, 0, 1.0f );
// See above re: decalHalfSizeZ hack.
clipper.mPlaneList[0].set( ( decalPos + ( -newRight * halfSize ) ), -newRight );
clipper.mPlaneList[1].set( ( decalPos + ( -newFwd * halfSize ) ), -newFwd );
clipper.mPlaneList[2].set( ( decalPos + ( -crossVec * decalHalfSizeZ ) ), -crossVec );
clipper.mPlaneList[3].set( ( decalPos + ( newRight * halfSize ) ), newRight );
clipper.mPlaneList[4].set( ( decalPos + ( newFwd * halfSize ) ), newFwd );
clipper.mPlaneList[5].set( ( decalPos + ( crossVec * negHalfSize ) ), crossVec );
clipper.mNormal = -decal->mNormal;
Box3F box( -decalHalfSizeZ, decalHalfSizeZ );
projMat.mul( box );
DecalData *decalData = decal->mDataBlock;
PROFILE_START( DecalManager_clipDecal_buildPolyList );
getContainer()->buildPolyList( box, decalData->clippingMasks, &clipper );
PROFILE_END();
clipper.cullUnusedVerts();
clipper.triangulate();
clipper.generateNormals();
if ( clipper.mVertexList.empty() )
return false;
#ifdef DECALMANAGER_DEBUG
mDebugPlanes.clear();
mDebugPlanes.merge( clipper.mPlaneList );
#endif
decal->mVertCount = clipper.mVertexList.size();
decal->mIndxCount = clipper.mIndexList.size();
Vector<Point3F> tmpPoints;
tmpPoints.push_back(( objFwd * decalHalfSize ) + ( objRight * decalHalfSize ));
tmpPoints.push_back(( objFwd * decalHalfSize ) + ( -objRight * decalHalfSize ));
tmpPoints.push_back(( -objFwd * decalHalfSize ) + ( -objRight * decalHalfSize ));
Point3F lowerLeft(( -objFwd * decalHalfSize ) + ( objRight * decalHalfSize ));
projMat.inverse();
_generateWindingOrder( lowerLeft, &tmpPoints );
BiQuadToSqr quadToSquare( Point2F( lowerLeft.x, lowerLeft.y ),
Point2F( tmpPoints[0].x, tmpPoints[0].y ),
Point2F( tmpPoints[1].x, tmpPoints[1].y ),
Point2F( tmpPoints[2].x, tmpPoints[2].y ) );
Point2F uv( 0, 0 );
Point3F vecX(0.0f, 0.0f, 0.0f);
// Allocate memory for vert and index arrays
_allocBuffers( decal );
Point3F vertPoint( 0, 0, 0 );
for ( U32 i = 0; i < clipper.mVertexList.size(); i++ )
{
const ClippedPolyList::Vertex &vert = clipper.mVertexList[i];
vertPoint = vert.point;
// Transform this point to
// object space to look up the
// UV coordinate for this vertex.
projMat.mulP( vertPoint );
// Clamp the point to be within the quad.
vertPoint.x = mClampF( vertPoint.x, -decalHalfSize.x, decalHalfSize.x );
vertPoint.y = mClampF( vertPoint.y, -decalHalfSize.y, decalHalfSize.y );
// Get our UV.
uv = quadToSquare.transform( Point2F( vertPoint.x, vertPoint.y ) );
const RectF &rect = decal->mDataBlock->texRect[decal->mTextureRectIdx];
uv *= rect.extent;
uv += rect.point;
// Set the world space vertex position.
decal->mVerts[i].point = vert.point;
decal->mVerts[i].texCoord.set( uv.x, uv.y );
decal->mVerts[i].normal = clipper.mNormalList[i];
decal->mVerts[i].normal.normalize();
if( mFabs( decal->mVerts[i].normal.z ) > 0.8f )
mCross( decal->mVerts[i].normal, Point3F( 1.0f, 0.0f, 0.0f ), &vecX );
else if ( mFabs( decal->mVerts[i].normal.x ) > 0.8f )
mCross( decal->mVerts[i].normal, Point3F( 0.0f, 1.0f, 0.0f ), &vecX );
else if ( mFabs( decal->mVerts[i].normal.y ) > 0.8f )
mCross( decal->mVerts[i].normal, Point3F( 0.0f, 0.0f, 1.0f ), &vecX );
decal->mVerts[i].tangent = mCross( decal->mVerts[i].normal, vecX );
}
U32 curIdx = 0;
for ( U32 j = 0; j < clipper.mPolyList.size(); j++ )
{
// Write indices for each Poly
ClippedPolyList::Poly *poly = &clipper.mPolyList[j];
AssertFatal( poly->vertexCount == 3, "Got non-triangle poly!" );
decal->mIndices[curIdx] = clipper.mIndexList[poly->vertexStart];
curIdx++;
decal->mIndices[curIdx] = clipper.mIndexList[poly->vertexStart + 1];
curIdx++;
decal->mIndices[curIdx] = clipper.mIndexList[poly->vertexStart + 2];
curIdx++;
}
if ( !edgeVerts )
return true;
Point3F tmpHullPt( 0, 0, 0 );
Vector<Point3F> tmpHullPts;
for ( U32 i = 0; i < clipper.mVertexList.size(); i++ )
{
const ClippedPolyList::Vertex &vert = clipper.mVertexList[i];
tmpHullPt = vert.point;
projMat.mulP( tmpHullPt );
tmpHullPts.push_back( tmpHullPt );
}
edgeVerts->clear();
U32 verts = _generateConvexHull( tmpHullPts, edgeVerts );
edgeVerts->setSize( verts );
projMat.inverse();
for ( U32 i = 0; i < edgeVerts->size(); i++ )
projMat.mulP( (*edgeVerts)[i] );
return true;
}
int
process(const ecto::tendrils& inputs, const ecto::tendrils& outputs)
{
// Get the origin of the plane
cv::Point origin;
cv::Matx33f K, R;
cv::Vec3f T;
if (*do_center_)
origin = cv::Point(masks_->cols/2, masks_->rows/2);
else {
if (!K_ || K_->empty() || !R_in_ || R_in_->empty() || !T_in_ || T_in_->empty()) {
*found_ = false;
return ecto::OK;
}
K = *K_;
R = *R_in_;
T = *T_in_;
cv::Vec3f T = K*T;
origin = cv::Point(T(0)/T(2), T(1)/T(2));
}
// Go over the plane masks and simply count the occurrence of each mask
std::vector<int> occurrences(256, 0);
for(int y = std::max(0, origin.y - *window_size_); y < std::min(masks_->rows, origin.y + *window_size_); ++y) {
uchar *mask = masks_->ptr<uchar>(y) + std::max(0, origin.x - *window_size_);
uchar *mask_end = masks_->ptr<uchar>(y) + std::min(masks_->cols, origin.x + *window_size_);
for(; mask != mask_end; ++mask)
if (*mask != 255)
++occurrences[*mask];
}
// Find the most common plane
int best_index = -1;
int best_count = 0;
for(size_t i = 0; i < 255; ++i) {
if (occurrences[i] > best_count) {
best_index = i;
best_count = occurrences[i];
}
}
// Convert the plane coefficients to R,t
cv::Matx33f rotation;
cv::Vec3f translation;
if (best_index >= 0) {
*coeffs_ = (*planes_)[best_index];
float a = (*coeffs_)[0], b = (*coeffs_)[1], c = (*coeffs_)[2], d = (*coeffs_)[3];
// Deal with translation
if (*do_center_) {
getPlaneTransform(*coeffs_, rotation, translation);
// Make sure T_ points to the center of the image
translation = cv::Vec3f(0,0,-d/c);
} else {
// Have T_ point to the origin. Find alpha such that alpha*Kinv*origin is on the plane
cv::Matx33f K_inv = K.inv();
cv::Vec3f origin_inv = cv::Mat(K_inv * cv::Vec3f(origin.x, origin.y, 1));
float alpha = -d/(a*origin_inv(0) + b*origin_inv(1) + c*origin_inv(2));
translation = alpha*origin_inv;
if (translation(2) < 0)
translation = -translation;
// Make the rotation fit to the plane (but as close as possible to the current estimate
// Get the Z axis
cv::Vec3f N(a, b, c);
N = N/cv::norm(N);
// Get the X, Y axes
cv::Vec3f vecX(R(0,0), R(1,0), R(2,0));
cv::Vec3f vecY(R(0,1), R(1,1), R(2,1));
// Project them onto the plane
vecX = vecX - vecX.dot(N)*N;
vecY = vecY - vecY.dot(N)*N;
vecX = vecX/cv::norm(vecX);
vecY = vecY/cv::norm(vecY);
// Get the median
cv::Vec3f median = vecX + vecY;
median = median/cv::norm(median);
// Get a new basis
cv::Vec3f vecYtmp = vecY - median.dot(vecY)*median;
cv::Vec3f vecXtmp = vecX - median.dot(vecX)*median;
vecYtmp = vecYtmp/cv::norm(vecYtmp);
vecXtmp = vecXtmp/cv::norm(vecXtmp);
// Get the rectified X/Y axes
cv::Vec3f vecXnew = median + vecXtmp;
cv::Vec3f vecYnew = median + vecYtmp;
vecXnew = vecXnew/cv::norm(vecXnew);
vecYnew = vecYnew/cv::norm(vecYnew);
// Fill in the matrix
rotation = cv::Matx33f(vecXnew(0), vecYnew(0), N(0), vecXnew(1), vecYnew(1), N(1), vecXnew(2), vecYnew(2), N(2));
}
*R_out_ = cv::Mat(rotation);
*T_out_ = cv::Mat(translation);
*found_ = true;
} else
*found_ = false;
return ecto::OK;
}
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;
}
