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(); }