void KF_joseph_update(VectorXf &x, MatrixXf &P,float v,float R, MatrixXf H)
{
VectorXf PHt = P*H.transpose();
MatrixXf S = H*PHt;
S(0,0) += R;
MatrixXf Si = S.inverse();
Si = make_symmetric(Si);
MatrixXf PSD_check = Si.llt().matrixL(); //chol of scalar is sqrt
PSD_check.transpose();
PSD_check.conjugate();
VectorXf W = PHt*Si;
x = x+W*v;
//Joseph-form covariance update
MatrixXf eye(P.rows(), P.cols());
eye.setIdentity();
MatrixXf C = eye - W*H;
P = C*P*C.transpose() + W*R*W.transpose();
float eps = 2.2204*pow(10.0,-16); //numerical safety
P = P+eye*eps;
PSD_check = P.llt().matrixL();
PSD_check.transpose();
PSD_check.conjugate(); //for upper tri
}
VectorXf param_sensitivity_widget::computeSensitivity(
MatrixXf ¶meterMatrix, VectorXf &responseVector)
{
MatrixXf Ctemp = parameterMatrix.transpose()*parameterMatrix;
MatrixXf C;
C = Ctemp.inverse();
VectorXf b = C*parameterMatrix.transpose()*responseVector;
VectorXf Y_hat = parameterMatrix*b;
int p = b.rows();
VectorXf sigma2Vec = responseVector-Y_hat;
float sigma2 = sigma2Vec.squaredNorm();
sigma2= sigma2/(parameterMatrix.rows() - p);
Ctemp = C*sigma2;
MatrixXf denominator = Ctemp.diagonal();
// Do element-wise division
VectorXf t = b;
for (int i = 0; i < b.rows(); i++)
{
t(i) = abs(b(i)/sqrt(denominator(i)));
}
return t;
}
MatrixXf Arm::pseudo_inverse() {
MatrixXf Jacovian = compute_Jacobian();
MatrixXf jjtInv = (Jacovian * Jacovian.transpose());
jjtInv = jjtInv.inverse();
return (Jacovian.transpose() * jjtInv);
}
void call_ref()
{
VectorXcf ca = VectorXcf::Random(10);
VectorXf a = VectorXf::Random(10);
RowVectorXf b = RowVectorXf::Random(10);
MatrixXf A = MatrixXf::Random(10,10);
RowVector3f c = RowVector3f::Random();
const VectorXf& ac(a);
VectorBlock<VectorXf> ab(a,0,3);
const VectorBlock<VectorXf> abc(a,0,3);
VERIFY_EVALUATION_COUNT( call_ref_1(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(b,b.transpose()), 0);
// call_ref_1(ac,a<c); // does not compile because ac is const
VERIFY_EVALUATION_COUNT( call_ref_1(ab,ab), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(a.head(4),a.head(4)), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(abc,abc), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(A.col(3),A.col(3)), 0);
// call_ref_1(A.row(3),A.row(3)); // does not compile because innerstride!=1
VERIFY_EVALUATION_COUNT( call_ref_3(A.row(3),A.row(3).transpose()), 0);
VERIFY_EVALUATION_COUNT( call_ref_4(A.row(3),A.row(3).transpose()), 0);
// call_ref_1(a+a, a+a); // does not compile for obvious reason
MatrixXf tmp = A*A.col(1);
VERIFY_EVALUATION_COUNT( call_ref_2(A*A.col(1), tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_2(ac.head(5),ac.head(5)), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(ac,ac), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(ab,ab), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(a.head(4),a.head(4)), 0);
tmp = a+a;
VERIFY_EVALUATION_COUNT( call_ref_2(a+a,tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_2(ca.imag(),ca.imag()), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_4(ac.head(5),ac.head(5)), 0);
tmp = a+a;
VERIFY_EVALUATION_COUNT( call_ref_4(a+a,tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_4(ca.imag(),ca.imag()), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(a.head(3),a.head(3)), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(A,A), 0);
// call_ref_5(A.transpose(),A.transpose()); // does not compile because storage order does not match
VERIFY_EVALUATION_COUNT( call_ref_5(A.block(1,1,2,2),A.block(1,1,2,2)), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(b,b), 0); // storage order do not match, but this is a degenerate case that should work
VERIFY_EVALUATION_COUNT( call_ref_5(a.row(3),a.row(3)), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(a.head(3),a.head(3)), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(A.row(3),A.row(3)), 1); // evaluated into a temp thouth it could be avoided by viewing it as a 1xn matrix
tmp = A+A;
VERIFY_EVALUATION_COUNT( call_ref_6(A+A,tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_6(A,A), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(A.transpose(),A.transpose()), 1); // evaluated into a temp because the storage orders do not match
VERIFY_EVALUATION_COUNT( call_ref_6(A.block(1,1,2,2),A.block(1,1,2,2)), 0);
VERIFY_EVALUATION_COUNT( call_ref_7(c,c), 0);
}
FeatureModel::FeatureModel( const CamModel & _camera,
const MotionModel & _motion,
const FeaturesState & _featuresState,
const Mat & frame,
const Point2f & pf,
int patchSize,
float rho_0,
float sigma_rho):
camera(_camera),
motion(_motion),
features(_featuresState)
{
Mat newPatch(cv::Mat(frame, cv::Rect(pf.x-patchSize/2, pf.y-patchSize/2, patchSize,patchSize)));
this->imageLocation << pf.x, pf.y;
this->Patch = newPatch.clone();
Vector2f hd = (Vector2f << (float)pf.x, (float)pf.y);
// TODO: convert using motionmodel
Vector3f r = motion->get_r();
Quatenionf q = motion->get_q();
Vector3f hC = this->camera.unproject(hd, true);
Matrix3f Jac_hCHd = this->camera.getUnprojectionJacobian();
Matrix3f Rot = quat2rot(q);
Vector3f hW = Rot*hC;
float hx = hW(0);
float hy = hW(1);
float hz = hW(2);
float theta = atan2(hx,hz);
float phi = atan2(-hy, sqrt(hx*hx+hz*hz));
// Updating state and Sigma
MatrixXf Jx = this->computeJx();
MatrixXf Jm = this->computeJm();
Matrix2f Sigma_mm = this->computeSigma_mm(sigma_pixel, sigma_pixel);
this->f = (VectorXf(6) << r, theta, phi, rho_0);
this->Sigma_ii = Jm*Sigma_mm*Jm.transpose() + Jx*motion->getSigma_xx()*Jx.transpose();
this->motion->updateVariaceBlocks(Jx);
this->features->updateVariaceBlocks(Jx);
this->features.push_back((*this));
return 1;
}
MatrixXf LinkedStructure::pseudoInverse()
{
// Simple math that represents the mathematics
// explained on the website to computing the
// pseudo inverse. this is exactly the math
// discussed in the tutorial!!!
MatrixXf j = jacobian();
MatrixXf jjtInv = (j * j.transpose());
jjtInv = jjtInv.inverse();
return (j.transpose() * jjtInv);
}
void Lu::Initialize(MatrixXf x3d_h,MatrixXf x2d_h, Matrix3f A) {
x2d=x2d_h.transpose();
// std::cout<<"x2dtrasn"<<x2d<<std::endl;
x3d=x3d_h.transpose();
//std::cout<<"x3dtrasn"<<x3d<<std::endl;
x2dn=A.inverse ()*x2d;
//std::cout<<"x2dn"<<x2dn<<std::endl;
K=A;
//std::cout<<"Intialize points "<<std::endl;
}
void multi(dym tdim,MatrixXf &c)
{
if(dim==tdim)
data=data*c.transpose();
else
rotate_top(tdim),data=c*data;
}
void computeHomographyResidue(MatrixXf pts1, MatrixXf pts2, const Matrix3f &H, MatrixXf &residue){
// cross residue
filterPointAtInfinity(pts1, pts2);
residue.resize(pts1.rows(), 1);
MatrixXf Hx1 = (H*pts1.transpose()).transpose();
MatrixXf invHx2 = (H.inverse()*pts2.transpose()).transpose();
noHomogeneous(Hx1);
noHomogeneous(invHx2);
noHomogeneous(pts1);
noHomogeneous(pts2);
MatrixXf diffHx1pts2 = Hx1 - pts2;
MatrixXf diffinvHx2pts1 = invHx2 - pts1;
residue = diffHx1pts2.rowwise().squaredNorm() + diffinvHx2pts1.rowwise().squaredNorm();
}
void TestLeastSquares() {
MatrixXf A = MatrixXf::Random(10, 2);
VectorXf b = VectorXf::Random(10);
Vector2f x;
std::cout << "=============================" << std::endl;
std::cout << "Testing least squares solvers" << std::endl;
std::cout << "=============================" << std::endl;
x = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
std::cout << "Solution using Jacobi SVD = " << x.transpose() << std::endl;
x = A.colPivHouseholderQr().solve(b);
std::cout << "Solution using column pivoting Householder QR = " << x.transpose() << std::endl;
// If the matrix A is ill-conditioned, then this is not a good method
x = (A.transpose() * A).ldlt().solve(A.transpose() * b);
std::cout << "Solution using normal equation = " << x.transpose() << std::endl;
}
virtual VectorXf gradient( const MatrixXf & a, const MatrixXf & b ) const {
if (ktype_ == CONST_KERNEL)
return VectorXf();
MatrixXf fg = featureGradient( a, b );
if (ktype_ == DIAG_KERNEL)
return (f_.array()*fg.array()).rowwise().sum();
else {
MatrixXf p = fg*f_.transpose();
p.resize( p.cols()*p.rows(), 1 );
return p;
}
}
VectorXf project1D( const RMatrixXf & Y, int * rep_label=NULL ) {
// const int MAX_SAMPLE = 20000;
const bool fast = true, very_fast = true;
// Remove the DC (Y : N x M)
RMatrixXf dY = Y.rowwise() - Y.colwise().mean();
// RMatrixXf sY = dY;
// if( 0 < MAX_SAMPLE && MAX_SAMPLE < dY.rows() ) {
// VectorXi samples = randomChoose( dY.rows(), MAX_SAMPLE );
// std::sort( samples.data(), samples.data()+samples.size() );
// sY = RMatrixXf( samples.size(), dY.cols() );
// for( int i=0; i<samples.size(); i++ )
// sY.row(i) = dY.row( samples[i] );
// }
// ... and use (pc > 0)
VectorXf lbl = VectorXf::Zero( Y.rows() );
// Find the largest PC of (dY.T * dY) and project onto it
if( very_fast ) {
// Find the largest PC using poweriterations
VectorXf U = VectorXf::Random( dY.cols() );
U = U.array() / U.norm()+std::numeric_limits<float>::min();
for( int it=0; it<20; it++ ){
// Normalize
VectorXf s = dY.transpose()*(dY*U);
s.array() /= s.norm()+std::numeric_limits<float>::min();
if ( (U-s).norm() < 1e-6 )
break;
U = s;
}
// Project onto the PC
lbl = dY*U;
}
else if(fast) {
// Compute the eigen values of the covariance (and project onto the largest eigenvector)
MatrixXf cov = dY.transpose()*dY;
SelfAdjointEigenSolver<MatrixXf> eigensolver(0.5*(cov+cov.transpose()));
MatrixXf ev = eigensolver.eigenvectors();
lbl = dY * ev.col( ev.cols()-1 );
}
else {
// Use the SVD
JacobiSVD<RMatrixXf> svd = dY.jacobiSvd(ComputeThinU | ComputeThinV );
// Project onto the largest PC
lbl = svd.matrixU().col(0) * svd.singularValues()[0];
}
// Find the representative label
if( rep_label )
dY.array().square().rowwise().sum().minCoeff( rep_label );
return (lbl.array() < 0).cast<float>();
}
Image smooth_detector(const Image& source, Interpolation level, int r) {
Image output(source.rows(), source.columns(), 1, numeric_limits<float>::max());
const MatrixXf reg_matrix = ComputeRegMatrix(level, r);
const LDLT<MatrixXf> solver = (reg_matrix.transpose() * reg_matrix).ldlt();
for (int pr = 0; pr <= source.rows() - r; ++pr) {
for (int pc = 0; pc <= source.columns() - r; ++pc) {
VectorXf dist = VectorXf::Zero(r * r);
for (int ch = 0; ch < source.channels(); ++ch) {
EigenImage y = ExtractPatch(source, r, pr, pc, ch);
VectorXf reg_surf = solver.solve(reg_matrix.transpose() * y.asvector());
dist += (reg_matrix * reg_surf - y.asvector()).cwiseAbs2();
}
dist = dist.cwiseSqrt();
for (int row = pr; row < min(output.rows(), pr + r); ++row) {
for (int col = pc; col < min(output.columns(), pc + r); ++col) {
output.val(col, row) = min(output.val(col, row), dist((row - pr) * r + col - pc));
}
}
}
}
return output;
}
int main(int, char**)
{
cout.precision(3);
SelfAdjointEigenSolver<MatrixXf> es(4);
MatrixXf X = MatrixXf::Random(4,4);
MatrixXf A = X + X.transpose();
es.compute(A);
cout << "The eigenvalues of A are: " << es.eigenvalues().transpose() << endl;
es.compute(A + MatrixXf::Identity(4,4)); // re-use es to compute eigenvalues of A+I
cout << "The eigenvalues of A+I are: " << es.eigenvalues().transpose() << endl;
return 0;
}
// todo: some weighting scheme
SparseArray calcCorrNN(const vector<btVector3>& estPts, const vector<btVector3>& obsPts, const vector<float>& pVis) {
int nEst = estPts.size();
int nObs = obsPts.size();
float r = (float)nObs/nEst;
SparseArray out(nEst);
MatrixXf distsEstObs = pairwiseSquareDist(toEigenMatrix(estPts), toEigenMatrix(obsPts));
vector<int> estToObs = argminAlongRows(distsEstObs);
vector<int> obsToEst = argminAlongRows(distsEstObs.transpose());
for (int iEst=0; iEst<nEst; iEst++)
out[iEst].push_back(IndVal(estToObs[iEst],.5*pVis[iEst]));
for (int iObs=0; iObs<nObs; iObs++) out[obsToEst[iObs]].push_back(IndVal(iObs,.5/r));
return out;
}
int main(int, char**)
{
cout.precision(3);
Matrix4f A = MatrixXf::Random(4,4);
cout << "Here is a random 4x4 matrix:" << endl << A << endl;
HessenbergDecomposition<MatrixXf> hessOfA(A);
MatrixXf H = hessOfA.matrixH();
cout << "The Hessenberg matrix H is:" << endl << H << endl;
MatrixXf Q = hessOfA.matrixQ();
cout << "The orthogonal matrix Q is:" << endl << Q << endl;
cout << "Q H Q^T is:" << endl << Q * H * Q.transpose() << endl;
return 0;
}
int main(int, char**)
{
cout.precision(3);
Tridiagonalization<MatrixXf> tri;
MatrixXf X = MatrixXf::Random(4,4);
MatrixXf A = X + X.transpose();
tri.compute(A);
cout << "The matrix T in the tridiagonal decomposition of A is: " << endl;
cout << tri.matrixT() << endl;
tri.compute(2*A); // re-use tri to compute eigenvalues of 2A
cout << "The matrix T in the tridiagonal decomposition of 2A is: " << endl;
cout << tri.matrixT() << endl;
return 0;
}
double MapOptimizer::compute_relentropy_gpu (MatrixXf & dH) const
{
initialize_P_gpu();
const MatrixXf & H = m_conditional;
MatrixXf PHt = H.transpose();
MatrixXf PtHt = PHt;
//TODO run cusparse in two different cuda streams
m_P->left_imul(PtHt, true);
m_Pt->left_imul(PHt, true);
MatrixXf HPHt(H.rows(), H.rows());
#ifdef HACK_TO_LIMIT_GPU_POWER_USAGE
HPHt.noalias() = H * PHt;
#else // HACK_TO_LIMIT_GPU_POWER_USAGE
Gpu::matrix_multiply(H, false, PHt, false, HPHt);
#endif // HACK_TO_LIMIT_GPU_POWER_USAGE
const float sum_P = m_dom_flow_sum;
const float sum_Q = m_cod_flow_sum;
const float dQ_scale = sum_P / sum_Q;
const MatrixSf & Q = cod_flow.joint;
MatrixSf & dQ = m_temp_dQ;
MatrixSf & dQt = m_temp_dQt;
double relentropy = 0;
for (int i = 0; i < Q.outerSize(); ++i) {
for (MatrixSf::InnerIterator iter(Q,i); iter; ++iter) {
const float Q_yy = iter.value();
const float HPH_yy = HPHt(iter.row(), iter.col());
const float dQ_yy = dQ_scale * Q_yy / HPH_yy;
relentropy += Q_yy * log(dQ_yy);
dQ.coeffRef(iter.row(), iter.col()) = dQ_yy;
dQt.coeffRef(iter.col(), iter.row()) = dQ_yy;
}
}
relentropy /= sum_Q;
ASSERT_LE(0, relentropy);
MatrixXf HP(H.rows(), H.cols());
MatrixXf HPt(H.rows(), H.cols());
#pragma omp sections
{
#pragma omp section
HP = PtHt.transpose();
#pragma omp section
HPt = PHt.transpose();
}
//TODO run cusparse in two different cuda streams
Gpu::SparseMultiplier(dQ).left_mul(HP, dH, true);
Gpu::SparseMultiplier(dQt).left_fma(HPt, dH, true);
return relentropy;
}
double NHitSeedFinder::fitTrack(SimpleTrack3D& track, vector<double>& chi2_hit)
{
if(using_vertex == true)
{
track.hits.push_back(SimpleHit3D(0.,0., 0.,0., 0.,0., 0, 0));
}
chi2_hit.clear();
chi2_hit.resize(track.hits.size(), 0.);
MatrixXf y = MatrixXf::Zero(track.hits.size(), 1);
for(unsigned int i=0;i<track.hits.size();i++)
{
y(i, 0) = ( pow(track.hits[i].x,2) + pow(track.hits[i].y,2) );
if((using_vertex==true ) && (i == (track.hits.size() - 1))){y(i, 0) /= vertex_sigma_xy;}
else{y(i, 0) /= layer_xy_resolution[track.hits[i].layer];}
}
MatrixXf X = MatrixXf::Zero(track.hits.size(), 3);
for(unsigned int i=0;i<track.hits.size();i++)
{
X(i, 0) = track.hits[i].x;
X(i, 1) = track.hits[i].y;
X(i, 2) = -1.;
if((using_vertex==true ) && (i == (track.hits.size() - 1)))
{
X(i, 0) /= vertex_sigma_xy;
X(i, 1) /= vertex_sigma_xy;
X(i, 2) /= vertex_sigma_xy;
}
else
{
X(i, 0) /= layer_xy_resolution[track.hits[i].layer];
X(i, 1) /= layer_xy_resolution[track.hits[i].layer];
X(i, 2) /= layer_xy_resolution[track.hits[i].layer];
}
}
MatrixXf Xt = X.transpose();
MatrixXf prod = Xt*X;
MatrixXf inv = prod.fullPivLu().inverse();
MatrixXf beta = inv*Xt*y;
float cx = beta(0,0)*0.5;
float cy = beta(1,0)*0.5;
float r = sqrt(cx*cx + cy*cy - beta(2,0));
float phi = atan2(cy, cx);
float d = sqrt(cx*cx + cy*cy) - r;
float k = 1./r;
MatrixXf diff = y - (X*beta);
MatrixXf chi2 = (diff.transpose())*diff;
float dx = d*cos(phi);
float dy = d*sin(phi);
MatrixXf y2 = MatrixXf::Zero(track.hits.size(), 1);
for(unsigned int i=0;i<track.hits.size();i++)
{
y2(i,0) = track.hits[i].z;
if((using_vertex==true ) && (i == (track.hits.size() - 1))){y2(i, 0) /= vertex_sigma_z;}
else{y2(i, 0) /= layer_z_resolution[track.hits[i].layer];}
}
MatrixXf X2 = MatrixXf::Zero(track.hits.size(), 2);
for(unsigned int i=0;i<track.hits.size();i++)
{
float D = sqrt( pow(dx - track.hits[i].x, 2) + pow(dy - track.hits[i].y,2));
float s = 0.0;
if(0.5*k*D > 0.1)
{
float v = 0.5*k*D;
if(v >= 0.999999){v = 0.999999;}
s = 2.*asin(v)/k;
}
else
{
float temp1 = k*D*0.5;temp1*=temp1;
float temp2 = D*0.5;
s += 2.*temp2;
temp2*=temp1;
s += temp2/3.;
temp2*=temp1;
s += (3./20.)*temp2;
temp2*=temp1;
s += (5./56.)*temp2;
}
X2(i,0) = s;
X2(i,1) = 1.0;
if((using_vertex==true ) && (i == (track.hits.size() - 1)))
{
X2(i, 0) /= vertex_sigma_z;
X2(i, 1) /= vertex_sigma_z;
}
else
{
X2(i, 0) /= layer_z_resolution[track.hits[i].layer];
X2(i, 1) /= layer_z_resolution[track.hits[i].layer];
}
}
MatrixXf Xt2 = X2.transpose();
MatrixXf prod2 = Xt2*X2;
MatrixXf inv2 = prod2.fullPivLu().inverse();
MatrixXf beta2 = inv2*Xt2*y2;
MatrixXf diff2 = y2 - (X2*beta2);
MatrixXf chi2_z = (diff2.transpose())*diff2;
float z0 = beta2(1,0);
float dzdl = beta2(0,0)/sqrt(1. + beta2(0,0)*beta2(0,0));
track.phi = phi;
track.d = d;
track.kappa = k;
track.dzdl = dzdl;
track.z0 = z0;
if(track.kappa!=0.)
{
r=1./track.kappa;
}
else
{
r=1.0e10;
}
cx = (track.d+r)*cos(track.phi);
cy = (track.d+r)*sin(track.phi);
float chi2_tot = 0.;
for(unsigned int h=0;h<track.hits.size();h++)
{
float dx1 = track.hits[h].x - cx;
float dy1 = track.hits[h].y - cy;
float dx2 = track.hits[h].x + cx;
float dy2 = track.hits[h].y + cy;
float xydiff1 = sqrt(dx1*dx1 + dy1*dy1) - r;
float xydiff2 = sqrt(dx2*dx2 + dy2*dy2) - r;
float xydiff = xydiff2;
if(fabs(xydiff1) < fabs(xydiff2)){ xydiff = xydiff1; }
float ls_xy = layer_xy_resolution[track.hits[h].layer];
if((using_vertex == true) && (h == (track.hits.size() - 1)))
{
ls_xy = vertex_sigma_xy;
}
chi2_hit[h] = 0.;
chi2_hit[h] += xydiff*xydiff/(ls_xy*ls_xy);
chi2_hit[h] += diff2(h,0)*diff2(h,0);
chi2_tot += chi2_hit[h];
}
unsigned int deg_of_freedom = 2*track.hits.size() - 5;
if(using_vertex == true)
{
track.hits.pop_back();
chi2_hit.pop_back();
}
return (chi2_tot)/((double)(deg_of_freedom));
}
inline
void r_and_t(MatrixXf &rot_cw, VectorXf &pos_cw,MatrixXf start_points, MatrixXf end_points,
MatrixXf P1w,MatrixXf P2w,MatrixXf initRot_cw,VectorXf initPos_cw,
int maxIterNum,float TerminateTh,int nargin)
{
if(nargin<6)
{
return;
}
if(nargin<8)
{
maxIterNum = 8;
TerminateTh = 1e-5;
}
int n = start_points.cols();
if(n != end_points.cols() || n!= P1w.cols() || n!= P2w.cols())
{
return;
}
if(n<4)
{
return;
}
//first compute the weight of each line and the normal of
//the interpretation plane passing through to camera center and the line
VectorXf w = VectorXf::Zero(n);
MatrixXf nc = MatrixXf::Zero(3,n);
for(int i = 0 ; i < n ; i++)
{
//the weight of a line is the inverse of its image length
w[i] = 1/(start_points.col(i)-end_points.col(i)).norm();
vfloat3 v1 = start_points.col(i);
vfloat3 v2 = end_points.col(i);
vfloat3 temp = v1.cross(v2);
nc.col(i) = temp/temp.norm();
}
MatrixXf rot_wc = initPos_cw.transpose();
MatrixXf pos_wc = - initRot_cw.transpose() * initPos_cw;
for(int iter = 1 ; iter < maxIterNum ; iter++)
{
//construct the equation (31)
MatrixXf A = MatrixXf::Zero(6,7);
MatrixXf C = MatrixXf::Zero(3,3);
MatrixXf D = MatrixXf::Zero(3,3);
MatrixXf F = MatrixXf::Zero(3,3);
vfloat3 c_bar = vfloat3(0,0,0);
vfloat3 d_bar = vfloat3(0,0,0);
for(int i = 0 ; i < n ; i++)
{
//for first point on line
vfloat3 Pi = rot_wc * P1w.col(i);
vfloat3 Ni = nc.col(i);
float wi = w[i];
vfloat3 bi = Pi.cross(Ni);
C = C + wi*Ni*Ni.transpose();
D = D + wi*bi*bi.transpose();
F = F + wi*Ni*bi.transpose();
vfloat3 tempi = Pi + pos_wc;
float scale = Ni.transpose() * tempi;
scale *= wi;
c_bar = c_bar + scale * Ni;
d_bar = d_bar + scale*bi;
//for second point on line
Pi = rot_wc * P2w.col(i);
Ni = nc.col(i);
wi = w[i];
bi = Pi.cross(Ni);
C = C + wi*Ni*Ni.transpose();
D = D + wi*bi*bi.transpose();
F = F + wi*Ni*bi.transpose();
scale = (Ni.transpose() * (Pi + pos_wc));
scale *= wi;
c_bar = c_bar + scale * Ni;
d_bar = d_bar + scale * bi;
}
A.block<3,3>(0,0) = C;
A.block<3,3>(0,3) = F;
(A.col(6)).segment(0,2) = c_bar;
A.block<3,3>(3,0) = F.transpose();
A.block<3,3>(2,2) = D;
(A.col(6)).segment(3,5) = d_bar;
//sovle the system by using SVD;
JacobiSVD<MatrixXf> svd(A, ComputeThinU | ComputeThinV);
VectorXf vec(7);
//the last column of Vmat;
vec = (svd.matrixV()).col(6);
//the condition that the last element of vec should be 1.
vec = vec/vec[6];
//update the rotation and translation parameters;
vfloat3 dT = vec.segment(0,2);
vfloat3 dOmiga = vec.segment(3,5);
MatrixXf rtemp(3,3);
rtemp << 1, -dOmiga[2], dOmiga[1], dOmiga[2], 1, -dOmiga[1], -dOmiga[1], dOmiga[0], 1;
rot_wc = rtemp * rot_wc;
//newRot_wc = ( I + [dOmiga]x ) oldRot_wc
//may be we can compute new R using rodrigues(r+dr)
pos_wc = pos_wc + dT;
if(dT.norm() < TerminateTh && dOmiga.norm() < 0.1*TerminateTh)
{
break;
}
}
rot_cw = rot_wc.transpose();
pos_cw = -rot_cw * pos_wc;
}
Tridiagonalization<MatrixXf> tri; MatrixXf X = MatrixXf::Random(4,4); MatrixXf A = X + X.transpose(); tri.compute(A); cout << "The matrix T in the tridiagonal decomposition of A is: " << endl; cout << tri.matrixT() << endl; tri.compute(2*A); // re-use tri to compute eigenvalues of 2A cout << "The matrix T in the tridiagonal decomposition of 2A is: " << endl; cout << tri.matrixT() << endl;
//compute proposal distribution, then sample from it, and compute new particle weight
void sample_proposal(Particle &particle, vector<VectorXf> z, vector<int> idf, MatrixXf R)
{
assert(isfinite(particle.w()));
VectorXf xv = VectorXf(particle.xv()); //robot position
MatrixXf Pv = MatrixXf(particle.Pv()); //controls (motion command)
VectorXf xv0 = VectorXf(xv);
MatrixXf Pv0 = MatrixXf(Pv);
vector<MatrixXf> Hv;
vector<MatrixXf> Hf;
vector<MatrixXf> Sf;
vector<VectorXf> zp;
VectorXf zpi;
MatrixXf Hvi;
MatrixXf Hfi;
MatrixXf Sfi;
//process each feature, incrementally refine proposal distribution
unsigned i,r;
vector<int> j;
for (i =0; i<idf.size(); i++) {
j.clear();
j.push_back(idf[i]);
Hv.clear();
Hf.clear();
Sf.clear();
zp.clear();
compute_jacobians(particle,j,R,zp,&Hv,&Hf,&Sf);
zpi = zp[0];
Hvi = Hv[0];
Hfi = Hf[0];
Sfi = Sf[0];
VectorXf vi = z[i] - zpi;
vi[1] = pi_to_pi(vi[1]);
//proposal covariance
Pv = Hvi.transpose() * Sfi * Hvi + Pv.inverse();
Pv = Pv.inverse();
//proposal mean
xv = xv + Pv * Hvi.transpose() * Sfi * vi;
particle.setXv(xv);
particle.setPv(Pv);
}
//sample from proposal distribution
VectorXf xvs = multivariate_gauss(xv,Pv,1);
particle.setXv(xvs);
MatrixXf zeros(3,3);
zeros.setZero();
particle.setPv(zeros);
//compute sample weight: w = w* p(z|xk) p(xk|xk-1) / proposal
float like = likelihood_given_xv(particle, z, idf, R);
float prior = gauss_evaluate(delta_xv(xv0,xvs), Pv0,0);
float prop = gauss_evaluate(delta_xv(xv,xvs),Pv,0);
assert(isfinite(particle.w()));
float a = prior/prop;
float b = particle.w() * a;
float newW = like * b;
//float newW = particle.w() * like * prior / prop;
#if 0
if (!isfinite(newW)) {
cout<<"LIKELIHOOD GIVEN XV INPUTS"<<endl;
cout<<"particle.w()"<<endl;
cout<<particle.w()<<endl;
cout<<"particle.xv()"<<endl;
cout<<particle.xv()<<endl;
cout<<"particle.Pv()"<<endl;
cout<<particle.Pv()<<endl;
cout<<"particle.xf"<<endl;
for (int i =0; i<particle.xf().size(); i++) {
cout<<particle.xf()[i]<<endl;
}
cout<<endl;
cout<<"particle.Pf()"<<endl;
for (int i =0; i< particle.Pf().size(); i++) {
cout<<particle.Pf()[i]<<endl;
}
cout<<endl;
cout<<"z"<<endl;
for (int i=0; i<z.size(); i++) {
cout<<z[i]<<endl;
}
cout<<endl;
cout<<"idf"<<endl;
for (int i =0; i<idf.size(); i++){
cout<<idf[i]<<" ";
}
cout<<endl;
cout<<"R"<<endl;
cout<<R<<endl;
cout<<"GAUSS EVALUATE INPUTS"<<endl;
cout<<"delta_xv(xv0,xvs)"<<endl;
cout<<delta_xv(xv0,xvs)<<endl;
cout<<"Pv0"<<endl;
cout<<Pv0<<endl;
cout<<"delta_xv(xv,xvs)"<<endl;
cout<<delta_xv(xv,xvs)<<endl;
cout<<"Pv"<<endl;
cout<<Pv<<endl;
cout<<"like"<<endl;
cout<<like<<endl;
cout<<"prior"<<endl;
cout<<prior<<endl;
cout<<"prop"<<endl;
cout<<prop<<endl;
}
#endif
particle.setW(newW);
}
IplImage* CloudProjection::computeProjection(const sensor_msgs::PointCloud& data,
const std::vector<int>& interest_region_indices)
{
// -- Put cluster points into matrix form.
MatrixXf points(interest_region_indices.size(), 3);
for(size_t i=0; i<interest_region_indices.size(); ++i) {
points(i, 0) = data.points[interest_region_indices[i]].x;
points(i, 1) = data.points[interest_region_indices[i]].y;
points(i, 2) = data.points[interest_region_indices[i]].z;
}
// -- Subtract off the mean and flatten to z=0 to prepare for PCA.
MatrixXf X = points;
X.col(2) = VectorXf::Zero(X.rows());
VectorXf pt_mean = X.colwise().sum() / (float)X.rows();
for(int i=0; i<X.rows(); ++i) {
X.row(i) -= pt_mean.transpose();
}
MatrixXf Xt = X.transpose();
// -- Find the long axis.
// Start with a random vector.
VectorXf pc = VectorXf::Zero(3);
pc(0) = 1; //Chosen by fair dice roll.
pc(1) = 1;
pc.normalize();
// Power method.
VectorXf prev = pc;
double thresh = 1e-4;
int ctr = 0;
while(true) {
prev = pc;
pc = Xt * (X * pc);
pc.normalize();
ctr++;
if((pc - prev).norm() < thresh)
break;
}
assert(abs(pc(2)) < 1e-4);
// -- Find the short axis.
VectorXf shrt = VectorXf::Zero(3);
shrt(1) = -pc(0);
shrt(0) = pc(1);
assert(abs(shrt.norm() - 1) < 1e-4);
assert(abs(shrt.dot(pc)) < 1e-4);
// -- Build the basis of normalized coordinates.
MatrixXf basis = MatrixXf::Zero(3,3);
basis.col(0) = pc;
basis.col(1) = shrt;
basis(2,2) = -1.0;
assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
assert(abs(basis.col(0).norm() - 1) < 1e-4);
assert(abs(basis.col(1).norm() - 1) < 1e-4);
assert(abs(basis.col(2).norm() - 1) < 1e-4);
// -- Put the cluster into normalized coordinates, and choose which axis to project on.
MatrixXf projected_basis(3, 2);
if(axis_ == 0) {
projected_basis.col(0) = basis.col(1);
projected_basis.col(1) = basis.col(2);
}
else if(axis_ == 1) {
projected_basis.col(0) = basis.col(0);
projected_basis.col(1) = basis.col(2);
}
else if(axis_ == 2) {
projected_basis.col(0) = basis.col(0);
projected_basis.col(1) = basis.col(1);
}
MatrixXf projected = points * projected_basis;
// -- Transform into pixel units.
for(int i=0; i<projected.rows(); ++i) {
projected(i, 0) *= pixels_per_meter_;
projected(i, 1) *= pixels_per_meter_;
}
// -- Find min and max of u and v. TODO: noise sensitivity?
float min_v = FLT_MAX;
float min_u = FLT_MAX;
float max_v = -FLT_MAX;
float max_u = -FLT_MAX;
for(int i=0; i<projected.rows(); ++i) {
float u = projected(i, 0);
float v = projected(i, 1);
if(u < min_u)
min_u = u;
if(u > max_u)
max_u = u;
if(v < min_v)
min_v = v;
if(v > max_v)
max_v = v;
}
// -- Shift the origin based on {u,v}_offset_pct.
// u_offset_pct_ is the percent of the way from min_u to max_u that the
// u_offset should be set to. If this makes the window fall outside min_u or max_u,
// then shift the window so that it is inside.
float u_offset = u_offset_pct_ * (max_u - min_u) + min_u;
float v_offset = v_offset_pct_ * (max_v - min_v) + min_v;
if(u_offset_pct_ > 0.5 && u_offset + cols_ / 2 > max_u)
u_offset = max_u - cols_ / 2 + 1;
if(u_offset_pct_ < 0.5 && u_offset - cols_ / 2 < min_u)
u_offset = min_u + cols_ / 2 - 1;
if(v_offset_pct_ > 0.5 && v_offset + rows_ / 2 > max_v)
v_offset = max_v - rows_ / 2 + 1;
if(v_offset_pct_ < 0.5 && v_offset - rows_ / 2 < min_v)
v_offset = min_v + rows_ / 2 - 1;
for(int i=0; i<projected.rows(); ++i) {
projected(i, 0) -= u_offset - (float)cols_ / 2.0;
projected(i, 1) -= v_offset - (float)rows_ / 2.0;
}
// -- Fill the IplImages.
assert(sizeof(float) == 4);
IplImage* acc = cvCreateImage(cvSize(cols_, rows_), IPL_DEPTH_32F, 1);
IplImage* img = cvCreateImage(cvSize(cols_, rows_), IPL_DEPTH_32F, 1);
cvSetZero(acc);
cvSetZero(img);
for(int i=0; i<projected.rows(); ++i) {
int row = floor(projected(i, 1));
int col = floor(projected(i, 0));
if(row >= rows_ || col >= cols_ || row < 0 || col < 0)
continue;
float intensity = (float)data.channels[0].values[interest_region_indices[i]] / 255.0 * (3.0 / 4.0) + 0.25;
//cout << i << ": " << interest_region_indices[i] << "/" << data.channels[0].values.size() << " " << (float)data.channels[0].values[interest_region_indices[i]] << " " << intensity << endl;
assert(interest_region_indices[i] < (int)data.channels[0].values.size() && (int)interest_region_indices[i] >= 0);
assert(intensity <= 1.0 && intensity >= 0.0);
((float*)(img->imageData + row * img->widthStep))[col] += intensity;
((float*)(acc->imageData + row * acc->widthStep))[col]++;
}
// -- Normalize by the number of points falling in each pixel.
for(int v=0; v<rows_; ++v) {
float* img_ptr = (float*)(img->imageData + v * img->widthStep);
float* acc_ptr = (float*)(acc->imageData + v * acc->widthStep);
for(int u=0; u<cols_; ++u) {
if(*acc_ptr == 0)
*img_ptr = 0;
else
*img_ptr = *img_ptr / *acc_ptr;
img_ptr++;
acc_ptr++;
}
}
// -- Clean up and return.
cvReleaseImage(&acc);
return img;
}
void CloudOrienter::_compute() {
assert(input_cloud_);
assert(input_intensities_);
assert(!output_cloud_);
//cout << input_intensities_->rows() << " " << input_cloud_->rows() << endl;
assert(input_cloud_->rows() == input_intensities_->rows());
assert(input_cloud_->rows() > 2);
// -- Subtract off the mean of the points.
MatrixXf& points = *input_cloud_;
VectorXf pt_mean = points.colwise().sum() / (float)points.rows();
for(int i=0; i<points.rows(); ++i)
points.row(i) -= pt_mean.transpose();
// -- Flatten to z == 0.
MatrixXf X = points;
X.col(2) = VectorXf::Zero(X.rows());
MatrixXf Xt = X.transpose();
// -- Find the long axis.
// Start with a random vector.
VectorXf pc = VectorXf::Zero(3);
pc(0) = 1; //Chosen by fair dice roll.
pc(1) = 1;
pc.normalize();
// Power method.
VectorXf prev = pc;
double thresh = 1e-4;
int ctr = 0;
while(true) {
prev = pc;
pc = Xt * (X * pc);
pc.normalize();
ctr++;
if((pc - prev).norm() < thresh)
break;
// -- In some degenerate cases, it is possible for the vector
// to never settle down to the first PC.
if(ctr > 100)
break;
}
assert(abs(pc(2)) < 1e-4);
// -- Find the short axis.
VectorXf shrt = VectorXf::Zero(3);
shrt(1) = -pc(0);
shrt(0) = pc(1);
assert(abs(shrt.norm() - 1) < 1e-4);
assert(abs(shrt.dot(pc)) < 1e-4);
// -- Build the basis of normalized coordinates.
MatrixXf basis = MatrixXf::Zero(3,3);
basis.col(0) = pc;
basis.col(1) = shrt;
basis(2,2) = 1.0;
assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
assert(abs(basis.col(0).norm() - 1) < 1e-4);
assert(abs(basis.col(1).norm() - 1) < 1e-4);
assert(abs(basis.col(2).norm() - 1) < 1e-4);
// -- Rotate and set the output_cloud_.
output_cloud_ = shared_ptr<MatrixXf>(new MatrixXf);
*output_cloud_ = points * basis;
assert(output_cloud_->rows() == input_cloud_->rows());
}
visualization_msgs::MarkerArray RosVSLAM::getFeatures() {
visualization_msgs::Marker pointsINV;
pointsINV.header.frame_id = "/world";
pointsINV.header.stamp = ros::Time::now();
pointsINV.ns = "points_and_lines";
pointsINV.action = visualization_msgs::Marker::ADD;
pointsINV.pose.orientation.w = 1.0;
pointsINV.id = 0;
pointsINV.type = visualization_msgs::Marker::SPHERE_LIST;
pointsINV.scale.x = 0.1;
pointsINV.scale.y = 0.1;
pointsINV.scale.z = 0.1;
pointsINV.color.r = 0.0f;
pointsINV.color.g = 0.0f;
pointsINV.color.b = 0.0f;
pointsINV.color.a = 1.0;
visualization_msgs::MarkerArray points;
for (int i = 0; i < this->patches.size(); ++i) {
int pos = this->patches[i].position_in_state;
Vector3f d;
Matrix3f Cov;
if (!patches[i].isXYZ()) {
MatrixXf Jf;
d = depth2XYZ(mu.segment<6>(pos), Jf);
geometry_msgs::Point p;
p.x = d(0)*map_scale;
p.y = d(1)*map_scale;
p.z = d(2)*map_scale;
pointsINV.points.push_back(p);
Cov = Jf*Sigma.block<6,6>(pos,pos)*Jf.transpose();
SelfAdjointEigenSolver<MatrixXf> eigenSolver(Cov);
Vector3f eigs = eigenSolver.eigenvalues();
Matrix3f vecs = eigenSolver.eigenvectors();
Quaternionf q(vecs);
visualization_msgs::Marker pointsINV;
pointsINV.header.frame_id = "/world";
pointsINV.header.stamp = ros::Time::now();
char name[20];
sprintf(name, "F%d",i);
std::stringstream ss;
ss << i;
pointsINV.ns = name;
pointsINV.action = visualization_msgs::Marker::ADD;
// pointsXYZ.pose.orientation.w = 1.0;
pointsINV.id = i;
pointsINV.type = visualization_msgs::Marker::SPHERE;
pointsINV.scale.x = eigs(0)*map_scale;
pointsINV.scale.y = eigs(1)*map_scale;
pointsINV.scale.z = eigs(2)*map_scale;
pointsINV.pose.orientation.w = q.w();
pointsINV.pose.orientation.x = q.x();
pointsINV.pose.orientation.y = q.y();
pointsINV.pose.orientation.z = q.z();
pointsINV.pose.position.x = d(0)*map_scale;
pointsINV.pose.position.y = d(1)*map_scale;
pointsINV.pose.position.z = d(2)*map_scale;
pointsINV.color.g = 1.0f;
pointsINV.color.a = 0.5;
pointsINV.lifetime.sec = 1;
// points.markers.push_back(pointsINV);
} else {
d = mu.segment<3>(pos);
Cov = Sigma.block<3,3>(pos,pos);
Cov.eigenvalues();
SelfAdjointEigenSolver<MatrixXf> eigenSolver(Cov);
Vector3f eigs = eigenSolver.eigenvalues();
Matrix3f vecs = eigenSolver.eigenvectors();
Quaternion<float> q(vecs);
visualization_msgs::Marker pointsXYZ;
pointsXYZ.header.frame_id = "/world";
pointsXYZ.header.stamp = ros::Time::now();
char name[20];
sprintf(name, "F%d",i);
std::stringstream ss;
ss << i;
pointsXYZ.ns = name;
pointsXYZ.action = visualization_msgs::Marker::ADD;
pointsXYZ.id = i;
pointsXYZ.type = visualization_msgs::Marker::SPHERE;
pointsXYZ.scale.x = eigs(0)*map_scale;
pointsXYZ.scale.y = eigs(1)*map_scale;
pointsXYZ.scale.z = eigs(2)*map_scale;
pointsXYZ.pose.orientation.w = q.w();
pointsXYZ.pose.orientation.x = q.x();
pointsXYZ.pose.orientation.y = q.y();
pointsXYZ.pose.orientation.z = q.z();
pointsXYZ.pose.position.x = d(0)*map_scale;
pointsXYZ.pose.position.y = d(1)*map_scale;
pointsXYZ.pose.position.z = d(2)*map_scale;
pointsXYZ.color.r = 1.0f;
pointsXYZ.color.a = 0.5;
pointsXYZ.lifetime.sec = 1;
//points.markers.push_back(pointsXYZ);
visualization_msgs::Marker textpointsXYZ;
textpointsXYZ.header.frame_id = "/world";
textpointsXYZ.header.stamp = ros::Time::now();
sprintf(name, "name%d",i);
textpointsXYZ.ns = name;
textpointsXYZ.text = ss.str();
textpointsXYZ.action = visualization_msgs::Marker::ADD;
// pointsXYZ.pose.orientation.w = 1.0;
textpointsXYZ.id = 0;
textpointsXYZ.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
textpointsXYZ.scale.x = 1;
textpointsXYZ.scale.y = 1;
textpointsXYZ.scale.z = 1;
textpointsXYZ.pose.position.x = d(0)*map_scale;
textpointsXYZ.pose.position.y = d(1)*map_scale;
textpointsXYZ.pose.position.z = d(2)*map_scale;
textpointsXYZ.color.r = 0.0f;
textpointsXYZ.color.g = 0.0f;
textpointsXYZ.color.b = 0.0f;
textpointsXYZ.color.a = 1;
textpointsXYZ.lifetime.sec = 1;
// points.markers.push_back(textpointsXYZ);
geometry_msgs::Point p;
p.x = d(0)*map_scale;
p.y = d(1)*map_scale;
p.z = d(2)*map_scale;
pointsINV.points.push_back(p);
}
}
points.markers.push_back(pointsINV);
static int count_OLD = 0;
char name[100];
visualization_msgs::Marker pointsOLD;
pointsOLD.header.frame_id = "/world";
pointsOLD.header.stamp = ros::Time::now();
pointsOLD.action = visualization_msgs::Marker::ADD;
pointsOLD.pose.orientation.w = 1.0;
pointsOLD.id = 0;
pointsOLD.type = visualization_msgs::Marker::SPHERE_LIST;
pointsOLD.scale.x = 0.1;
pointsOLD.scale.y = 0.1;
pointsOLD.scale.z = 0.1;
pointsOLD.color.r = 0.0f;
pointsOLD.color.g = 0.0f;
pointsOLD.color.b = 0.0f;
pointsOLD.color.a = 1.0;
std::cout << "size = " <<deleted_patches.size() << std::endl;
if (deleted_patches.size() > 1000) {
for (int i = 0; i < deleted_patches.size(); i++) {
sprintf(name, "OLD_points%d", count_OLD++);
pointsOLD.ns = name;
geometry_msgs::Point p;
p.x = deleted_patches[i].XYZ_pos(0)*map_scale;
p.y = deleted_patches[i].XYZ_pos(1)*map_scale;
p.z = deleted_patches[i].XYZ_pos(2)*map_scale;
pointsOLD.points.push_back(p);
}
deleted_patches.clear();
} else {
for (int i = 0; i < deleted_patches.size(); i++) {
pointsOLD.ns = "OLD_points";
geometry_msgs::Point p;
p.x = deleted_patches[i].XYZ_pos(0)*map_scale;
p.y = deleted_patches[i].XYZ_pos(1)*map_scale;
p.z = deleted_patches[i].XYZ_pos(2)*map_scale;
pointsOLD.points.push_back(p);
}
}
points.markers.push_back(pointsOLD);
return points;
}
MatrixXf D3DCloudOrienter::orientCloud(const sensor_msgs::PointCloud& data,
const std::vector<int>& interest_region_indices)
{
// -- Put cluster points into matrix form.
MatrixXf points(interest_region_indices.size(), 3);
for(size_t i=0; i<interest_region_indices.size(); ++i) {
points(i, 0) = data.points[interest_region_indices[i]].x;
points(i, 1) = data.points[interest_region_indices[i]].y;
points(i, 2) = data.points[interest_region_indices[i]].z;
}
// -- Subtract off the mean of the points.
VectorXf pt_mean = points.colwise().sum() / (float)points.rows();
for(int i=0; i<points.rows(); ++i)
points.row(i) -= pt_mean.transpose();
// -- Flatten to z == 0.
MatrixXf X = points;
X.col(2) = VectorXf::Zero(X.rows());
MatrixXf Xt = X.transpose();
// -- Find the long axis.
// Start with a random vector.
VectorXf pc = VectorXf::Zero(3);
pc(0) = 1; //Chosen by fair dice roll.
pc(1) = 1;
pc.normalize();
// Power method.
VectorXf prev = pc;
double thresh = 1e-4;
int ctr = 0;
while(true) {
prev = pc;
pc = Xt * (X * pc);
pc.normalize();
ctr++;
if((pc - prev).norm() < thresh)
break;
}
assert(abs(pc(2)) < 1e-4);
// -- Find the short axis.
VectorXf shrt = VectorXf::Zero(3);
shrt(1) = -pc(0);
shrt(0) = pc(1);
assert(abs(shrt.norm() - 1) < 1e-4);
assert(abs(shrt.dot(pc)) < 1e-4);
// -- Build the basis of normalized coordinates.
MatrixXf basis = MatrixXf::Zero(3,3);
basis.col(0) = pc;
basis.col(1) = shrt;
basis(2,2) = 1.0;
assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
assert(abs(basis.col(0).norm() - 1) < 1e-4);
assert(abs(basis.col(1).norm() - 1) < 1e-4);
assert(abs(basis.col(2).norm() - 1) < 1e-4);
// -- Rotate and return.
MatrixXf oriented = points * basis;
return oriented;
}
MatrixXf make_symmetric(MatrixXf P)
{
return (P + P.transpose())*0.5;
}