/*
pA: the pointer of the Matrix A|b
rowMax,colMax:size of matirx
pX: solution vector
return value: -1 solving equation failed
1 resolve result successfully
*/
int gaussLesung(double *pA, int rowMax,int colMax, double *pX)
{
double *pMatrix = pA;
int i;
double product;
product = 1;
if((1+rowMax)!=colMax) return -1;
for(i = 0; i < rowMax; i++)
{
int l;
printf("%d,\n",i);
l = findMax(pMatrix, rowMax, colMax, i,i);
exchangeRow(pMatrix, rowMax, colMax, l, i);
eliminate(pMatrix, rowMax, colMax, i, i);
outputMatrix(pMatrix, rowMax, colMax);
}
for(i = 0; i < rowMax; i++)product = product * pMatrix[i*colMax+i];
if(product==0)return -1;
solveX(pMatrix, rowMax,colMax, pX);
return 1;
}
void PointCloudProcessing::getRTGeometricLinearSystem(PointCloudC::Ptr corrRef,
PointCloudC::Ptr corrNew,
Eigen::Matrix4f &transMat)
{
// build linear system Ax = b;
int rows = 3*corrRef->points.size();
int cols = 6;
Eigen::MatrixXf A(rows, cols); A.setZero();
Eigen::MatrixXf b(rows, 1); b.setZero();
for(int i=0; i<corrRef->points.size(); i++)
{
float X1 = corrRef->points.at(i).x;
float Y1 = corrRef->points.at(i).y;
float Z1 = corrRef->points.at(i).z;
float X0 = corrNew->points.at(i).x;
float Y0 = corrNew->points.at(i).y;
float Z0 = corrNew->points.at(i).z;
A(3*i, 0) = 0; A(3*i, 1) = Z0+Z1; A(3*i, 2) = -Y0-Y1; A(3*i, 3) = 1;
A(3*i+1, 0) = -Z0-Z1; A(3*i+1, 1) = 0; A(3*i+1, 2) = X0+X1; A(3*i+1, 4) = 1;
A(3*i+2, 0) = Y0+Y1; A(3*i+1, 1) = -X0-X1; A(3*i+2, 2) = 0; A(3*i+2, 5) = 1;
b(3*i, 0) = X1-X0;
b(3*i+1, 0) = Y1-Y0;
b(3*i+2, 0) = Z1-Z0;
}
// std::cout<<"A = "<<A<<std::endl;
// std::cout<<"b = "<<b<<std::endl;
Eigen::MatrixXf solveX(rows, 1); solveX.setZero();
solveX = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
// std::cout<<"solveX: "<<solveX<<"\n";
// solveX = [Rx, Ry, Rz, T'x, T'y, T'z];
Eigen::Matrix3f I_Vx; I_Vx.setOnes();
I_Vx(0,0) = 1; I_Vx(0,1) = solveX(2); I_Vx(0,2) = -solveX(1);
I_Vx(1,0) = -solveX(2); I_Vx(1,1) = 1; I_Vx(1,2) = solveX(0);
I_Vx(2,0) = solveX(1); I_Vx(2,1) = -solveX(0); I_Vx(2,2) = 1;
Eigen::Vector3f Tx; Tx.setZero();
Tx(0) = solveX(3); Tx(1) = solveX(4); Tx(2) = solveX(5);
Eigen::Vector3f tx; tx.setZero();
tx = I_Vx.inverse()*Tx;
transMat(0,3) = tx(0); transMat(1,3) = tx(1); transMat(2,3) = tx(2);
Eigen::Matrix3f IplusVx; IplusVx.setOnes();
IplusVx(0,0) = 1; IplusVx(0,1) = -solveX(2); IplusVx(0,2) = solveX(1);
IplusVx(1,0) = solveX(2); IplusVx(1,1) = 1; IplusVx(1,2) = -solveX(0);
IplusVx(2,0) = -solveX(1); IplusVx(2,1) = solveX(0); IplusVx(2,2) = 1;
Eigen::Matrix3f Ro; Ro.setZero();
Ro = I_Vx.inverse()*IplusVx;
transMat(0,0) = Ro(0,0); transMat(0,1) = Ro(0,1); transMat(0,2) = Ro(0,2); transMat(0,3) = tx(0);
transMat(1,0) = Ro(1,0); transMat(1,1) = Ro(1,1); transMat(1,2) = Ro(1,2); transMat(1,3) = tx(1);
transMat(2,0) = Ro(2,0); transMat(2,1) = Ro(2,1); transMat(2,2) = Ro(2,2); transMat(2,3) = tx(2);
// std::cout<<"transMat Geometric: "<<transMat<<"\n";
}
// bi-conjugate linear equation solver
// overwrites pyramid!
static void linbcg(pyramid_t* pyramid, pyramid_t* pC, float* const b, float* const x, const int itmax, const float tol, pfstmo_progress_callback progress_cb)
{
const int rows = pyramid->rows;
const int cols = pyramid->cols;
const int n = rows*cols;
const float tol2 = tol*tol;
float* const z = matrix_alloc(n);
float* const zz = matrix_alloc(n);
float* const p = matrix_alloc(n);
float* const pp = matrix_alloc(n);
float* const r = matrix_alloc(n);
float* const rr = matrix_alloc(n);
float* const x_save = matrix_alloc(n);
const float bnrm2 = matrix_DotProduct(n, b, b);
multiplyA(pyramid, pC, x, r); // r = A*x = divergence(x)
matrix_subtract(n, b, r); // r = b - r
float err2 = matrix_DotProduct(n, r, r); // err2 = r.r
// matrix_copy(n, r, rr); // rr = r
multiplyA(pyramid, pC, r, rr); // rr = A*r
float bkden = 0;
float saved_err2 = err2;
matrix_copy(n, x, x_save);
const float ierr2 = err2;
const float percent_sf = 100.0f/logf(tol2*bnrm2/ierr2);
int iter = 0;
bool reset = true;
int num_backwards = 0;
const int num_backwards_ceiling = 3;
for (; iter < itmax; iter++)
{
if( progress_cb != NULL )
progress_cb( (int) (logf(err2/ierr2)*percent_sf));
solveX(n, r, z); // z = ~A(-1) * r = -0.25 * r
solveX(n, rr, zz); // zz = ~A(-1) * rr = -0.25 * rr
const float bknum = matrix_DotProduct(n, z, rr);
if(reset)
{
reset = false;
matrix_copy(n, z, p);
matrix_copy(n, zz, pp);
}
else
{
const float bk = bknum / bkden; // beta = ...
#pragma omp parallel for schedule(static)
for (int i = 0; i < n; i++)
{
p[i] = z[i] + bk * p[i];
pp[i] = zz[i] + bk * pp[i];
}
}
bkden = bknum; // numerato becomes the dominator for the next iteration
multiplyA(pyramid, pC, p, z); // z = A* p = divergence( p)
multiplyA(pyramid, pC, pp, zz); // zz = A*pp = divergence(pp)
const float ak = bknum / matrix_DotProduct(n, z, pp); // alfa = ...
#pragma omp parallel for schedule(static)
for(int i = 0 ; i < n ; i++ )
{
r[i] -= ak * z[i]; // r = r - alfa * z
rr[i] -= ak * zz[i]; //rr = rr - alfa * zz
}
const float old_err2 = err2;
err2 = matrix_DotProduct(n, r, r);
// Have we gone unstable?
if (err2 > old_err2)
{
// Save where we've got to if it's the best yet
if (num_backwards == 0 && old_err2 < saved_err2)
{
saved_err2 = old_err2;
matrix_copy(n, x, x_save);
}
num_backwards++;
}
else
{
num_backwards = 0;
}
#pragma omp parallel for schedule(static)
for(int i = 0 ; i < n ; i++ )
x[i] += ak * p[i]; // x = x + alfa * p
if (num_backwards > num_backwards_ceiling)
{
// Reset
reset = true;
num_backwards = 0;
// Recover saved value
matrix_copy(n, x_save, x);
// r = Ax
multiplyA(pyramid, pC, x, r);
// r = b - r
matrix_subtract(n, b, r);
// err2 = r.r
err2 = matrix_DotProduct(n, r, r);
saved_err2 = err2;
// rr = A*r
multiplyA(pyramid, pC, r, rr);
}
// fprintf(stderr, "iter:%d err:%f\n", iter+1, sqrtf(err2/bnrm2));
if(err2/bnrm2 < tol2)
break;
}
// Use the best version we found
if (err2 > saved_err2)
{
err2 = saved_err2;
matrix_copy(n, x_save, x);
}
if (err2/bnrm2 > tol2)
{
// Not converged
if( progress_cb != NULL )
progress_cb( (int) (logf(err2/ierr2)*percent_sf));
if (iter == itmax)
fprintf(stderr, "\npfstmo_mantiuk06: Warning: Not converged (hit maximum iterations), error = %g (should be below %g).\n", sqrtf(err2/bnrm2), tol);
else
fprintf(stderr, "\npfstmo_mantiuk06: Warning: Not converged (going unstable), error = %g (should be below %g).\n", sqrtf(err2/bnrm2), tol);
}
else if (progress_cb != NULL)
progress_cb(100);
matrix_free(x_save);
matrix_free(p);
matrix_free(pp);
matrix_free(z);
matrix_free(zz);
matrix_free(r);
matrix_free(rr);
}
