/*!
Realize the VVS loop for the tracking
\param nbInfos : Size of the features
\param w : weight of the features after M-Estimation.
*/
void
vpMbKltTracker::computeVVS(const unsigned int &nbInfos, vpColVector &w)
{
vpMatrix L; // interaction matrix
vpColVector R; // residu
vpMatrix L_true; // interaction matrix
vpMatrix LVJ_true;
//vpColVector R_true; // residu
vpColVector v; // "speed" for VVS
vpHomography H;
vpColVector w_true;
vpRobust robust(2*nbInfos);
vpMatrix LTL;
vpColVector LTR;
vpHomogeneousMatrix cMoPrev;
vpHomogeneousMatrix ctTc0_Prev;
vpColVector error_prev(2*nbInfos);
double mu = 0.01;
double normRes = 0;
double normRes_1 = -1;
unsigned int iter = 0;
R.resize(2*nbInfos);
L.resize(2*nbInfos, 6, 0);
while( ((int)((normRes - normRes_1)*1e8) != 0 ) && (iter<maxIter) ){
unsigned int shift = 0;
computeVVSInteractionMatrixAndResidu(shift, R, L, H, kltPolygons, kltCylinders, ctTc0);
bool reStartFromLastIncrement = false;
computeVVSCheckLevenbergMarquardtKlt(iter, nbInfos, cMoPrev, error_prev, ctTc0_Prev, mu, reStartFromLastIncrement);
if(!reStartFromLastIncrement){
computeVVSWeights(iter, nbInfos, R, w_true, w, robust);
computeVVSPoseEstimation(iter, L, w, L_true, LVJ_true, normRes, normRes_1, w_true, R, LTL, LTR,
error_prev, v, mu, cMoPrev, ctTc0_Prev);
} // endif(!reStartFromLastIncrement)
iter++;
}
if(computeCovariance){
computeVVSCovariance(w_true, cMoPrev, L_true, LVJ_true);
}
}
/*!
\brief Compute the pose using virtual visual servoing approach and
a robust cotrol law
This approach is described in
A.I. Comport, E. Marchand, M. Pressigout, F. Chaumette. Real-time
markerless tracking for augmented reality: the virtual visual servoing
framework. IEEE Trans. on Visualization and Computer Graphics,
12(4):615-628, Juillet 2006.
*/
void
vpPose::poseVirtualVSrobust(vpHomogeneousMatrix & cMo)
{
try{
double residu_1 = 1e8 ;
double r =1e8-1;
// we stop the minimization when the error is bellow 1e-8
vpMatrix W ;
vpRobust robust(2*listP.size()) ;
robust.setThreshold(0.0000) ;
vpColVector w,res ;
unsigned int nb = listP.size() ;
vpMatrix L(2*nb,6) ;
vpColVector error(2*nb) ;
vpColVector sd(2*nb),s(2*nb) ;
vpColVector v ;
listP.front() ;
vpPoint P;
std::list<vpPoint> lP ;
// create sd
unsigned int k =0 ;
for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
{
P = *it;
sd[2*k] = P.get_x() ;
sd[2*k+1] = P.get_y() ;
lP.push_back(P) ;
k ++;
}
int iter = 0 ;
res.resize(s.getRows()/2) ;
w.resize(s.getRows()/2) ;
W.resize(s.getRows(), s.getRows()) ;
w =1 ;
while((int)((residu_1 - r)*1e12) !=0)
{
residu_1 = r ;
// Compute the interaction matrix and the error
vpPoint P ;
k =0 ;
for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
{
P = *it;
// forward projection of the 3D model for a given pose
// change frame coordinates
// perspective projection
P.track(cMo) ;
double x = s[2*k] = P.get_x(); // point projected from cMo
double y = s[2*k+1] = P.get_y();
double Z = P.get_Z() ;
L[2*k][0] = -1/Z ;
L[2*k][1] = 0 ;
L[2*k][2] = x/Z ;
L[2*k][3] = x*y ;
L[2*k][4] = -(1+x*x) ;
L[2*k][5] = y ;
L[2*k+1][0] = 0 ;
L[2*k+1][1] = -1/Z ;
L[2*k+1][2] = y/Z ;
L[2*k+1][3] = 1+y*y ;
L[2*k+1][4] = -x*y ;
L[2*k+1][5] = -x ;
k ++;
}
error = s - sd ;
// compute the residual
r = error.sumSquare() ;
for(unsigned int k=0 ; k <error.getRows()/2 ; k++)
{
res[k] = vpMath::sqr(error[2*k]) + vpMath::sqr(error[2*k+1]) ;
}
robust.setIteration(0);
robust.MEstimator(vpRobust::TUKEY, res, w);
// compute the pseudo inverse of the interaction matrix
for (unsigned int k=0 ; k < error.getRows()/2 ; k++)
{
W[2*k][2*k] = w[k] ;
W[2*k+1][2*k+1] = w[k] ;
}
// compute the pseudo inverse of the interaction matrix
vpMatrix Lp ;
(W*L).pseudoInverse(Lp,1e-6) ;
// compute the VVS control law
v = -lambda*Lp*W*error ;
cMo = vpExponentialMap::direct(v).inverse()*cMo ; ;
if (iter++>vvsIterMax) break ;
}
if(computeCovariance)
covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*error, W*W); // Remark: W*W = W*W.t() since the matrix is diagonale, but using W*W is more efficient.
}
catch(...)
{
vpERROR_TRACE(" ") ;
throw ;
}
}
double
vpHomography::computeRotation(unsigned int nbpoint,
vpPoint *c1P,
vpPoint *c2P,
vpHomogeneousMatrix &c2Mc1,
int userobust
)
{
vpColVector e(2) ;
double r_1 = -1 ;
vpColVector p2(3) ;
vpColVector p1(3) ;
vpColVector Hp2(3) ;
vpColVector Hp1(3) ;
vpMatrix H2(2,3) ;
vpColVector e2(2) ;
vpMatrix H1(2,3) ;
vpColVector e1(2) ;
int only_1 = 0 ;
int only_2 = 0 ;
int iter = 0 ;
unsigned int n=0 ;
for (unsigned int i=0 ; i < nbpoint ; i++) {
// if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
if ( (std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.)))
&&
(std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.))) )
{
n++ ;
}
}
if ((only_1==1) || (only_2==1)) ; else n *=2 ;
vpRobust robust(n);
vpColVector res(n) ;
vpColVector w(n) ;
w =1 ;
robust.setThreshold(0.0000) ;
vpMatrix W(2*n,2*n) ;
W = 0 ;
vpMatrix c2Rc1(3,3) ;
double r =0 ;
while (vpMath::equal(r_1,r,threshold_rotation) == false )
{
r_1 =r ;
// compute current position
//Change frame (current)
for (unsigned int i=0 ; i < 3 ; i++)
for (unsigned int j=0 ; j < 3 ; j++)
c2Rc1[i][j] = c2Mc1[i][j] ;
vpMatrix L(2,3), Lp ;
int k =0 ;
for (unsigned int i=0 ; i < nbpoint ; i++) {
//if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
if ( (std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.)))
&&
(std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.))) )
{
p2[0] = c2P[i].get_x() ;
p2[1] = c2P[i].get_y() ;
p2[2] = 1.0 ;
p1[0] = c1P[i].get_x() ;
p1[1] = c1P[i].get_y() ;
p1[2] = 1.0 ;
Hp2 = c2Rc1.t()*p2 ; // p2 = Hp1
Hp1 = c2Rc1*p1 ; // p1 = Hp2
Hp2 /= Hp2[2] ; // normalisation
Hp1 /= Hp1[2] ;
// set up the interaction matrix
double x = Hp2[0] ;
double y = Hp2[1] ;
H2[0][0] = x*y ; H2[0][1] = -(1+x*x) ; H2[0][2] = y ;
H2[1][0] = 1+y*y ; H2[1][1] = -x*y ; H2[1][2] = -x ;
H2 *=-1 ;
H2 = H2*c2Rc1.t() ;
// Set up the error vector
e2[0] = Hp2[0] - c1P[i].get_x() ;
e2[1] = Hp2[1] - c1P[i].get_y() ;
// set up the interaction matrix
x = Hp1[0] ;
y = Hp1[1] ;
H1[0][0] = x*y ; H1[0][1] = -(1+x*x) ; H1[0][2] = y ;
H1[1][0] = 1+y*y ; H1[1][1] = -x*y ; H1[1][2] = -x ;
// Set up the error vector
e1[0] = Hp1[0] - c2P[i].get_x() ;
e1[1] = Hp1[1] - c2P[i].get_y() ;
if (only_2==1)
{
if (k == 0) { L = H2 ; e = e2 ; }
else
{
L = vpMatrix::stackMatrices(L,H2) ;
e = vpMatrix::stackMatrices(e,e2) ;
}
}
else
if (only_1==1)
{
if (k == 0) { L = H1 ; e= e1 ; }
else
{
L = vpMatrix::stackMatrices(L,H1) ;
e = vpMatrix::stackMatrices(e,e1) ;
}
}
else
{
if (k == 0) {L = H2 ; e = e2 ; }
else
{
L = vpMatrix::stackMatrices(L,H2) ;
e = vpMatrix::stackMatrices(e,e2) ;
}
L = vpMatrix::stackMatrices(L,H1) ;
e = vpMatrix::stackMatrices(e,e1) ;
}
k++ ;
}
}
if (userobust)
{
robust.setIteration(0);
for (unsigned int k=0 ; k < n ; k++)
{
res[k] = vpMath::sqr(e[2*k]) + vpMath::sqr(e[2*k+1]) ;
}
robust.MEstimator(vpRobust::TUKEY, res, w);
// compute the pseudo inverse of the interaction matrix
for (unsigned int k=0 ; k < n ; k++)
{
W[2*k][2*k] = w[k] ;
W[2*k+1][2*k+1] = w[k] ;
}
}
else
{
for (unsigned int k=0 ; k < 2*n ; k++) W[k][k] = 1 ;
}
// CreateDiagonalMatrix(w, W) ;
(L).pseudoInverse(Lp, 1e-6) ;
// Compute the camera velocity
vpColVector c2Rc1, v(6) ;
c2Rc1 = -2*Lp*W*e ;
for (unsigned int i=0 ; i < 3 ; i++) v[i+3] = c2Rc1[i] ;
// only for simulation
updatePoseRotation(c2Rc1, c2Mc1) ;
r =e.sumSquare() ;
if ((W*e).sumSquare() < 1e-10) break ;
if (iter>25) break ;
iter++ ; // std::cout << iter <<" e=" <<(e).sumSquare() <<" e=" <<(W*e).sumSquare() <<std::endl ;
}
// std::cout << c2Mc1 <<std::endl ;
return (W*e).sumSquare() ;
}
double
vpHomography::computeDisplacement(unsigned int nbpoint,
vpPoint *c1P,
vpPoint *c2P,
vpPlane *oN,
vpHomogeneousMatrix &c2Mc1,
vpHomogeneousMatrix &c1Mo,
int userobust
)
{
vpColVector e(2) ;
double r_1 = -1 ;
vpColVector p2(3) ;
vpColVector p1(3) ;
vpColVector Hp2(3) ;
vpColVector Hp1(3) ;
vpMatrix H2(2,6) ;
vpColVector e2(2) ;
vpMatrix H1(2,6) ;
vpColVector e1(2) ;
int only_1 = 1 ;
int only_2 = 0 ;
int iter = 0 ;
unsigned int i ;
unsigned int n=0 ;
n = nbpoint ;
if ((only_1==1) || (only_2==1)) ; else n *=2 ;
vpRobust robust(n);
vpColVector res(n) ;
vpColVector w(n) ;
w =1 ;
robust.setThreshold(0.0000) ;
vpMatrix W(2*n,2*n) ;
W = 0 ;
vpColVector N1(3), N2(3) ;
double d1, d2 ;
double r =1e10 ;
iter =0 ;
while (vpMath::equal(r_1,r,threshold_displacement) == false )
{
r_1 =r ;
// compute current position
//Change frame (current)
vpHomogeneousMatrix c1Mc2, c2Mo ;
vpRotationMatrix c1Rc2, c2Rc1 ;
vpTranslationVector c1Tc2, c2Tc1 ;
c1Mc2 = c2Mc1.inverse() ;
c2Mc1.extract(c2Rc1) ;
c2Mc1.extract(c2Tc1) ;
c2Mc1.extract(c1Rc2) ;
c1Mc2.extract(c1Tc2) ;
c2Mo = c2Mc1*c1Mo ;
vpMatrix L(2,3), Lp ;
int k =0 ;
for (i=0 ; i < nbpoint ; i++)
{
getPlaneInfo(oN[i], c1Mo, N1, d1) ;
getPlaneInfo(oN[i], c2Mo, N2, d2) ;
p2[0] = c2P[i].get_x() ;
p2[1] = c2P[i].get_y() ;
p2[2] = 1.0 ;
p1[0] = c1P[i].get_x() ;
p1[1] = c1P[i].get_y() ;
p1[2] = 1.0 ;
vpMatrix H(3,3) ;
Hp2 = ((vpMatrix)c1Rc2 + ((vpMatrix)c1Tc2*N2.t())/d2)*p2 ; // p2 = Hp1
Hp1 = ((vpMatrix)c2Rc1 + ((vpMatrix)c2Tc1*N1.t())/d1)*p1 ; // p1 = Hp2
Hp2 /= Hp2[2] ; // normalisation
Hp1 /= Hp1[2] ;
// set up the interaction matrix
double x = Hp2[0] ;
double y = Hp2[1] ;
double Z1 ;
Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ; // 1/z
H2[0][0] = -Z1 ; H2[0][1] = 0 ; H2[0][2] = x*Z1 ;
H2[1][0] = 0 ; H2[1][1] = -Z1 ; H2[1][2] = y*Z1 ;
H2[0][3] = x*y ; H2[0][4] = -(1+x*x) ; H2[0][5] = y ;
H2[1][3] = 1+y*y ; H2[1][4] = -x*y ; H2[1][5] = -x ;
H2 *=-1 ;
vpMatrix c1CFc2(6,6) ;
{
vpMatrix sTR = c1Tc2.skew()*(vpMatrix)c1Rc2 ;
for (unsigned int k=0 ; k < 3 ; k++)
for (unsigned int l=0 ; l<3 ; l++)
{
c1CFc2[k][l] = c1Rc2[k][l] ;
c1CFc2[k+3][l+3] = c1Rc2[k][l] ;
c1CFc2[k][l+3] = sTR[k][l] ;
}
}
H2 = H2*c1CFc2 ;
// Set up the error vector
e2[0] = Hp2[0] - c1P[i].get_x() ;
e2[1] = Hp2[1] - c1P[i].get_y() ;
x = Hp1[0] ;
y = Hp1[1] ;
Z1 = (N2[0]*x+N2[1]*y+N2[2])/d2 ; // 1/z
H1[0][0] = -Z1 ; H1[0][1] = 0 ; H1[0][2] = x*Z1 ;
H1[1][0] = 0 ; H1[1][1] = -Z1 ; H1[1][2] = y*Z1;
H1[0][3] = x*y ; H1[0][4] = -(1+x*x) ; H1[0][5] = y ;
H1[1][3] = 1+y*y ; H1[1][4] = -x*y ; H1[1][5] = -x ;
// Set up the error vector
e1[0] = Hp1[0] - c2P[i].get_x() ;
e1[1] = Hp1[1] - c2P[i].get_y() ;
if (only_2==1)
{
if (k == 0) { L = H2 ; e = e2 ; }
else
{
L = vpMatrix::stackMatrices(L,H2) ;
e = vpMatrix::stackMatrices(e,e2) ;
}
}
else
if (only_1==1)
{
if (k == 0) { L = H1 ; e= e1 ; }
else
{
L = vpMatrix::stackMatrices(L,H1) ;
e = vpMatrix::stackMatrices(e,e1) ;
}
}
else
{
if (k == 0) {L = H2 ; e = e2 ; }
else
{
L = vpMatrix::stackMatrices(L,H2) ;
e = vpMatrix::stackMatrices(e,e2) ;
}
L = vpMatrix::stackMatrices(L,H1) ;
e = vpMatrix::stackMatrices(e,e1) ;
}
k++ ;
}
if (userobust)
{
robust.setIteration(0);
for (unsigned int k=0 ; k < n ; k++)
{
res[k] = vpMath::sqr(e[2*k]) + vpMath::sqr(e[2*k+1]) ;
}
robust.MEstimator(vpRobust::TUKEY, res, w);
// compute the pseudo inverse of the interaction matrix
for (unsigned int k=0 ; k < n ; k++)
{
W[2*k][2*k] = w[k] ;
W[2*k+1][2*k+1] = w[k] ;
}
}
else
{
for (unsigned int k=0 ; k < 2*n ; k++) W[k][k] = 1 ;
}
(W*L).pseudoInverse(Lp, 1e-16) ;
// Compute the camera velocity
vpColVector c2Tcc1 ;
c2Tcc1 = -1*Lp*W*e ;
// only for simulation
c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse()*c2Mc1 ; ;
// UpdatePose2(c2Tcc1, c2Mc1) ;
r =(W*e).sumSquare() ;
if (r < 1e-15) {break ; }
if (iter>1000){break ; }
if (r>r_1) { break ; }
iter++ ;
}
return (W*e).sumSquare() ;
}
AbstractBuffer<int32_t> ADCensus::constructDisparityMap(const AbstractBuffer<pixel> *leftImage, const AbstractBuffer<pixel> *rightImage,
const AbstractBuffer<grayPixel> *leftGrayImage, const AbstractBuffer<grayPixel> *rightGrayImage) {
// Initialization
int width = leftImage->w;
int height = leftImage->h;
BaseTimeStatisticsCollector collector;
Statistics outerStats;
outerStats.setValue("H", height);
outerStats.setValue("W", width);
AbstractBuffer<int32_t> bestDisparities = AbstractBuffer<int32_t>(height, width);
AbstractBuffer<COST_TYPE> minCosts = AbstractBuffer<COST_TYPE>(height, width);
minCosts.fillWith(-1);
// Disparity computation
outerStats.startInterval();
AbstractBuffer<int64_t> leftCensus = AbstractBuffer<int64_t>(height, width);
AbstractBuffer<int64_t> rightCensus = AbstractBuffer<int64_t>(height, width);
makeCensus(leftGrayImage, leftCensus);
makeCensus(rightGrayImage, rightCensus);
outerStats.resetInterval("Making census");
makeAggregationCrosses(leftImage);
outerStats.resetInterval("Making aggregation crosses");
for (uint i = 0; i < CORE_COUNT_OF(table1); i++)
{
table1[i] = robust(i, lambdaCT);
table2[i] = robust(i, lambdaAD);
}
bool parallelDisp = true;
parallelable_for(0, width / 3,
[this, &minCosts, &bestDisparities, &leftImage, &rightImage,
&leftCensus, &rightCensus, &collector, height, width, parallelDisp](const BlockedRange<int> &r)
{
for (int d = r.begin(); d != r.end(); ++d) {
Statistics stats;
stats.startInterval();
AbstractBuffer<COST_TYPE> costs = AbstractBuffer<COST_TYPE>(height, width);
stats.resetInterval("Matrix construction");
parallelable_for(windowHh, height - windowHh,
[this, &costs, &leftImage, &rightImage, &leftCensus, &rightCensus, d, width](const BlockedRange<int> &r)
{
for (int y = r.begin(); y != r.end(); ++y) {
auto *im1 = &leftImage->element(y, windowWh + d);
auto *im2 = &rightImage->element(y, windowWh);
int64_t *cen1 = &leftCensus.element(y, windowWh + d);
int64_t *cen2 = &rightCensus.element(y, windowWh);
int x = windowWh + d;
#ifdef WITH_SSE
for (; x < width - windowWh; x += 8) {
FixedVector<Int16x8, 4> c1 = SSEReader8BBBB_DDDD::read((uint32_t *)im1);
FixedVector<Int16x8, 4> c2 = SSEReader8BBBB_DDDD::read((uint32_t *)im2);
UInt16x8 dr = SSEMath::difference(UInt16x8(c1[RGBColor::FIELD_R]), UInt16x8(c2[RGBColor::FIELD_R]));
UInt16x8 dg = SSEMath::difference(UInt16x8(c1[RGBColor::FIELD_G]), UInt16x8(c2[RGBColor::FIELD_G]));
UInt16x8 db = SSEMath::difference(UInt16x8(c1[RGBColor::FIELD_B]), UInt16x8(c2[RGBColor::FIELD_B]));
UInt16x8 ad = (dr + dg + db) >> 2;
Int16x8 cost_ad = Int16x8(robustLUTAD(ad[0]),
robustLUTAD(ad[1]),
robustLUTAD(ad[2]),
robustLUTAD(ad[3]),
robustLUTAD(ad[4]),
robustLUTAD(ad[5]),
robustLUTAD(ad[6]),
robustLUTAD(ad[7]));
Int64x2 cen10(&cen1[0]);
Int64x2 cen12(&cen1[2]);
Int64x2 cen14(&cen1[4]);
Int64x2 cen16(&cen1[6]);
Int64x2 cen20(&cen2[0]);
Int64x2 cen22(&cen2[2]);
Int64x2 cen24(&cen2[4]);
Int64x2 cen26(&cen2[6]);
Int64x2 diff0 = cen10 ^ cen20;
Int64x2 diff2 = cen12 ^ cen22;
Int64x2 diff4 = cen14 ^ cen24;
Int64x2 diff6 = cen16 ^ cen26;
Int16x8 cost_ct(robustLUTCen(_mm_popcnt_u64(diff0.getInt(0))), robustLUTCen(_mm_popcnt_u64(diff0.getInt(1))),
robustLUTCen(_mm_popcnt_u64(diff2.getInt(0))), robustLUTCen(_mm_popcnt_u64(diff2.getInt(1))),
robustLUTCen(_mm_popcnt_u64(diff4.getInt(0))), robustLUTCen(_mm_popcnt_u64(diff4.getInt(1))),
robustLUTCen(_mm_popcnt_u64(diff6.getInt(0))), robustLUTCen(_mm_popcnt_u64(diff6.getInt(1))));
Int16x8 cost_total = cost_ad + cost_ct;
for (int i = 0; i < 8; ++i) {
costs.element(y, x + i) = cost_total[i];
}
im1 += 8;
im2 += 8;
cen1+= 8;
cen2+= 8;
}
#else
for (; x < width - windowWh; ++x) {
uint8_t c_ad = costAD(*im1, *im2);
uint8_t c_census = hammingDist(*cen1, *cen2);
costs.element(y, x) = robustLUTCen(c_census) + robustLUTAD(c_ad);
im1 ++;
im2 ++;
cen1++;
cen2++;
}
#endif
}
}, !parallelDisp
);
stats.resetInterval("Cost computation");
aggregateCosts(&costs, windowWh + d, windowHh, width - windowWh, height - windowHh);
stats.resetInterval("Cost aggregation");
for (int x = windowWh + d; x < width - windowWh; ++x) {
for (int y = windowHh; y < height - windowHh; ++y) {
tbb::mutex::scoped_lock(bestDisparitiesMutex);
if(costs.element(y, x) < minCosts.element(y, x)) {
minCosts.element(y, x) = costs.element(y, x);
bestDisparities.element(y, x) = d;
//result.element(y,x) = (bestDisparities.element(y, x) / (double)width * 255 * 3);
}
}
}
//BMPLoader().save("../../result.bmp", result);
stats.endInterval("Comparing with previous minimum");
collector.addStatistics(stats);
}
}, parallelDisp);
