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;

//.........这里部分代码省略.........

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

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)
        {
//.........这里部分代码省略.........

MatrixXd MNEInverseOperator::cluster_kernel(const AnnotationSet &p_AnnotationSet, qint32 p_iClusterSize, MatrixXd& p_D) const
{
    MatrixXd p_outMT = this->m_K.transpose();

    QList<MNEClusterInfo> t_qListMNEClusterInfo;
    MNEClusterInfo t_MNEClusterInfo;
    t_qListMNEClusterInfo.append(t_MNEClusterInfo);
    t_qListMNEClusterInfo.append(t_MNEClusterInfo);

    //
    // Check consisty
    //
    if(this->isFixedOrient())
    {
        printf("Error: Fixed orientation not implemented jet!\n");
        return p_outMT;
    }

//    qDebug() << "p_outMT" << p_outMT.rows() << "x" << p_outMT.cols();

//    MatrixXd t_G_Whitened(0,0);
//    bool t_bUseWhitened = false;
//    //
//    //Whiten gain matrix before clustering -> cause diffenerent units Magnetometer, Gradiometer and EEG
//    //
//    if(!p_pNoise_cov.isEmpty() && !p_pInfo.isEmpty())
//    {
//        FiffInfo p_outFwdInfo;
//        FiffCov p_outNoiseCov;
//        MatrixXd p_outWhitener;
//        qint32 p_outNumNonZero;
//        //do whitening with noise cov
//        this->prepare_forward(p_pInfo, p_pNoise_cov, false, p_outFwdInfo, t_G_Whitened, p_outNoiseCov, p_outWhitener, p_outNumNonZero);
//        printf("\tWhitening the forward solution.\n");

//        t_G_Whitened = p_outWhitener*t_G_Whitened;
//        t_bUseWhitened = true;
//    }



    //
    // Assemble input data
    //
    qint32 count;
    qint32 offset;

    MatrixXd t_MT_new;

    for(qint32 h = 0; h < this->src.size(); ++h )
    {

        count = 0;
        offset = 0;

        // Offset for continuous indexing;
        if(h > 0)
            for(qint32 j = 0; j < h; ++j)
                offset += this->src[j].nuse;

        if(h == 0)
            printf("Cluster Left Hemisphere\n");
        else
            printf("Cluster Right Hemisphere\n");

        Colortable t_CurrentColorTable = p_AnnotationSet[h].getColortable();
        VectorXi label_ids = t_CurrentColorTable.getLabelIds();

        // Get label ids for every vertex
        VectorXi vertno_labeled = VectorXi::Zero(this->src[h].vertno.rows());

        //ToDo make this more universal -> using Label instead of annotations - obsolete when using Labels
        for(qint32 i = 0; i < vertno_labeled.rows(); ++i)
            vertno_labeled[i] = p_AnnotationSet[h].getLabelIds()[this->src[h].vertno[i]];

        //Qt Concurrent List
        QList<RegionMT> m_qListRegionMTIn;

        //
        // Generate cluster input data
        //
        for (qint32 i = 0; i < label_ids.rows(); ++i)
        {
            if (label_ids[i] != 0)
            {
                QString curr_name = t_CurrentColorTable.struct_names[i];//obj.label2AtlasName(label(i));
                printf("\tCluster %d / %li %s...", i+1, label_ids.rows(), curr_name.toUtf8().constData());

                //
                // Get source space indeces
                //
                VectorXi idcs = VectorXi::Zero(vertno_labeled.rows());
                qint32 c = 0;

                //Select ROIs //change this use label info with a hash tabel
                for(qint32 j = 0; j < vertno_labeled.rows(); ++j)
                {
                    if(vertno_labeled[j] == label_ids[i])
                    {
                        idcs[c] = j;
//.........这里部分代码省略.........