C++ MatrixXd::rowwise方法代码示例

void mrpt::vision::pnp::rpnp::calcampose(Eigen::MatrixXd& XXc, Eigen::MatrixXd& XXw, Eigen::Matrix3d& R2, Eigen::Vector3d& t2)
{
	Eigen::MatrixXd X = XXc;
	Eigen::MatrixXd Y = XXw;
	Eigen::MatrixXd K = Eigen::MatrixXd::Identity(n, n) - Eigen::MatrixXd::Ones(n, n) * 1 / n;
	Eigen::VectorXd ux, uy;
	uy = X.rowwise().mean();
	ux = Y.rowwise().mean();

	// Need to verify sigmax2
	double sigmax2 = (((X*K).array() * (X*K).array()).colwise().sum()).mean();

	Eigen::MatrixXd SXY = Y*K*(X.transpose()) / n;

	Eigen::JacobiSVD<Eigen::MatrixXd> svd(SXY, Eigen::ComputeThinU | Eigen::ComputeThinV);

	Eigen::Matrix3d S = Eigen::MatrixXd::Identity(3, 3);
	if (SXY.determinant() <0)
		S(2, 2) = -1;

	R2 = svd.matrixV()*S*svd.matrixU().transpose();

	double c2 = (svd.singularValues().asDiagonal()*S).trace() / sigmax2;
	t2 = uy - c2*R2*ux;
	 
	Eigen::Vector3d x, y, z;
	x = R2.col(0);
	y = R2.col(1);
	z = R2.col(2);

	if ((x.cross(y) - z).norm()>0.02)
		R2.col(2) = -R2.col(2);
}

        // Original code from the ROS vslam package pe3d.cpp
        // uses the SVD procedure for aligning point clouds
        //   SEE: Arun, Huang, Blostein: Least-Squares Fitting of Two 3D Point Sets
        Eigen::Matrix4d threePointSvd(Eigen::MatrixXd const & p0, Eigen::MatrixXd const & p1)
        {
            using namespace Eigen;

            SM_ASSERT_EQ_DBG(std::runtime_error, p0.rows(), 3, "p0 must be a 3xK matrix");
            SM_ASSERT_EQ_DBG(std::runtime_error, p1.rows(), 3, "p1 must be a 3xK matrix");
            SM_ASSERT_EQ_DBG(std::runtime_error, p0.cols(), p1.cols(), "p0 and p1 must have the same number of columns");

            Vector3d c0 = p0.rowwise().mean();
            Vector3d c1 = p1.rowwise().mean();

            Matrix3d H(Matrix3d::Zero());
            // subtract out
            // p0a -= c0;
            // p0b -= c0;
            // p0c -= c0;
            // p1a -= c1;
            // p1b -= c1;
            // p1c -= c1;

            // Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
            // 	p1c*p0c.transpose();
            for(int i = 0; i < p0.cols(); ++i)
            {
                H += (p0.col(i) - c0) * (p1.col(i) - c1).transpose();
            }
      

            // do the SVD thang
            JacobiSVD<Matrix3d> svd(H,ComputeFullU | ComputeFullV);
            Matrix3d V = svd.matrixV();
            Matrix3d R = V * svd.matrixU().transpose();          
            double det = R.determinant();
        
            if (det < 0.0)
            {
                V.col(2) = V.col(2) * -1.0;
                R = V * svd.matrixU().transpose();
            }
            Vector3d tr = c0-R.transpose()*c1;    // translation 
      
            // transformation matrix, 3x4
            Matrix4d tfm(Matrix4d::Identity());
            //        tfm.block<3,3>(0,0) = R.transpose();
            //        tfm.col(3) = -R.transpose()*tr;
            tfm.topLeftCorner<3,3>() = R.transpose();
            tfm.topRightCorner<3,1>() = tr;
      
            return tfm;
      
        }

bool CGEOM::generate_scene_trans
( const SceneGeneratorOptions& sc_opts,
  Eigen::MatrixXd& mP3D,
  Eigen::MatrixXd& mMeasT,
  Eigen::MatrixXd& mMeasN,
  Eigen::Matrix3d& mR,
  Eigen::Vector3d& mt
) {
    if( !generate_scene( sc_opts,
                         mP3D, mMeasT, mMeasN
                       ) ) {
        return false;
    }

    // Create random transform
    mt = mP3D.rowwise().mean();
    const double drotx = rand_range_d( -45., 45. )*3.14159/180.;
    const double droty = rand_range_d( -45., 45. )*3.14159/180.;
    const double drotz = rand_range_d( -45., 45. )*3.14159/180.;
#if 1
    mR =
        ( CEIGEN::skew_rot<Eigen::Matrix3d,double>( drotx, 0., 0. ) +
          CEIGEN::skew_rot<Eigen::Matrix3d,double>( 0., droty , 0.) +
          CEIGEN::skew_rot<Eigen::Matrix3d,double>( 0., 0., drotz ) ).exp();
#else
    mR = Eigen::Matrix3d::Identity();
#endif

    mP3D.colwise() -= mt;
    mP3D = mR.transpose() * mP3D;

    return true;
}

Eigen::Vector3d PlaneRegistration::estimate_trj_unit_vector( Eigen::Vector3d& start_point,
							     Eigen::Vector3d& curr_end_point,
							     Eigen::MatrixXd& unit_vector_history ){							     					    

  Eigen::Vector3d curr_vector = ( curr_end_point - start_point );
  Eigen::Vector3d curr_unit_vector = curr_vector / curr_vector.norm();
  
  int num_rows = unit_vector_history.rows();
  int num_cols = unit_vector_history.cols();

  // add the current vector into history
  unit_vector_history.conservativeResize( num_rows, num_cols + 1 );
  unit_vector_history.col( num_cols ) = curr_unit_vector;


  // add current point into history
  curr_point_history.conservativeResize( num_rows, num_cols + 1 );
  curr_point_history.col( num_cols ) = curr_end_point;


  if ( unit_vector_history.cols() > 25 ){
    remove_matrix_column( unit_vector_history, 0 );
    remove_matrix_column( curr_point_history, 0 );
  }

  Eigen::Vector3d avg_vector = unit_vector_history.rowwise().mean();

  return avg_vector / avg_vector.norm();;

}

 Eigen::MatrixXd parse_scheme (const Header& header)
 {
   Eigen::MatrixXd PE;
   const auto it = header.keyval().find ("pe_scheme");
   if (it != header.keyval().end()) {
     try {
       PE = parse_matrix (it->second);
     } catch (Exception& e) {
       throw Exception (e, "malformed PE scheme in image \"" + header.name() + "\"");
     }
     if (ssize_t(PE.rows()) != ((header.ndim() > 3) ? header.size(3) : 1))
       throw Exception ("malformed PE scheme in image \"" + header.name() + "\" - number of rows does not equal number of volumes");
   } else {
     // Header entries are cast to lowercase at some point
     const auto it_dir  = header.keyval().find ("PhaseEncodingDirection");
     const auto it_time = header.keyval().find ("TotalReadoutTime");
     if (it_dir != header.keyval().end() && it_time != header.keyval().end()) {
       Eigen::Matrix<default_type, 4, 1> row;
       row.head<3>() = Axes::id2dir (it_dir->second);
       row[3] = to<default_type>(it_time->second);
       PE.resize ((header.ndim() > 3) ? header.size(3) : 1, 4);
       PE.rowwise() = row.transpose();
     }
   }
   return PE;
 }

void StompOptimizationTask::computeCosts(const Eigen::MatrixXd& features, Eigen::VectorXd& costs, Eigen::MatrixXd& weighted_feature_values) const
{
  weighted_feature_values = Eigen::MatrixXd::Zero(num_time_steps_, num_split_features_);
  for (int t=0; t<num_time_steps_; ++t)
  {
    weighted_feature_values.row(t) = (((features.row(t+stomp::TRAJECTORY_PADDING) - feature_means_.transpose()).array() /
        feature_variances_.array().transpose()) * feature_weights_.array().transpose()).matrix();
  }
  costs = weighted_feature_values.rowwise().sum();
}

void drawField(igl::viewer::Viewer &viewer,
               const Eigen::MatrixXd &field,
               const Eigen::RowVector3d &color)
{
  for (int n=0; n<2; ++n)
  {
    Eigen::MatrixXd VF = field.block(0,n*3,F.rows(),3);
    Eigen::VectorXd c = VF.rowwise().norm();
    viewer.data.add_edges(B - global_scale*VF, B + global_scale*VF , color);
  }
}

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;
  igl::readOBJ(TUTORIAL_SHARED_PATH "/bump-domain.obj",V,F);
  U=V;
  // Find boundary vertices outside annulus
  typedef Matrix<bool,Dynamic,1> VectorXb;
  VectorXb is_outer = (V.rowwise().norm().array()-1.0)>-1e-15;
  VectorXb is_inner = (V.rowwise().norm().array()-0.15)<1e-15;
  VectorXb in_b = is_outer.array() || is_inner.array();
  igl::colon<int>(0,V.rows()-1,b);
  b.conservativeResize(stable_partition( b.data(), b.data()+b.size(),
   [&in_b](int i)->bool{return in_b(i);})-b.data());
  bc.resize(b.size(),1);
  for(int bi = 0;bi<b.size();bi++)
  {
    bc(bi) = (is_outer(b(bi))?0.0:1.0);
  }


  // Pseudo-color based on selection
  MatrixXd C(F.rows(),3);
  RowVector3d purple(80.0/255.0,64.0/255.0,255.0/255.0);
  RowVector3d gold(255.0/255.0,228.0/255.0,58.0/255.0);
  for(int f = 0;f<F.rows();f++)
  {
    if( in_b(F(f,0)) && in_b(F(f,1)) && in_b(F(f,2)))
    {
      C.row(f) = purple;
    }else
    {
      C.row(f) = gold;
    }
  }

  // Plot the mesh with pseudocolors
  igl::viewer::Viewer viewer;
  viewer.data.set_mesh(U, F);
  viewer.core.show_lines = false;
  viewer.data.set_colors(C);
  viewer.core.trackball_angle = Eigen::Quaternionf(0.81,-0.58,-0.03,-0.03);
  viewer.core.trackball_angle.normalize();
  viewer.callback_pre_draw = &pre_draw;
  viewer.callback_key_down = &key_down;
  viewer.core.is_animating = true;
  viewer.core.animation_max_fps = 30.;
  cout<<
    "Press [space] to toggle animation."<<endl<<
    "Press '.' to increase k."<<endl<<
    "Press ',' to decrease k."<<endl;
  viewer.launch();
}

void RigidObject::computeBoundingBox() {
  _bounding_box.resize(8 * 3);
  Eigen::Map<const Eigen::MatrixXf> vertices_f(getPositions().data(), 3,
                                               getNPositions());
  Eigen::MatrixXd vertices;
  vertices = vertices_f.cast<double>();

  // subtract vertices mean
  Eigen::Vector3d mean_vertices = vertices.rowwise().mean();
  vertices = vertices - mean_vertices.replicate(1, getNPositions());

  // compute eigenvector covariance matrix
  Eigen::EigenSolver<Eigen::MatrixXd> eigen_solver(vertices *
                                                   vertices.transpose());
  Eigen::MatrixXd real_eigenvectors = eigen_solver.eigenvectors().real();

  // rotate centered vertices with inverse eigenvector matrix
  vertices = real_eigenvectors.transpose() * vertices;

  // compute simple bounding box
  auto mn = vertices.rowwise().minCoeff();
  auto mx = vertices.rowwise().maxCoeff();
  Eigen::Matrix<double, 3, 8> bounding_box;
  bounding_box << mn(0), mn(0), mn(0), mn(0), mx(0), mx(0), mx(0), mx(0), mn(1),
      mn(1), mx(1), mx(1), mn(1), mn(1), mx(1), mx(1), mn(2), mx(2), mn(2),
      mx(2), mn(2), mx(2), mn(2), mx(2);

  // rotate and translate bounding box back to original position
  Eigen::Matrix3d rot_back = real_eigenvectors;
  Eigen::Translation<double, 3> tra_back(mean_vertices);
  Eigen::Transform<double, 3, Eigen::Affine> t = tra_back * rot_back;
  bounding_box = t * bounding_box;

  // convert to float
  Eigen::Map<Eigen::MatrixXf> bounding_box_f(_bounding_box.data(), 3, 8);

  bounding_box_f = bounding_box.cast<float>();
}

Eigen::MatrixXd getMaxDiffForeachEdge(const Eigen::MatrixXd& m)
{
  if(m.cols() == 1){
    return m.cwiseAbs();
  }
  else
    {

      Eigen::MatrixXd maxValueForEachRow = m.rowwise().maxCoeff();
      Eigen::MatrixXd minValueForEachRow = m.rowwise().minCoeff();
      Eigen::MatrixXd maxDiffForEachRow = maxValueForEachRow - minValueForEachRow;
      Eigen::MatrixXd maxAbsDiffForEachRow = maxDiffForEachRow.cwiseAbs();
      return maxAbsDiffForEachRow;
  }

}

TEST(slice_into,density_reverse)
{
  {
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(10,9);
    Eigen::VectorXi I = Eigen::VectorXi::LinSpaced(A.rows(),A.rows()-1,0);
    Eigen::VectorXi J = Eigen::VectorXi::LinSpaced(A.cols(),0,A.cols()-1);
    Eigen::MatrixXd B(I.maxCoeff()+1,J.maxCoeff()+1);
    igl::slice_into(A,I,J,B);
    // reverse rows (i.e., reverse each column vector)
    Eigen::MatrixXd C = A.colwise().reverse().eval();
    test_common::assert_eq(B,C);
  }
  {
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(10,9);
    Eigen::VectorXi I = Eigen::VectorXi::LinSpaced(A.rows(),0,A.rows()-1);
    Eigen::VectorXi J = Eigen::VectorXi::LinSpaced(A.cols(),A.cols()-1,0);
    Eigen::MatrixXd B(I.maxCoeff()+1,J.maxCoeff()+1);
    igl::slice_into(A,I,J,B);
    // reverse cols (i.e., reverse each row vector)
    Eigen::MatrixXd C = A.rowwise().reverse().eval();
    test_common::assert_eq(B,C);
  }
}

	void object::test<6>()
	{
		for (int i = 0;i<10;i++)
		{
			int dim = rand()%1000+2;
			int samplenum1 = rand()%1000+100;
			int samplenum2 = rand()%1000+100;

			Eigen::MatrixXd ha = Eigen::MatrixXd::Random(dim,samplenum1);
			Eigen::MatrixXd hb = Eigen::MatrixXd::Random(dim,samplenum2);



			Eigen::VectorXd haa = (ha.array()*ha.array()).colwise().sum();
			Eigen::VectorXd hbb = (hb.array()*hb.array()).colwise().sum();

			Eigen::MatrixXd hdist = -2*ha.transpose()*hb;

			hdist.colwise() += haa;

			hdist.rowwise() += hbb.transpose();

			
			Matrix<double> ga(ha),gb(hb);

			Matrix<double> gdist = -2*ga.transpose()*gb;

			Vector<double> gaa = (ga.array()*ga.array()).colwise().sum();
			Vector<double> gbb = (gb.array()*gb.array()).colwise().sum();

			gdist.colwise() += gaa;
			gdist.rowwise() += gbb;

			ensure(check_diff(hdist,gdist));
		}
	}

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;

  // Load a mesh in OFF format
  igl::readOFF("../shared/cow.off", V, F);

  // Compute Laplace-Beltrami operator: #V by #V
  igl::cotmatrix(V,F,L);

  // Alternative construction of same Laplacian
  SparseMatrix<double> G,K;
  // Gradient/Divergence
  igl::grad(V,F,G);
  // Diagonal per-triangle "mass matrix"
  VectorXd dblA;
  igl::doublearea(V,F,dblA);
  // Place areas along diagonal #dim times
  const auto & T = 1.*(dblA.replicate(3,1)*0.5).asDiagonal();
  // Laplacian K built as discrete divergence of gradient or equivalently
  // discrete Dirichelet energy Hessian
  K = -G.transpose() * T * G;
  cout<<"|K-L|: "<<(K-L).norm()<<endl;

  const auto &key_down = [](igl::Viewer &viewer,unsigned char key,int mod)->bool
  {
    switch(key)
    {
      case 'r':
      case 'R':
        U = V;
        break;
      case ' ':
      {
        // Recompute just mass matrix on each step
        SparseMatrix<double> M;
        igl::massmatrix(U,F,igl::MASSMATRIX_TYPE_BARYCENTRIC,M);
        // Solve (M-delta*L) U = M*U
        const auto & S = (M - 0.001*L);
        Eigen::SimplicialLLT<Eigen::SparseMatrix<double > > solver(S);
        assert(solver.info() == Eigen::Success);
        U = solver.solve(M*U).eval();
        // Compute centroid and subtract (also important for numerics)
        VectorXd dblA;
        igl::doublearea(U,F,dblA);
        double area = 0.5*dblA.sum();
        MatrixXd BC;
        igl::barycenter(U,F,BC);
        RowVector3d centroid(0,0,0);
        for(int i = 0;i<BC.rows();i++)
        {
          centroid += 0.5*dblA(i)/area*BC.row(i);
        }
        U.rowwise() -= centroid;
        // Normalize to unit surface area (important for numerics)
        U.array() /= sqrt(area);
        break;
      }
      default:
        return false;
    }
    // Send new positions, update normals, recenter
    viewer.data.set_vertices(U);
    viewer.data.compute_normals();
    viewer.core.align_camera_center(U,F);
    return true;
  };


  // Use original normals as pseudo-colors
  MatrixXd N;
  igl::per_vertex_normals(V,F,N);
  MatrixXd C = N.rowwise().normalized().array()*0.5+0.5;

  // Initialize smoothing with base mesh
  U = V;
  viewer.data.set_mesh(U, F);
  viewer.data.set_colors(C);
  viewer.callback_key_down = key_down;

  cout<<"Press [space] to smooth."<<endl;;
  cout<<"Press [r] to reset."<<endl;;
  return viewer.launch();
}

//.........这里部分代码省略.........
    for (int i = 0; i < numParticles; i ++) {
        // Root
        for (int j = 0; j < cdim; j++) {
            fullJointPrev[i][j] = b_Xprior[0][j] + b_Xprior[1][j] * (dist(rd));
            tmpConfig[j] = fullJointPrev[i][j];
        }

        // Front Plane
        cspace relativeConfig, baseConfig, transformedConfig, edgeConfig;
        cspace frontPlaneConfig, sidePlaneConfig, otherSidePlaneConfig;
        relativeConfig[0] = 1.22;
        relativeConfig[1] = -0.025;
        relativeConfig[2] = 0;
        relativeConfig[3] = 0;
        relativeConfig[4] = 0;
        relativeConfig[5] = Pi;
        baseConfig = tmpConfig;
        transFrameConfig(baseConfig, relativeConfig, frontPlaneConfig);
        //TEMP:
        if (frontPlaneConfig[5] < 0)  frontPlaneConfig[5] += 2 * Pi;
        copyParticles(frontPlaneConfig, fullJointPrev[i], cdim);

        // Bottom Edge
        cspace prior1[2] = {{0,0,0,1.22,0,0},{0,0,0,0.0005,0.0005,0.0005}};
        for (int j = 0; j < cdim; j++) {
            relativeConfig[j] = prior1[0][j] + prior1[1][j] * (dist(rd));
        }
        baseConfig = tmpConfig;
        transPointConfig(baseConfig, relativeConfig, edgeConfig);
        copyParticles(edgeConfig, fullJointPrev[i], 2 * cdim);

        // Side Edge
        cspace prior2[2] = {{0,-0.025,0,0,-0.025,0.23},{0,0,0,0.0005,0.0005,0.0005}};
        for (int j = 0; j < cdim; j++) {
            relativeConfig[j] = prior2[0][j] + prior2[1][j] * (dist(rd));
        }
        baseConfig = tmpConfig;
        transPointConfig(baseConfig, relativeConfig, transformedConfig);
        copyParticles(transformedConfig, fullJointPrev[i], 3 * cdim);

        // Top edge
        double edgeTol = 0.001;
        double edgeOffSet = 0.23;
        Eigen::Vector3d pa, pb;
        pa << edgeConfig[0], edgeConfig[1], edgeConfig[2];
        pb << edgeConfig[3], edgeConfig[4], edgeConfig[5];
        Eigen::Vector3d pa_prime, pb_prime;
        inverseTransform(pa, frontPlaneConfig, pa_prime);
        inverseTransform(pb, frontPlaneConfig, pb_prime);
        double td = dist(rd) * edgeTol;
        // pa_prime(1) = 0;
        // pb_prime(1) = 0;
        Eigen::Vector3d normVec;
        normVec << (pb_prime(2) - pa_prime(2)), 0, (pa_prime(0) - pb_prime(0));
        normVec.normalize();
        normVec *= (edgeOffSet + td);
        pa_prime(0) += normVec(0);
        pb_prime(0) += normVec(0);
        pa_prime(2) += normVec(2);
        pb_prime(2) += normVec(2);
        Transform(pa_prime, frontPlaneConfig, pa);
        Transform(pb_prime, frontPlaneConfig, pb);
        fullJointPrev[i][24] = pa(0);
        fullJointPrev[i][25] = pa(1);
        fullJointPrev[i][26] = pa(2);
        fullJointPrev[i][27] = pb(0);
        fullJointPrev[i][28] = pb(1);
        fullJointPrev[i][29] = pb(2);

        // Side Plane
        relativeConfig[0] = 0;
        relativeConfig[1] = 0;
        relativeConfig[2] = 0;
        relativeConfig[3] = 0;
        relativeConfig[4] = 0;
        relativeConfig[5] = -Pi / 2.0;
        baseConfig = tmpConfig;
        transFrameConfig(baseConfig, relativeConfig, sidePlaneConfig);
        copyParticles(sidePlaneConfig, fullJointPrev[i], 5 * cdim);

        // Other Side Plane
        relativeConfig[0] = 1.24 + dist(rd) * 0.003;
        // relativeConfig[0] = 1.2192;
        relativeConfig[1] = 0;
        relativeConfig[2] = 0;
        relativeConfig[3] = 0;
        relativeConfig[4] = 0;
        relativeConfig[5] = Pi / 2.0;
        baseConfig = tmpConfig;
        transFrameConfig(baseConfig, relativeConfig, otherSidePlaneConfig);
        copyParticles(otherSidePlaneConfig, fullJointPrev[i], 6 * cdim);
        // Hole


    }
    fullJoint = fullJointPrev;
    Eigen::MatrixXd mat = Eigen::Map<Eigen::MatrixXd>((double *)fullJoint.data(), fulldim, numParticles);
    Eigen::MatrixXd mat_centered = mat.colwise() - mat.rowwise().mean();
    cov_mat = (mat_centered * mat_centered.adjoint()) / double(max2(mat.cols() - 1, 1));
}

//.........这里部分代码省略.........
                //if (d > 0.01)
                //  cout << cur_inv_M[0][0] << "  " << cur_inv_M[0][1] << "  " << cur_inv_M[0][2] << "   " << d << "   " << D << //"   " << gradient << "   " << gradient.dot(touch_dir) <<
                //       "   " << dist_adjacent[0] << "   " << dist_adjacent[1] << "   " << dist_adjacent[2] << "   " << particles[i][2] << endl;
                i += 1;
            }
        }
        cout << "End updating Plane!" << endl;
    } else { // Edge
        // for (int i = 0; i < numParticles; i ++) {
        //   // for (int j = 0; j < cdim; j ++) {
        //   //   cout << particles[i][j] << " ,";
        //   // }
        //   cout << particles[i][0] << " ,";

        // }
        // cout << endl;
        // for (int i = 0; i < numParticles; i ++) {
        //   // for (int j = 0; j < 18; j ++) {
        //   //   cout << fullParticles[i][j] << " ,";
        //   // }
        //   cout << fullParticles[i][0] << " ,";
        // }
        //       cout << endl;

        while (i < numParticles && i < maxNumParticles) {
            idx = int(floor(distribution(rd)));

            for (int j = 0; j < fulldim; j++) {
                samples(j, 0) = scl(j, 0) * dist(rd);
            }
            rot_sample = rot*samples;
            for (int j = 0; j < fulldim; j++) {
                /* TODO: use quaternions instead of euler angles */
                tempFullState[j] = b_X[idx][j] + rot_sample(j, 0);
            }

            for (int j = 0; j < cdim; j++) {
                /* TODO: use quaternions instead of euler angles */
                tempState[j] = tempFullState[j + nodeidx * cdim];
            }
            Eigen::Vector3d x1, x2, x0, tmp1, tmp2;
            x1 << tempState[0], tempState[1], tempState[2];
            x2 << tempState[3], tempState[4], tempState[5];
            x0 << cur_M[0][0], cur_M[0][1], cur_M[0][2];
            tmp1 = x1 - x0;
            tmp2 = x2 - x1;
            D = (tmp1.squaredNorm() * tmp2.squaredNorm() - pow(tmp1.dot(tmp2),2)) / tmp2.squaredNorm();
            D = abs(sqrt(D)- R);
            // cout << "Cur distance: " << D << endl;
            // cout << "D: " << D << endl;

            // if (xind >= (dist_transform->num_voxels[0] - 1) || yind >= (dist_transform->num_voxels[1] - 1) || zind >= (dist_transform->num_voxels[2] - 1))
            //  continue;

            count += 1;
            // if (sqrt(count) == floor(sqrt(count))) cout << "DDDD " << D << endl;
            if (D <= signed_dist_check) {
                // if (sqrt(count) == floor(sqrt(count))) cout << "D " << D << endl;
                count2 ++;
                for (int j = 0; j < fulldim; j++) {
                    fullJoint[i][j] = tempFullState[j];
                }
                if (checkEmptyBin(&bins, tempState) == 1) {
                    num_bins++;
                    // if (i >= N_MIN) {
                    //int numBins = bins.size();
                    numParticles = min2(maxNumParticles, max2(((num_bins - 1) * 2), N_MIN));
                    // }
                }
                //double d = testResult(mesh, particles[i], cur_M, R);
                //if (d > 0.01)
                //  cout << cur_inv_M[0][0] << "  " << cur_inv_M[0][1] << "  " << cur_inv_M[0][2] << "   " << d << "   " << D << //"   " << gradient << "   " << gradient.dot(touch_dir) <<
                //       "   " << dist_adjacent[0] << "   " << dist_adjacent[1] << "   " << dist_adjacent[2] << "   " << particles[i][2] << endl;
                i += 1;
            }
        }
        cout << "End updating Edge!" << endl;
    }
    cout << "Number of total iterations: " << count << endl;
    cout << "Number of iterations after unsigned_dist_check: " << count2 << endl;
    cout << "Number of iterations before safepoint check: " << count3 << endl;
    cout << "Number of occupied bins: " << num_bins << endl;
    cout << "Number of particles: " << numParticles << endl;

    fullJointPrev = fullJoint;

    // double* tmpParticles = new double[numParticles * fulldim];
    // for(int i = 0; i < numParticles; ++i) {
    //   for (int j = 0; j < fulldim; j ++) {
    //     tmpParticles[i * fulldim + j] = fullParticlesPrev[i][j];
    //   }
    // }

    Eigen::MatrixXd mat = Eigen::Map<Eigen::MatrixXd>((double *)fullJoint.data(), fulldim, numParticles);
    Eigen::MatrixXd mat_centered = mat.colwise() - mat.rowwise().mean();
    cov_mat = (mat_centered * mat_centered.adjoint()) / double(max2(mat.cols() - 1, 1));

    cout << "End updating!" << endl;
    return iffar;
}