void FFOgkBasis(
const MatrixXd& xi,
const int& calcM,
const int& intercept,
VectorXi& warn,
const int& h,
VectorXi& dIn,
int w3
){
double (*pFo[])(Ref<VectorXd>,int)={&qn,&scaleTau2};
double (*qFo[])(Ref<VectorXd>,int)={&Fmedian,&scaleTau2};
const int p=xi.cols(),n=xi.rows(),h0=(n+1)/2;
double b1=0.0,b2=0.0;
const double tol=1e-8;
int i,j;
MatrixXd x=xi;
RowVectorXd lamba(p);
MatrixXd x2=x;
if(intercept){
for(i=0;i<p;i++) lamba(i)=qCalc(x2.col(i),1,qFo[calcM]);
x.rowwise()-=lamba;
}
for(i=0;i<p;i++) lamba(i)=pCalc(x2.col(i),0,pFo[calcM]);
for(i=0;i<p;i++) warn(i)=(lamba(i)<tol)?1:0;
i=warn.sum();
if(i>0) return;
for(i=0;i<p;i++) x.col(i).array()/=lamba(i);
VectorXd dvec1=VectorXd::Ones(p);
MatrixXd U=dvec1.asDiagonal();
VectorXd sYi(n);
VectorXd sYj(n);
VectorXd dY(n);
for(i=0;i<p;++i){
sYi=x.col(i);
for(j=0;j<i;++j){
sYj=x.col(j);
dY=sYi+sYj;
b1=pCalc(dY,0,pFo[calcM]);
b1*=b1;
dY=sYi-sYj;
b2=pCalc(dY,0,pFo[calcM]);
b2*=b2;
U(i,j)=0.25*(b1-b2);
U(j,i)=U(i,j);
}
}
JacobiSVD<MatrixXd> svd(U,ComputeThinV);
x2=x*svd.matrixV();
for(i=0;i<p;i++) lamba(i)=pCalc(x2.col(i),0,pFo[calcM]);
for(i=0;i<p;i++) warn(i)=(lamba(i)<tol)?1:0;
i=warn.sum();
if(i>0) return;
for(i=0;i<p;i++) x2.col(i).array()/=lamba(i);
dY=x2.array().abs2().rowwise().sum();
dIn.setLinSpaced(n,0,n-1);
std::nth_element(dIn.data(),dIn.data()+h,dIn.data()+dIn.size(),IdLess(dY.data()));
cov_CStep(dIn,x,h,h,w3);
return;
}
void get_submatrix(const MatrixXd& M, const VectorXi& ind, MatrixXd& Msub)
{
Msub.setZero(ind.sum(),M.cols());
int k = 0;
for(int i=0;i<M.rows();i++){
if(ind(i) == 1)
Msub.row(k++) = M.row(i);
}
}
MatrixXd get_submatrix(const MatrixXd& M, const VectorXi& ind)
{
MatrixXd Msub(ind.sum(),M.cols());
int k = 0;
for(int i=0;i<M.rows();i++){
if(ind(i) == 1)
Msub.row(k++) = M.row(i);
}
return Msub;
}
//port faster cross product
RcppExport SEXP crossprodxval(SEXP X, SEXP idxvec_)
{
using namespace Rcpp;
using namespace RcppEigen;
try {
using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::VectorXi;
using Eigen::Lower;
using Rcpp::List;
typedef float Scalar;
typedef double Double;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRXd;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
typedef Eigen::Map<Matrix> MapMat;
typedef Eigen::Map<MatrixRXd> MapRMat;
typedef Eigen::Map<const Vector> MapVec;
typedef Map<VectorXd> MapVecd;
typedef Map<VectorXi> MapVeci;
typedef Eigen::SparseVector<double> SparseVector;
typedef Eigen::SparseVector<int> SparseVectori;
const MapMat AA(as<MapMat>(X));
const MapVeci idxvec(as<MapVeci>(idxvec_));
const int n(AA.cols());
MatrixXd AtA(MatrixXd(n, n));
MatrixRXd A = AA;
int nfolds = idxvec.maxCoeff();
List xtxlist(nfolds);
for (int k = 1; k < nfolds + 1; ++k)
{
VectorXi idxbool = (idxvec.array() == k).cast<int>();
int nrow = idxbool.size();
int numelem = idxbool.sum();
VectorXi idx(numelem);
int c = 0;
for (int i = 0; i < nrow; ++i)
{
if (idxbool(i) == 1)
{
idx(c) = i;
++c;
}
}
int nvars = A.cols() - 1;
MatrixXd sub(numelem, n);
for (int r = 0; r < numelem; ++r)
{
sub.row(r) = A.row(idx(r));
}
MatrixXd AtAtmp(MatrixXd(n, n).setZero().
selfadjointView<Lower>().rankUpdate(sub.adjoint() ));
xtxlist[k-1] = AtAtmp;
}
return wrap(xtxlist);
} catch (std::exception &ex) {
forward_exception_to_r(ex);
} catch (...) {
::Rf_error("C++ exception (unknown reason)");
}
return R_NilValue; //-Wall
}
bool KMeans::onlineUpdate(MatrixXd& X, MatrixXd& C, VectorXi& idx)
{
// Initialize some cluster information prior to phase two
MatrixXd Xmid1;
MatrixXd Xmid2;
if (m_sDistance.compare("cityblock") == 0)
{
Xmid1 = MatrixXd::Zero(k,p);
Xmid2 = MatrixXd::Zero(k,p);
for(qint32 i = 0; i < k; ++i)
{
if (m[i] > 0)
{
// Separate out sorted coords for points in i'th cluster,
// and save values above and below median, component-wise
MatrixXd Xsorted(m[i],p);
qint32 c = 0;
for(qint32 j = 0; j < idx.rows(); ++j)
{
if(idx[j] == i)
{
Xsorted.row(c) = X.row(j);
++c;
}
}
for(qint32 j = 0; j < Xsorted.cols(); ++j)
std::sort(Xsorted.col(j).data(),Xsorted.col(j).data()+Xsorted.rows());
qint32 nn = floor(0.5*m[i])-1;
if ((m[i] % 2) == 0)
{
Xmid1.row(i) = Xsorted.row(nn);
Xmid2.row(i) = Xsorted.row(nn+1);
}
else if (m[i] > 1)
{
Xmid1.row(i) = Xsorted.row(nn);
Xmid2.row(i) = Xsorted.row(nn+2);
}
else
{
Xmid1.row(i) = Xsorted.row(0);
Xmid2.row(i) = Xsorted.row(0);
}
}
}
}
else if (m_sDistance.compare("hamming") == 0)
{
// Xsum = zeros(k,p);
// for i = 1:k
// if m(i) > 0
// % Sum coords for points in i'th cluster, component-wise
// Xsum(i,:) = sum(X(idx==i,:), 1);
// end
// end
}
//
// Begin phase two: single reassignments
//
VectorXi changed = VectorXi(m.rows());
qint32 count = 0;
for(qint32 i = 0; i < m.rows(); ++i)
{
if(m[i] > 0)
{
changed[count] = i;
++count;
}
}
changed.conservativeResize(count);
qint32 lastmoved = 0;
qint32 nummoved = 0;
qint32 iter1 = iter;
bool converged = false;
while (iter < m_iMaxit)
{
// Calculate distances to each cluster from each point, and the
// potential change in total sum of errors for adding or removing
// each point from each cluster. Clusters that have not changed
// membership need not be updated.
//
// Singleton clusters are a special case for the sum of dists
// calculation. Removing their only point is never best, so the
// reassignment criterion had better guarantee that a singleton
// point will stay in its own cluster. Happily, we get
// Del(i,idx(i)) == 0 automatically for them.
if (m_sDistance.compare("sqeuclidean") == 0)
{
for(qint32 j = 0; j < changed.rows(); ++j)
{
qint32 i = changed[j];
VectorXi mbrs = VectorXi::Zero(idx.rows());
for(qint32 l = 0; l < idx.rows(); ++l)
if(idx[l] == i)
mbrs[l] = 1;
VectorXi sgn = 1 - 2 * mbrs.array(); // -1 for members, 1 for nonmembers
if (m[i] == 1)
for(qint32 l = 0; l < mbrs.rows(); ++l)
if(mbrs[l])
sgn[l] = 0; // prevent divide-by-zero for singleton mbrs
Del.col(i) = ((double)m[i] / ((double)m[i] + sgn.cast<double>().array()));
Del.col(i).array() *= (X - C.row(i).replicate(n,1)).array().pow(2).rowwise().sum().array();
}
}
else if (m_sDistance.compare("cityblock") == 0)
{
for(qint32 j = 0; j < changed.rows(); ++j)
{
qint32 i = changed[j];
if (m(i) % 2 == 0) // this will never catch singleton clusters
{
MatrixXd ldist = Xmid1.row(i).replicate(n,1) - X;
MatrixXd rdist = X - Xmid2.row(i).replicate(n,1);
VectorXd mbrs = VectorXd::Zero(idx.rows());
for(qint32 l = 0; l < idx.rows(); ++l)
if(idx[l] == i)
mbrs[l] = 1;
MatrixXd sgn = ((-2*mbrs).array() + 1).replicate(1, p); // -1 for members, 1 for nonmembers
rdist = sgn.array()*rdist.array(); ldist = sgn.array()*ldist.array();
for(qint32 l = 0; l < idx.rows(); ++l)
{
double sum = 0;
for(qint32 h = 0; h < rdist.cols(); ++h)
sum += rdist(l,h) > ldist(l,h) ? rdist(l,h) < 0 ? 0 : rdist(l,h) : ldist(l,h) < 0 ? 0 : ldist(l,h);
Del(l,i) = sum;
}
}
else
Del.col(i) = ((X - C.row(i).replicate(n,1)).array().abs()).rowwise().sum();
}
}
else if (m_sDistance.compare("cosine") == 0 || m_sDistance.compare("correlation") == 0)
{
// The points are normalized, centroids are not, so normalize them
MatrixXd normC = C.array().pow(2).rowwise().sum().sqrt();
// if any(normC < eps(class(normC))) % small relative to unit-length data points
// error('Zero cluster centroid created at iteration %d during replicate %d.',iter, rep);
// end
// This can be done without a loop, but the loop saves memory allocations
MatrixXd XCi;
qint32 i;
for(qint32 j = 0; j < changed.rows(); ++j)
{
i = changed[j];
XCi = X * C.row(i).transpose();
VectorXi mbrs = VectorXi::Zero(idx.rows());
for(qint32 l = 0; l < idx.rows(); ++l)
if(idx[l] == i)
mbrs[l] = 1;
VectorXi sgn = 1 - 2 * mbrs.array(); // -1 for members, 1 for nonmembers
double A = (double)m[i] * normC(i,0);
double B = pow(((double)m[i] * normC(i,0)),2);
Del.col(i) = 1 + sgn.cast<double>().array()*
(A - (B + 2 * sgn.cast<double>().array() * m[i] * XCi.array() + 1).sqrt());
std::cout << "Del.col(i)\n" << Del.col(i) << std::endl;
// Del(:,i) = 1 + sgn .*...
// (m(i).*normC(i) - sqrt((m(i).*normC(i)).^2 + 2.*sgn.*m(i).*XCi + 1));
}
}
else if (m_sDistance.compare("hamming") == 0)
{
// for i = changed
// if mod(m(i),2) == 0 % this will never catch singleton clusters
// % coords with an unequal number of 0s and 1s have a
// % different contribution than coords with an equal
// % number
// unequal01 = find(2*Xsum(i,:) ~= m(i));
// numequal01 = p - length(unequal01);
// mbrs = (idx == i);
// Di = abs(X(:,unequal01) - C(repmat(i,n,1),unequal01));
// Del(:,i) = (sum(Di, 2) + mbrs*numequal01) / p;
// else
// Del(:,i) = sum(abs(X - C(repmat(i,n,1),:)), 2) / p;
// end
// end
}
// Determine best possible move, if any, for each point. Next we
// will pick one from those that actually did move.
previdx = idx;
prevtotsumD = totsumD;
VectorXi nidx = VectorXi::Zero(Del.rows());
VectorXd minDel = VectorXd::Zero(Del.rows());
for(qint32 i = 0; i < Del.rows(); ++i)
minDel[i] = Del.row(i).minCoeff(&nidx[i]);
VectorXi moved = VectorXi::Zero(previdx.rows());
qint32 count = 0;
for(qint32 i = 0; i < moved.rows(); ++i)
{
if(previdx[i] != nidx[i])
{
moved[count] = i;
++count;
}
}
moved.conservativeResize(count);
if (moved.sum() > 0)
{
// Resolve ties in favor of not moving
VectorXi moved_new = VectorXi::Zero(moved.rows());
count = 0;
for(qint32 i = 0; i < moved.rows(); ++i)
{
if ( Del.array()(previdx[moved(i)]*n + moved(i)) > minDel(moved(i)))
{
moved_new[count] = moved[i];
++count;
}
}
moved_new.conservativeResize(count);
moved = moved_new;
}
if (moved.rows() <= 0)
{
// Count an iteration if phase 2 did nothing at all, or if we're
// in the middle of a pass through all the points
if ((iter == iter1) || nummoved > 0)
{
++iter;
// printf("%6d\t%6d\t%8d\t%12g\n",iter,2,nummoved,totsumD);
}
converged = true;
break;
}
// Pick the next move in cyclic order
VectorXi moved_new(moved.rows());
for(qint32 i = 0; i < moved.rows(); ++i)
moved_new[i] = ((moved[i] - lastmoved) % n) + lastmoved;
moved[0] = moved_new.minCoeff() % n;//+1
moved.conservativeResize(1);
// If we've gone once through all the points, that's an iteration
if (moved[0] <= lastmoved)
{
++iter;
// printf("%6d\t%6d\t%8d\t%12g\n",iter,2,nummoved,totsumD);
if(iter >= m_iMaxit)
break;
nummoved = 0;
}
++nummoved;
lastmoved = moved[0];
qint32 oidx = idx(moved[0]);
nidx[0] = nidx(moved[0]);
nidx.conservativeResize(1);
totsumD += Del(moved[0],nidx[0]) - Del(moved[0],oidx);
// Update the cluster index vector, and the old and new cluster
// counts and centroids
idx[ moved[0] ] = nidx[0];
m( nidx[0] ) = m( nidx[0] ) + 1;
m( oidx ) = m( oidx ) - 1;
if (m_sDistance.compare("sqeuclidean") == 0)
{
C.row(nidx[0]) = C.row(nidx[0]).array() + (X.row(moved[0]) - C.row(nidx[0])).array() / m[nidx[0]];
C.row(oidx) = C.row(oidx).array() - (X.row(moved[0]) - C.row(oidx)).array() / m[oidx];
}
else if (m_sDistance.compare("cityblock") == 0)
{
VectorXi onidx(2);
onidx << oidx, nidx[0];//ToDo always right?
qint32 i;
for(qint32 h = 0; h < 2; ++h)
{
i = onidx[h];
// Separate out sorted coords for points in each cluster.
// New centroid is the coord median, save values above and
// below median. All done component-wise.
MatrixXd Xsorted(m[i],p);
qint32 c = 0;
for(qint32 j = 0; j < idx.rows(); ++j)
{
if(idx[j] == i)
{
Xsorted.row(c) = X.row(j);
++c;
}
}
for(qint32 j = 0; j < Xsorted.cols(); ++j)
std::sort(Xsorted.col(j).data(),Xsorted.col(j).data()+Xsorted.rows());
qint32 nn = floor(0.5*m[i])-1;
if ((m[i] % 2) == 0)
{
C.row(i) = 0.5 * (Xsorted.row(nn) + Xsorted.row(nn+1));
Xmid1.row(i) = Xsorted.row(nn);
Xmid2.row(i) = Xsorted.row(nn+1);
}
else
{
C.row(i) = Xsorted.row(nn+1);
if (m(i) > 1)
{
Xmid1.row(i) = Xsorted.row(nn);
Xmid2.row(i) = Xsorted.row(nn+2);
}
else
{
Xmid1.row(i) = Xsorted.row(0);
Xmid2.row(i) = Xsorted.row(0);
}
}
}
}
else if (m_sDistance.compare("cosine") == 0 || m_sDistance.compare("correlation") == 0)
{
C.row(nidx[0]).array() += (X.row(moved[0]) - C.row(nidx[0])).array() / m[nidx[0]];
C.row(oidx).array() += (X.row(moved[0]) - C.row(oidx)).array() / m[oidx];
}
else if (m_sDistance.compare("hamming") == 0)
{
// % Update summed coords for points in each cluster. New
// % centroid is the coord median. All done component-wise.
// Xsum(nidx,:) = Xsum(nidx,:) + X(moved,:);
// Xsum(oidx,:) = Xsum(oidx,:) - X(moved,:);
// C(nidx,:) = .5*sign(2*Xsum(nidx,:) - m(nidx)) + .5;
// C(oidx,:) = .5*sign(2*Xsum(oidx,:) - m(oidx)) + .5;
}
VectorXi sorted_onidx(1+nidx.rows());
sorted_onidx << oidx, nidx;
std::sort(sorted_onidx.data(), sorted_onidx.data()+sorted_onidx.rows());
changed = sorted_onidx;
} // phase two
return converged;
} // nested function
bool KMeans::batchUpdate(MatrixXd& X, MatrixXd& C, VectorXi& idx)
{
// Every point moved, every cluster will need an update
qint32 i = 0;
VectorXi moved(n);
for(i = 0; i < n; ++i)
moved[i] = i;
VectorXi changed(k);
for(i = 0; i < k; ++i)
changed[i] = i;
previdx = VectorXi::Zero(n);
prevtotsumD = std::numeric_limits<double>::max();//max double
MatrixXd D = MatrixXd::Zero(X.rows(), k);
//
// Begin phase one: batch reassignments
//
iter = 0;
bool converged = false;
while(true)
{
++iter;
// Calculate the new cluster centroids and counts, and update the
// distance from every point to those new cluster centroids
MatrixXd C_new;
VectorXi m_new;
KMeans::gcentroids(X, idx, changed, C_new, m_new);
MatrixXd D_new = distfun(X, C_new, iter);
for(qint32 i = 0; i < changed.rows(); ++i)
{
C.row(changed[i]) = C_new.row(i);
D.col(changed[i]) = D_new.col(i);
m[changed[i]] = m_new[i];
}
// Deal with clusters that have just lost all their members
VectorXi empties = VectorXi::Zero(changed.rows());
for(qint32 i = 0; i < changed.rows(); ++i)
if(m(i) == 0)
empties[i] = 1;
if (empties.sum() > 0)
{
if (m_sEmptyact.compare("error") == 0)
{
throw 0;
}
else if (m_sEmptyact.compare("drop") == 0)
{
// // Remove the empty cluster from any further processing
// D(:,empties) = NaN;
// changed = changed(m(changed) > 0);
// warning('Empty cluster created at iteration %d during replicate %d.',iter, rep,);
}
else if (m_sEmptyact.compare("singleton") == 0)
{
// warning('Empty cluster created at iteration %d during replicate %d.', iter, rep);
// for i = empties
// d = D((idx-1)*n + (1:n)'); // use newly updated distances
// % Find the point furthest away from its current cluster.
// % Take that point out of its cluster and use it to create
// % a new singleton cluster to replace the empty one.
// [dlarge, lonely] = max(d);
// from = idx(lonely); % taking from this cluster
// if m(from) < 2
// % In the very unusual event that the cluster had only
// % one member, pick any other non-singleton point.
// from = find(m>1,1,'first');
// lonely = find(idx==from,1,'first');
// end
// C(i,:) = X(lonely,:);
// m(i) = 1;
// idx(lonely) = i;
// D(:,i) = distfun(X, C(i,:), distance, iter);
// % Update clusters from which points are taken
// [C(from,:), m(from)] = gcentroids(X, idx, from, distance);
// D(:,from) = distfun(X, C(from,:), distance, iter);
// changed = unique([changed from]);
// end
}
}
// Compute the total sum of distances for the current configuration.
totsumD = 0;
for(qint32 i = 0; i < n; ++i)
totsumD += D.array()(idx[i]*n+i);//Colum Major
// Test for a cycle: if objective is not decreased, back out
// the last step and move on to the single update phase
if(prevtotsumD <= totsumD)
{
idx = previdx;
MatrixXd C_new;
VectorXi m_new;
gcentroids(X, idx, changed, C_new, m_new);
C.block(0,0,k,C.cols()) = C_new;
m.block(0,0,k,1) = m_new;
--iter;
break;
}
// printf("%6d\t%6d\t%8d\t%12g\n",iter,1,moved.rows(),totsumD);
if (iter >= m_iMaxit)
break;
// Determine closest cluster for each point and reassign points to clusters
previdx = idx;
prevtotsumD = totsumD;
VectorXi nidx(D.rows());
for(qint32 i = 0; i < D.rows(); ++i)
d[i] = D.row(i).minCoeff(&nidx[i]);
// Determine which points moved
VectorXi moved = VectorXi::Zero(nidx.rows());
qint32 count = 0;
for(qint32 i = 0; i < nidx.rows(); ++i)
{
if(nidx[i] != previdx[i])
{
moved[count] = i;
++count;
}
}
moved.conservativeResize(count);
if (moved.rows() > 0)
{
// Resolve ties in favor of not moving
VectorXi moved_new = VectorXi::Zero(moved.rows());
count = 0;
for(qint32 i = 0; i < moved.rows(); ++i)
{
if(D.array()(previdx[moved[i]] * n + moved[i]) > d[moved[i]])
{
moved_new[count] = moved[i];
++count;
}
}
moved_new.conservativeResize(count);
moved = moved_new;
}
if (moved.rows() == 0)
{
converged = true;
break;
}
for(qint32 i = 0; i < moved.rows(); ++i)
if(moved[i] >= 0)
idx[ moved[i] ] = nidx[ moved[i] ];
// Find clusters that gained or lost members
std::vector<int> tmp;
for(qint32 i = 0; i < moved.rows(); ++i)
tmp.push_back(idx[moved[i]]);
for(qint32 i = 0; i < moved.rows(); ++i)
tmp.push_back(previdx[moved[i]]);
std::sort(tmp.begin(),tmp.end());
std::vector<int>::iterator it;
it = std::unique(tmp.begin(),tmp.end());
tmp.resize( it - tmp.begin() );
changed.conservativeResize(tmp.size());
for(quint32 i = 0; i < tmp.size(); ++i)
changed[i] = tmp[i];
} // phase one
return converged;
} // nested function
size_t SlamCalibrator::processMap(const StreamSequenceBase& sseq,
const Trajectory& traj, const Cloud& map,
DiscreteDepthDistortionModel* model) const
{
// -- Select which frame indices from the sequence to use.
// Consider only those with a pose in the Trajectory,
// and apply downsampling based on increment_.
vector<size_t> indices;
indices.reserve(traj.numValid());
int num = 0;
for(size_t i = 0; i < traj.size(); ++i) {
if(traj.exists(i)) {
++num;
if(num % increment_ == 0)
indices.push_back(i);
}
}
// -- For all selected frames, accumulate training examples
// in the distortion model.
VectorXi counts = VectorXi::Zero(indices.size());
#pragma omp parallel for
for(size_t i = 0; i < indices.size(); ++i) {
size_t idx = indices[i];
BOOST_ASSERT(traj.exists(idx));
cout << "." << flush;
Frame measurement;
sseq.readFrame(idx, &measurement);
Frame mapframe;
mapframe.depth_ = DepthMatPtr(new DepthMat);
sseq.proj_.estimateMapDepth(map, traj.get(idx).inverse().cast<float>(),
measurement, mapframe.depth_.get());
counts[i] = model->accumulate(*mapframe.depth_, *measurement.depth_);
// cv::imshow("map", mapframe.depthImage());
// cv::imshow("measurement", measurement.depthImage());
// cv::waitKey();
// -- Quick and dirty option for data inspection.
if(getenv("U") && getenv("V")) {
int u_center = atoi(getenv("U"));
int v_center = atoi(getenv("V"));
int radius = 1;
for(int u = max(0, u_center - radius); u < min(640, u_center + radius + 1); ++u) {
for(int v = max(0, v_center - radius); v < min(480, v_center + radius + 1); ++v) {
if(mapframe.depth_->coeffRef(v, u) == 0)
continue;
if(measurement.depth_->coeffRef(v, u) == 0)
continue;
cerr << mapframe.depth_->coeffRef(v, u) * 0.001
<< " "
<< measurement.depth_->coeffRef(v, u) * 0.001
<< endl;
}
}
}
}
cout << endl;
return counts.sum();
}
