示例#1
0
int softmax<T>::predict(cv::Mat const &input)
{
    Eigen::Map<EigenMat> const Map(reinterpret_cast<*>(input.data),
                                   input.rows,
                                   input.step / sizeof(T));
    return predict(Map.block(0, 0, input.rows, input.cols));
}
示例#2
0
文件: Function.cpp 项目: vanurag/dart
//==============================================================================
void NullFunction::evalHessian(
    const Eigen::VectorXd& _x,
    Eigen::Map<Eigen::VectorXd, Eigen::RowMajor> _Hess)
{
  _Hess.resize(pow(_x.size(),2));
  _Hess.setZero();
}
示例#3
0
/// Compute the Root Mean Square Error of the residuals
static double RMSE(const SfM_Data & sfm_data)
{
  // Compute residuals for each observation
  std::vector<double> vec;
  for(Landmarks::const_iterator iterTracks = sfm_data.GetLandmarks().begin();
      iterTracks != sfm_data.GetLandmarks().end();
      ++iterTracks)
  {
    const Observations & obs = iterTracks->second.obs;
    for(Observations::const_iterator itObs = obs.begin();
      itObs != obs.end(); ++itObs)
    {
      const View * view = sfm_data.GetViews().find(itObs->first)->second.get();
      const geometry::Pose3 pose = sfm_data.GetPoseOrDie(view);
      const std::shared_ptr<cameras::IntrinsicBase> intrinsic = sfm_data.GetIntrinsics().at(view->id_intrinsic);
      const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X, itObs->second.x);
      //std::cout << residual << " ";
      vec.push_back( residual(0) );
      vec.push_back( residual(1) );
    }
  }
  const Eigen::Map<Eigen::RowVectorXd> residuals(&vec[0], vec.size());
  const double RMSE = std::sqrt(residuals.squaredNorm() / vec.size());
  return RMSE;
}
示例#4
0
std::vector<int> const& softmax<T>::
batch_predicts(cv::Mat const &input)
{
    Eigen::Map<EigenMat> const Map(reinterpret_cast<*>(input.data),
                                   input.rows,
                                   input.step / sizeof(T));
    return batch_predicts(Map.block(0, 0, input.rows, input.cols));
}
示例#5
0
 void ExpMapQuaternion::applyDiffPseudoLog0_(
     RefMat out, const ConstRefMat& in, const ConstRefVec& x, ReusableTemporaryMap& m)
 {
   mnf_assert(in.cols() == InputDim_ && "Dimensions mismatch" );
   Eigen::Map<Eigen::MatrixXd, Eigen::Aligned> a = m.getMap(in.rows(),OutputDim_);
   a.noalias() = in*diffPseudoLog0_(x);
   out = a;
 }
示例#6
0
  void evalGradient(const Eigen::VectorXd& _x,
                    Eigen::Map<Eigen::VectorXd> _grad) override
  {
    computeResultVector(_x);

    _grad.setZero();
    int smaller = std::min(mResultVector.size(), _grad.size());
    for(int i=0; i < smaller; ++i)
      _grad[i] = mResultVector[i];
  }
示例#7
0
void RigidBody3DState::updateMandMinv()
{
  assert( unsigned( m_M0.nonZeros() ) == 6 * nbodies() );
  assert( m_M0.nonZeros() == m_Minv0.nonZeros() );
  assert( unsigned( m_M.nonZeros() ) == 12 * nbodies() );
  assert( m_M.nonZeros() == m_Minv.nonZeros() );

  for( unsigned bdy_idx = 0; bdy_idx < m_nbodies; ++bdy_idx )
  {
    // Orientation of the ith body
    const Eigen::Map<const Matrix33sr> R{ m_q.segment<9>( 3 * m_nbodies + 9 * bdy_idx ).data() };
    assert( fabs( ( R * R.transpose() - Matrix33sr::Identity() ).lpNorm<Eigen::Infinity>() ) <= 1.0e-9 );
    assert( fabs( R.determinant() - 1.0 ) <= 1.0e-9 );

    // Inertia tensor of the ith body
    {
      Eigen::Map<Matrix33sc> I{ &m_M.data().value( 3 * m_nbodies + 9 * bdy_idx ) };
      const Eigen::Map<const Vector3s> I0{ &m_M0.data().value( 3 * m_nbodies + 3 * bdy_idx ) };
      I = R * I0.asDiagonal() * R.transpose();
      assert( ( I - I.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-12 );
      assert( I.determinant() > 0.0 );
    }

    // Inverse of the inertia tensor of the ith body
    {
      Eigen::Map<Matrix33sc> Iinv{ &m_Minv.data().value( 3 * m_nbodies + 9 * bdy_idx ) };
      const Eigen::Map<const Vector3s> Iinv0{ &m_Minv0.data().value( 3 * m_nbodies + 3 * bdy_idx ) };
      Iinv = R * Iinv0.asDiagonal() * R.transpose();
      assert( ( Iinv - Iinv.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-12 );
      assert( Iinv.determinant() > 0.0 );
    }
  }

  assert( MathUtilities::isIdentity( m_M * m_Minv, 1.0e-9 ) );
}
示例#8
0
//Version 4
double basicBayes::evaluteMVG(std::vector<double>& sampleVecVect, std::vector<double>& meanVecVect, std::vector<double>& covMatVect){
	//this funciton is problem specific
	//Convert
	Eigen::Map<Eigen::MatrixXd> sampleVec = convertVect(sampleVecVect);
	Eigen::Map<Eigen::MatrixXd> meanVec = convertVect(meanVecVect);
	Eigen::Map<Eigen::MatrixXd> covMat = convertCovMat(covMatVect);
	
	Eigen::MatrixXd error = (sampleVec - meanVec);
	Eigen::Matrix<double,1,1> secondHalf = (error.transpose()*covMat.inverse()*error);
	return (1.0/(pow(2.0*M_PI,meanVec.rows()/2.0)*pow(covMat.determinant(),0.5)))*exp(-0.5*secondHalf(0,0));
}
示例#9
0
void ComputePcBoundaries(const pcl::PointCloud<pcl::PointXYZ>&
    pc, Eigen::Vector3f& min, Eigen::Vector3f& max) {
  const Eigen::Map<const Eigen::MatrixXf,1,Eigen::OuterStride<> > x = pc.getMatrixXfMap(1, 4, 0); // this works for PointXYZRGBNormal
  const Eigen::Map<const Eigen::MatrixXf,1,Eigen::OuterStride<> > y = pc.getMatrixXfMap(1, 4, 1); // this works for PointXYZRGBNormal
  const Eigen::Map<const Eigen::MatrixXf,1,Eigen::OuterStride<> > z = pc.getMatrixXfMap(1, 4, 2); // this works for PointXYZRGBNormal
  min << x.minCoeff(), y.minCoeff(), z.minCoeff();
  max << x.maxCoeff(), y.maxCoeff(), z.maxCoeff();
}
示例#10
0
    std::vector<VectorEntry<Kernel>> mapParameter(Eigen::Map<Derived>& map) {
        new(&map) Eigen::Map<Derived>(&m_parameters(++m_parameterOffset));
        
        std::vector<VectorEntry<Kernel>> result(map.rows());
        for (int i = 0; i < map.rows(); ++i)
            result[i] = {m_parameterOffset + i, &m_parameters(m_parameterOffset + i)};

        //minus one as we increase the parameter offset already in this function
        m_parameterOffset+= (map.rows()-1);
                
        return result;
    };
示例#11
0
static SparseMatrixsc formWorldSpaceInverseMassMatrix( const std::vector<scalar>& M, const std::vector<Vector3s>& I0, const std::vector<VectorXs>& R )
{
  assert( M.size() == I0.size() );
  assert( M.size() == I0.size() );
  const unsigned nbodies{ static_cast<unsigned>( I0.size() ) };
  const unsigned nvdofs{ 6 * nbodies };

  SparseMatrixsc Mbody{ static_cast<SparseMatrixsc::Index>( nvdofs ), static_cast<SparseMatrixsc::Index>( nvdofs ) };
  {
    VectorXi column_nonzeros{ nvdofs };
    column_nonzeros.segment( 0, 3 * nbodies ).setOnes();
    column_nonzeros.segment( 3 * nbodies, 3 * nbodies ).setConstant( 3 );
    Mbody.reserve( column_nonzeros );
  }
  // Load the total masses
  for( unsigned bdy_idx = 0; bdy_idx < nbodies; ++bdy_idx )
  {
    for( unsigned dof_idx = 0; dof_idx < 3; ++dof_idx )
    {
      const unsigned col{ 3 * bdy_idx + dof_idx };
      const unsigned row{ col };
      assert( M[ bdy_idx ] > 0.0 );
      Mbody.insert( row, col ) = 1.0 / M[ bdy_idx ];
    }
  }

  // Load the inertia tensors
  for( unsigned bdy_idx = 0; bdy_idx < nbodies; ++bdy_idx )
  {
    // Transform from principal axes rep
    const Eigen::Map<const Matrix33sr> Rmat{ R[bdy_idx].data() };
    assert( ( Rmat * Rmat.transpose() - Matrix33sr::Identity() ).lpNorm<Eigen::Infinity>() <= 1.0e-9 );
    assert( fabs( Rmat.determinant() - 1.0 ) <= 1.0e-9 );
    const Matrix33sr Iinv = Rmat * I0[bdy_idx].array().inverse().matrix().asDiagonal() * Rmat.transpose();
    assert( ( Iinv - Iinv.transpose() ).lpNorm<Eigen::Infinity>() <= 1.0e-12 );
    assert( Iinv.determinant() > 0.0 );
    for( unsigned row_idx = 0; row_idx < 3; ++row_idx )
    {
      const unsigned col{ 3 * nbodies + 3 * bdy_idx + row_idx };
      for( unsigned col_idx = 0; col_idx < 3; ++col_idx )
      {
        const unsigned row{ 3 * nbodies + 3 * bdy_idx + col_idx };
        Mbody.insert( row, col ) = Iinv( row_idx, col_idx );
      }
    }
  }
  assert( 12 * nbodies == unsigned( Mbody.nonZeros() ) );
  Mbody.makeCompressed();
  return Mbody;
}
示例#12
0
        double BSplineMotionError<SPLINE_T>::evaluateErrorImplementation()
        {       
            // the error is a scalar: c' Q c, with c the vector valued spline coefficients stacked

            const double* cMat = &((_splineDV->spline()).coefficients()(0,0));
            Eigen::Map<const Eigen::VectorXd> c = Eigen::VectorXd::Map(cMat, _coefficientVectorLength);

            // Q*c :
            // create result container:
            Eigen::VectorXd Qc(_Q.rows());  // number of rows of Q:
            Qc.setZero();
           //  std::cout << Qc->rows() << ":" << Qc->cols() << std::endl;
            _Q.multiply(&Qc, c);
            
            return c.transpose() * (Qc);
        }
示例#13
0
 void integrate_inplace(Eigen::Map<MatrixType> integral, Callable&& f) const noexcept
 {
     for (std::size_t index{0}; index < points(); ++index)
     {
         integral.noalias() += f(femvals[index], index) * m_weights[index];
     }
 }
示例#14
0
// [[Rcpp::export]]
Eigen::MatrixXd cholupdateL_rcpp(const Eigen::Map<Eigen::MatrixXd>& L, 
                                 const Eigen::Map<Eigen::MatrixXd>& V12, 
                                 const Eigen::Map<Eigen::MatrixXd>& V22) { 
      
  int k = L.rows();
  int k2 = V22.rows();
  Eigen::MatrixXd S(k, k2);
  Eigen::MatrixXd U(k2, k2);
  Eigen::MatrixXd M(k2, k2);
  Eigen::MatrixXd Lup(k+k2, k+k2);
  
  Lup.setZero();
  S = L.triangularView<Lower>().solve(V12);
  M = V22 -  S.adjoint() * S ;
  U =  M.adjoint().llt().matrixL();
  Lup.topLeftCorner(k,k) = L;
  Lup.bottomLeftCorner(k2,k) = S.adjoint();
  Lup.bottomRightCorner(k2,k2) = U;
  return Lup;
}
示例#15
0
double BodyNode::VelocityObjFunc::eval(Eigen::Map<const Eigen::VectorXd>& _x)
{
  assert(mBodyNode->getParentJoint()->getNumGenCoords() == _x.size());

  // Update forward kinematics information with _x
  // We are just insterested in spacial velocity of mBodyNode
  mBodyNode->getParentJoint()->setGenVels(_x, true, false);

  // Compute and return the geometric distance between body node transformation
  // and target transformation
  Eigen::Vector6d diff = mBodyNode->getWorldVelocity() - mVelocity;
  return diff.dot(diff);
}
示例#16
0
Rcpp::List GetIndCEScoresCPP( const Eigen::Map<Eigen::VectorXd> & yVec, const Eigen::Map<Eigen::VectorXd> & muVec, const Eigen::Map<Eigen::VectorXd> & lamVec, const Eigen::Map<Eigen::MatrixXd> & phiMat, const Eigen::Map<Eigen::MatrixXd> & SigmaYi){ 

  // Setting up initial values

  const unsigned int lenlamVec = lamVec.size(); 
 
  Eigen::MatrixXd xiVar = Eigen::MatrixXd::Constant(lenlamVec,lenlamVec,std::numeric_limits<double>::quiet_NaN());
  Eigen::MatrixXd xiEst = Eigen::MatrixXd::Constant(lenlamVec,1,std::numeric_limits<double>::quiet_NaN());
  Eigen::MatrixXd fittedY = Eigen::MatrixXd::Constant(lenlamVec,1,std::numeric_limits<double>::quiet_NaN());
   
  Eigen::MatrixXd LamPhi = lamVec.asDiagonal() * phiMat.transpose();  
  Eigen::LDLT<Eigen::MatrixXd> ldlt_SigmaYi(SigmaYi);
  xiEst = LamPhi * ldlt_SigmaYi.solve(yVec - muVec) ;       // LamPhiSig * (yVec - muVec); 
  xiVar = -LamPhi * ldlt_SigmaYi.solve(LamPhi.transpose()); // LamPhiSig.transpose(); 
  
  xiVar.diagonal() += lamVec;
  fittedY = muVec + phiMat * xiEst;

  return Rcpp::List::create(Rcpp::Named("xiEst") = xiEst,
                            Rcpp::Named("xiVar") = xiVar,
                            Rcpp::Named("fittedY") = fittedY);
}
示例#17
0
double BodyNode::TransformObjFunc::eval(Eigen::Map<const Eigen::VectorXd>& _x)
{
  assert(mBodyNode->getParentJoint()->getNumGenCoords() == _x.size());

  // Update forward kinematics information with _x
  // We are just insterested in transformation of mBodyNode
  mBodyNode->getParentJoint()->setConfigs(_x, true, false, false);

  // Compute and return the geometric distance between body node transformation
  // and target transformation
  Eigen::Isometry3d bodyT = mBodyNode->getWorldTransform();
  Eigen::Vector6d dist = math::logMap(bodyT.inverse() * mT);
  return dist.dot(dist);
}
示例#18
0
	void DiagonalGMM::sampleWithLimits( Eigen::Map<Eigen::VectorXf> &dst, const Eigen::VectorXf &minValues, const Eigen::VectorXf &maxValues )
	{
		//printf("sample with limits \n");
		int idx=sampleComponent();
		//printf("sampleWithLimits selected component %d\n",idx);
		if (dst.rows()!=nDimensions)
			Debug::throwError("Invalid dst dimensions for sampleWithLimits()!");
		for (int d=0; d<nDimensions; d++)
		{
			
			//dst[d]=randGaussianClipped(mean[idx][d],std[idx][d],minValues[d],maxValues[d]);
			//printf(" Sample with limit for %d-- mean %f, stdv %f, min %f, max %f \n",d,mean[idx][d],std[idx][d],minValues[d],maxValues[d] );
		
			dst[d]=randGaussianClipped(mean[idx][d],std[idx][d],minValues[d],maxValues[d]);
		}
	}
/* =================================================================
   GIBBS SAMPLER ALGORITHM TO DRAW A NEW VECTOR "y" ~ TruncNormal of dimension D
       GIVEN PREVIOUS VECTOR "y" AS INPUT
   Visits each entry in y in random order, 
     and resamples that entry using efficient 1D truncated normal sampling routines.
   When we visit the *target* dimension,
     we enforce that it must be larger than the maximum of all other entries
   Otherwise, we visit other dimensions and enforce that they must be smaller than y[target]
 */
void gibbs_sample_mv_randn_trunc( VectorType& y, const Eigen::Map<VectorType>& mu, int targetDim) {
  int D = (int) mu.size();
  double maxOthers;
  int *perm = new int[D];
  randperm( D, perm );

  for (int dd=0; dd < D; dd++) {
    int d = perm[dd];
    if (d == targetDim) {
      maxOthers = get_max_value_ignore_dim( y, targetDim );
      y(d) = randn_trunc_below( mu(d), 1.0, maxOthers );
    } else {
      y(d) = randn_trunc_above( mu(d), 1.0, y(targetDim) );
    }
  }
  delete [] perm;
  perm = NULL;
}
示例#20
0
// Correlation implementation in Eigen
//' @rdname corFamily
//' @export
// [[Rcpp::export]]
Eigen::MatrixXd corEigen(Eigen::Map<Eigen::MatrixXd> & X) {
  
  // Handle degenerate cases
  if (X.rows() == 0 && X.cols() > 0) {
    return Eigen::MatrixXd::Constant(X.cols(), X.cols(), 
                                     Rcpp::NumericVector::get_na());
  }
  
  // Computing degrees of freedom
  // n - 1 is the unbiased estimate whereas n is the MLE
  const int df = X.rows() - 1; // Subtract 1 by default
  
  X.rowwise() -= X.colwise().mean();  // Centering
  
  Eigen::MatrixXd cor = X.transpose() * X / df;   // The covariance matrix
  
  // Get 1 over the standard deviations
  Eigen::VectorXd inv_sds = cor.diagonal().array().sqrt().inverse();
  
  // Scale the covariance matrix
  cor = cor.cwiseProduct(inv_sds * inv_sds.transpose());
  
  return cor;
}
示例#21
0
文件: Function.cpp 项目: vanurag/dart
//==============================================================================
void NullFunction::evalGradient(const Eigen::VectorXd& _x,
                                Eigen::Map<Eigen::VectorXd> _grad)
{
  _grad.resize(_x.size());
  _grad.setZero();
}
//' Fast Matrix Inverse
//' 
//' Computes using RcppEigen the inverse of A
//' 
//' @param A is the matrix being inverted
//' 
//' @return inverse of A
//' 
//' @examples
//' \dontrun{
//' rcppeigen_fsolve(A)
//' }
//' 
//[[Rcpp::export]]
Eigen::MatrixXd rcppeigen_fsolve(const Eigen::Map<Eigen::MatrixXd> & A){
  Eigen::MatrixXd Ainv = A.inverse();
  return Ainv;
}
//' Fast Matrix Determinant
//' 
//' Computes using RcppEigen the determinant of A
//' 
//' @param A is the matrix whose determinant calculated 
//' 
//' @return determinant of A
//' 
//' @examples
//' \dontrun{
//' rcppeigen_fdet(A)
//' }
//' 
//[[Rcpp::export]]
double rcppeigen_fdet(const Eigen::Map<Eigen::MatrixXd> & A){
 return A.determinant();
}
//' Fast Matrix T-Cross-Product
//' 
//' Computes using RcppEigen the product of A and t(B)
//' 
//' @param A is the first parameter in A times t(B)
//' @param B is the second parameter in A times t(B)
//' 
//' @return matrix tcross-product A times t(B)
//' 
//' @examples
//' \dontrun{
//' rcppeigen_ftcrossprod(A, B)
//' }
//' 
//[[Rcpp::export]]
Eigen::MatrixXd rcppeigen_ftcrossprod(const Eigen::Map<Eigen::MatrixXd> & A, 
                       const Eigen::Map<Eigen::MatrixXd> & B){
 return A * B.transpose();
}
//' Fast Matrix Cross-Product
//' 
//' Computes using RcppEigen the product of t(A) and B
//' 
//' @param A is the first parameter in t(A) times B
//' @param B is the second parameter in t(A) times B
//' 
//' @return matrix cross-product t(A) times B
//' 
//' @examples
//' \dontrun{
//' rcppeigen_fcrossprod(A, B)
//' }
//' 
//[[Rcpp::export]]
Eigen::MatrixXd rcppeigen_fcrossprod(const Eigen::Map<Eigen::MatrixXd> & A, 
                       const Eigen::Map<Eigen::MatrixXd> & B){
 return A.transpose() * B;
}
示例#26
0
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computePoint (
    size_t index, const pcl::PointCloud<PointNT> &normals, float rf[9], std::vector<float> &desc)
{
  // The RF is formed as this x_axis | y_axis | normal
  Eigen::Map<Eigen::Vector3f> x_axis (rf);
  Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
  Eigen::Map<Eigen::Vector3f> normal (rf + 6);

  // Find every point within specified search_radius_
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;
  const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
  if (neighb_cnt == 0)
  {
    for (size_t i = 0; i < desc.size (); ++i)
      desc[i] = std::numeric_limits<float>::quiet_NaN ();

    memset (rf, 0, sizeof (rf[0]) * 9);
    return (false);
  }

  float minDist = std::numeric_limits<float>::max ();
  int minIndex = -1;
  for (size_t i = 0; i < nn_indices.size (); i++)
  {
	  if (nn_dists[i] < minDist)
	  {
      minDist = nn_dists[i];
      minIndex = nn_indices[i];
	  }
  }
  
  // Get origin point
  Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
  // Get origin normal
  // Use pre-computed normals
  normal = normals[minIndex].getNormalVector3fMap ();

  // Compute and store the RF direction
  x_axis[0] = rnd ();
  x_axis[1] = rnd ();
  x_axis[2] = rnd ();
  if (!pcl::utils::equal (normal[2], 0.0f))
    x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
  else if (!pcl::utils::equal (normal[1], 0.0f))
    x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
  else if (!pcl::utils::equal (normal[0], 0.0f))
    x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];

  x_axis.normalize ();

  // Check if the computed x axis is orthogonal to the normal
  assert (pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));

  // Store the 3rd frame vector
  y_axis = normal.cross (x_axis);

  // For each point within radius
  for (size_t ne = 0; ne < neighb_cnt; ne++)
  {
    if (pcl::utils::equal (nn_dists[ne], 0.0f))
		  continue;
    // Get neighbours coordinates
    Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();

    /// ----- Compute current neighbour polar coordinates -----
    /// Get distance between the neighbour and the origin
    float r = sqrt (nn_dists[ne]); 
    
    /// Project point into the tangent plane
    Eigen::Vector3f proj;
    pcl::geometry::project (neighbour, origin, normal, proj);
    proj -= origin;

    /// Normalize to compute the dot product
    proj.normalize ();
    
    /// Compute the angle between the projection and the x axis in the interval [0,360] 
    Eigen::Vector3f cross = x_axis.cross (proj);
    float phi = pcl::rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
    phi = cross.dot (normal) < 0.f ? (360.0 - phi) : phi;
    /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
    Eigen::Vector3f no = neighbour - origin;
    no.normalize ();
    float theta = normal.dot (no);
    theta = pcl::rad2deg (acos (std::min (1.0f, std::max (-1.0f, theta))));

    // Bin (j, k, l)
    size_t j = 0;
    size_t k = 0;
    size_t l = 0;

    // Compute the Bin(j, k, l) coordinates of current neighbour
    for (size_t rad = 1; rad < radius_bins_+1; rad++) 
    {
      if (r <= radii_interval_[rad]) 
      {
        j = rad-1;
        break;
      }
    }

    for (size_t ang = 1; ang < elevation_bins_+1; ang++) 
    {
      if (theta <= theta_divisions_[ang]) 
      {
        k = ang-1;
        break;
      }
    }

    for (size_t ang = 1; ang < azimuth_bins_+1; ang++) 
    {
      if (phi <= phi_divisions_[ang]) 
      {
        l = ang-1;
        break;
      }
    }

    // Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
    std::vector<int> neighbour_indices;
    std::vector<float> neighbour_distances;
    int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances);
    // point_density is NOT always bigger than 0 (on error, searchForNeighbors returns 0), so we must check for that
    if (point_density == 0)
      continue;

    float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) + 
                                                  (k*radius_bins_) + 
                                                  j];
      
    assert (w >= 0.0);
    if (w == std::numeric_limits<float>::infinity ())
      PCL_ERROR ("Shape Context Error INF!\n");
    if (w != w)
      PCL_ERROR ("Shape Context Error IND!\n");
    /// Accumulate w into correspondant Bin(j,k,l)
    desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;

    assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
  } // end for each neighbour

  // 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user 
  memset (rf, 0, sizeof (rf[0]) * 9);
  return (true);
}
示例#27
0
Eigen::MatrixXd RmullwlskCC( const Eigen::Map<Eigen::VectorXd> & bw, const std::string kernel_type, const Eigen::Map<Eigen::MatrixXd> & tPairs, const Eigen::Map<Eigen::MatrixXd> & cxxn, const Eigen::Map<Eigen::VectorXd> & win,  const Eigen::Map<Eigen::VectorXd> & xgrid, const Eigen::Map<Eigen::VectorXd> & ygrid, const bool & bwCheck){ 

  // tPairs : xin (in MATLAB code)
  // cxxn : yin (in MATLAB code)
  // xgrid: out1 (in MATLAB code)
  // ygrid: out2 (in MATLAB code)
  // bwCheck : boolean/ cause the function to simply run the bandwidth check.

  const double invSqrt2pi=  1./(sqrt(2.*M_PI));

  // Map the kernel name so we can use switches  
  std::map<std::string,int> possibleKernels; 
  possibleKernels["epan"]    = 1;   possibleKernels["rect"]    = 2;
  possibleKernels["gauss"]   = 3;   possibleKernels["gausvar"] = 4; 
  possibleKernels["quar"]    = 5; 
   
  // The following test is here for completeness, we mightwant to move it up a 
  // level (in the wrapper) in the future. 

  // If the kernel_type key exists set KernelName appropriately
  int KernelName = 0;
  if ( possibleKernels.count( kernel_type ) != 0){ 
    KernelName = possibleKernels.find( kernel_type )->second; //Set kernel choice
  } else {
  // otherwise use "epan"as the kernel_type 
    //Rcpp::Rcout << "Kernel_type argument was not set correctly; Epanechnikov kernel used." << std::endl;
    Rcpp::warning("Kernel_type argument was not set correctly; Epanechnikov kernel used.");
    KernelName = possibleKernels.find( "epan" )->second;;
  }

  // Check that we do not have zero weights // Should do a try-catch here
  // Again this might be best moved a level-up. 
  if ( !(win.all()) ){  // 
    Rcpp::Rcout << "Cases with zero-valued windows are not yet implemented" << std::endl;
    return (tPairs);
  } 

  // Start the actual smoother here  
  unsigned int xgridN = xgrid.size();  
  unsigned int ygridN = ygrid.size();  
  
  Eigen::MatrixXd mu(xgrid.size(), ygrid.size());
  mu.setZero();    
  const  double bufSmall = 1e-6; // pow(double(10),-6);

  for (unsigned int j = 0; j != ygridN; ++j) { 
    for (unsigned int i = 0; i != xgridN; ++i) {  

      //locating local window (LOL) (bad joke)
      std::vector <unsigned int> indx; 
      //if the kernel is not Gaussian
      if ( KernelName != 3) { 
        //construct listX as vectors / size is unknown originally
        for (unsigned int y = 0; y != tPairs.cols(); y++){ 
          if (  (std::abs( tPairs(0,y) - xgrid(i) ) <= (bw(0)+ bufSmall ) && std::abs( tPairs(1,y) - ygrid(j) ) <= (bw(1)+ bufSmall)) ) {
            indx.push_back(y);
          }
        }
  
      } else{ // just get the whole deal
        for (unsigned int y = 0; y != tPairs.cols(); ++y){
          indx.push_back(y);
        }
      }  
      
     //  for (unsigned int y = 0; y != indx.size(); ++y){
     //    Rcpp::Rcout << "indx.at(y): " << indx.at(y)<< ", ";
     //  }

      unsigned int indxSize = indx.size();
      Eigen::VectorXd lw(indxSize);  
      Eigen::VectorXd ly(indxSize);
      Eigen::MatrixXd lx(2,indxSize);

      for (unsigned int u = 0; u !=indxSize; ++u){ 
        lx.col(u) = tPairs.col(indx[u]); 
        lw(u) = win(indx[u]); 
        ly(u) = cxxn(indx[u]); 
      }

      // check enough points are in the local window 
      unsigned int meter=1;  
      for (unsigned int u =0; u< indxSize; ++u) { 
        for (unsigned int t = u + 1; t < indxSize; ++t) {
          if ( (lx(0,u) !=  lx(0,t) ) || (lx(1,u) != lx(1,t) ) ) {
            meter++;
          }
        }
        if (meter >= 3) { 
          break; 
        }
      }
   
      //computing weight matrix 
      if (meter >=  3 && !bwCheck) { 
        Eigen::VectorXd temp(indxSize);
        Eigen::MatrixXd llx(2, indxSize );  
        llx.row(0) = (lx.row(0).array() - xgrid(i))/bw(0);  
        llx.row(1) = (lx.row(1).array() - ygrid(j))/bw(1); 

        //define the kernel used 

        switch (KernelName){
          case 1: // Epan
            temp=  ((1-llx.row(0).array().pow(2))*(1- llx.row(1).array().pow(2))).array() * 
                   ((9./16)*lw).transpose().array(); 
            break;  
          case 2 : // Rect
            temp=(lw.array())*.25 ; 
            break;
          case 3 : // Gauss
            temp = ((-.5*(llx.row(1).array().pow(2))).exp()) * invSqrt2pi  *   
                   ((-.5*(llx.row(0).array().pow(2))).exp()) * invSqrt2pi  *
                   (lw.transpose().array()); 
            break;
          case 4 : // GausVar
            temp = (lw.transpose().array()) * 
		   ((-0.5 * llx.row(0).array().pow(2)).array().exp() * invSqrt2pi).array() *
                   ((-0.5 * llx.row(1).array().pow(2)).array().exp() * invSqrt2pi).array() * 
                   (1.25 - (0.25 * (llx.row(0).array().pow(2))).array())  * 
                   (1.50 - (0.50 * (llx.row(1).array().pow(2))).array()); 
            break;
          case 5 :  // Quar
              temp = (lw.transpose().array()) * 
                     ((1.-llx.row(0).array().pow(2)).array().pow(2)).array() *
                     ((1.-llx.row(1).array().pow(2)).array().pow(2)).array() * (225./256.);
            break;
        } 

        // make the design matrix
        Eigen::MatrixXd X(indxSize ,3);
        X.setOnes();    
        X.col(1) = lx.row(0).array() - xgrid(i);
        X.col(2) = lx.row(1).array() - ygrid(j); 
        Eigen::LDLT<Eigen::MatrixXd> ldlt_XTWX(X.transpose() * temp.asDiagonal() *X);
        // The solver should stop if the value is NaN. See the HOLE example in gcvlwls2dV2.
        // Rcpp::Rcout << X << std::endl;
        Eigen::VectorXd beta = ldlt_XTWX.solve(X.transpose() * temp.asDiagonal() * ly);  
        mu(i,j)=beta(0); 
      } else if(meter < 3){
        // Rcpp::Rcout <<"The meter value is:" << meter << std::endl;  
        if (bwCheck) {
            Eigen::MatrixXd checker(1,1);
            checker(0,0) = 0.;
            return(checker);
        } else {
            Rcpp::stop("No enough points in local window, please increase bandwidth.");
        }
      }
    }
  }

  if (bwCheck){
     Eigen::MatrixXd checker(1,1); 
     checker(0,0) = 1.; 
     return(checker);
  } 
      
  return ( mu ); 
}
示例#28
0
 Eigen::Matrix<double, 4, 3> ExpMapQuaternion::diffRetractation_(const ConstRefVec& x)
 {
   const Eigen::Map<const Eigen::Quaterniond> xQ(x.data());
   Eigen::Matrix<double, 4, 3> J;
   J <<  0.5*xQ.w(), -0.5*xQ.z(),  0.5*xQ.y(),
         0.5*xQ.z(),  0.5*xQ.w(), -0.5*xQ.x(),
        -0.5*xQ.y(),  0.5*xQ.x(),  0.5*xQ.w(),
        -0.5*xQ.x(), -0.5*xQ.y(), -0.5*xQ.z();
   return J;
 }
示例#29
0
inline bool Generate_SfM_Report
(
  const SfM_Data & sfm_data,
  const std::string & htmlFilename
)
{
  // Compute mean,max,median residual values per View
  IndexT residualCount = 0;
  Hash_Map< IndexT, std::vector<double> > residuals_per_view;
  for ( const auto & iterTracks : sfm_data.GetLandmarks() )
  {
    const Observations & obs = iterTracks.second.obs;
    for ( const auto & itObs : obs ) 
    {
      const View * view = sfm_data.GetViews().at(itObs.first).get();
      const geometry::Pose3 pose = sfm_data.GetPoseOrDie(view);
      const cameras::IntrinsicBase * intrinsic = sfm_data.GetIntrinsics().at(view->id_intrinsic).get();
      // Use absolute values
      const Vec2 residual = intrinsic->residual(pose, iterTracks.second.X, itObs.second.x).array().abs();
      residuals_per_view[itObs.first].push_back(residual(0));
      residuals_per_view[itObs.first].push_back(residual(1));
      ++residualCount;
    }
  }
  using namespace htmlDocument;
  // extract directory from htmlFilename
  const std::string sTableBegin = "<table border=\"1\">",
    sTableEnd = "</table>",
    sRowBegin= "<tr>", sRowEnd = "</tr>",
    sColBegin = "<td>", sColEnd = "</td>",
    sNewLine = "<br>", sFullLine = "<hr>";

  htmlDocument::htmlDocumentStream htmlDocStream("SFM report.");
  htmlDocStream.pushInfo(
  htmlDocument::htmlMarkup("h1", std::string("SFM report.")));
  htmlDocStream.pushInfo(sFullLine);

  htmlDocStream.pushInfo( "Dataset info:" + sNewLine );

  std::ostringstream os;
  os << " #views: " << sfm_data.GetViews().size() << sNewLine
  << " #poses: " << sfm_data.GetPoses().size() << sNewLine
  << " #intrinsics: " << sfm_data.GetIntrinsics().size() << sNewLine
  << " #tracks: " << sfm_data.GetLandmarks().size() << sNewLine
  << " #residuals: " << residualCount << sNewLine;

  htmlDocStream.pushInfo( os.str() );
  htmlDocStream.pushInfo( sFullLine );

  htmlDocStream.pushInfo( sTableBegin);
  os.str("");
  os << sRowBegin
    << sColBegin + "IdView" + sColEnd
    << sColBegin + "Basename" + sColEnd
    << sColBegin + "#Observations" + sColEnd
    << sColBegin + "Residuals min" + sColEnd
    << sColBegin + "Residuals median" + sColEnd
    << sColBegin + "Residuals mean" + sColEnd
    << sColBegin + "Residuals max" + sColEnd
    << sRowEnd;
  htmlDocStream.pushInfo( os.str() );

  for (const auto & iterV : sfm_data.GetViews() )
  {
    const View * v = iterV.second.get();
    const IndexT id_view = v->id_view;

    os.str("");
    os << sRowBegin
      << sColBegin << id_view << sColEnd
      << sColBegin + stlplus::basename_part(v->s_Img_path) + sColEnd;

    // IdView | basename | #Observations | residuals min | residual median | residual max
    if (sfm_data.IsPoseAndIntrinsicDefined(v))
    {
      if( residuals_per_view.find(id_view) != residuals_per_view.end() )
      {
        const std::vector<double> & residuals = residuals_per_view.at(id_view);
        if (!residuals.empty())
        {
          double min, max, mean, median;
          minMaxMeanMedian(residuals.begin(), residuals.end(), min, max, mean, median);
          os << sColBegin << residuals.size()/2 << sColEnd // #observations
            << sColBegin << min << sColEnd
            << sColBegin << median << sColEnd
            << sColBegin << mean << sColEnd
            << sColBegin << max <<sColEnd;
        }
      }
    }
    os << sRowEnd;
    htmlDocStream.pushInfo( os.str() );
  }
  htmlDocStream.pushInfo( sTableEnd );
  htmlDocStream.pushInfo( sFullLine );

  // combine all residual values into one vector
  // export the SVG histogram
  {
    IndexT residualCount = 0;
    for (Hash_Map< IndexT, std::vector<double> >::const_iterator
      it = residuals_per_view.begin();
      it != residuals_per_view.end();
      ++it)
    {
      residualCount += it->second.size();
    }
    // Concat per view residual values into one vector
    std::vector<double> residuals(residualCount);
    residualCount = 0;
    for (Hash_Map< IndexT, std::vector<double> >::const_iterator
      it = residuals_per_view.begin();
      it != residuals_per_view.end();
      ++it)
    {
      std::copy(it->second.begin(),
        it->second.begin()+it->second.size(),
        residuals.begin()+residualCount);
      residualCount += it->second.size();
    }
    if (!residuals.empty())
    {
      // RMSE computation
      const Eigen::Map<Eigen::RowVectorXd> residuals_mapping(&residuals[0], residuals.size());
      const double RMSE = std::sqrt(residuals_mapping.squaredNorm() / (double)residuals.size());
      os.str("");
      os << sFullLine << "SfM Scene RMSE: " << RMSE << sFullLine;
      htmlDocStream.pushInfo(os.str());

      const double maxRange = *max_element(residuals.begin(), residuals.end());
      Histogram<double> histo(0.0, maxRange, 100);
      histo.Add(residuals.begin(), residuals.end());

      svg::svgHisto svg_Histo;
      svg_Histo.draw(histo.GetHist(), std::pair<float,float>(0.f, maxRange),
        stlplus::create_filespec(stlplus::folder_part(htmlFilename), "residuals_histogram", "svg"),
        600, 200);

      os.str("");
      os << sNewLine<< "Residuals histogram" << sNewLine;
      os << "<img src=\""
        << "residuals_histogram.svg"
        << "\" height=\"300\" width =\"800\">\n";
      htmlDocStream.pushInfo(os.str());
    }
  }

  std::ofstream htmlFileStream(htmlFilename.c_str());
  htmlFileStream << htmlDocStream.getDoc();
  const bool bOk = !htmlFileStream.bad();
  return bOk;
}
示例#30
0
//[[Rcpp::export]]
List SPMBgraphsqrt(Eigen::Map<Eigen::MatrixXd> data, NumericVector &lambda, int nlambda, int d, NumericVector &x, IntegerVector &col_cnz, IntegerVector &row_idx)
{

    Eigen::ArrayXd Xb, r, grad, w1, Y, XX, gr;
    Eigen::ArrayXXd X;
    Eigen::MatrixXd tmp_icov;
    tmp_icov.resize(d, d);
    tmp_icov.setZero();
    std::vector<Eigen::MatrixXd > tmp_icov_p;
    tmp_icov_p.clear();
    for(int i = 0; i < nlambda; i++)
      tmp_icov_p.push_back(tmp_icov);
    int n = data.rows();
    X = data;
    XX.resize(d);
    for (int j = 0; j < d; j++)
      XX[j] = (X.col(j)*X.col(j)).sum()/n;
    double prec = 1e-4;
    int max_iter = 1000;
    int num_relaxation_round = 3;
	  int cnz = 0;
    for(int m=0;m<d;m++)
    {
      Xb.resize(n);
      Xb.setZero();
      grad.resize(d);
      grad.setZero();
      gr.resize(d);
      gr.setZero();
      w1.resize(d);
      w1.setZero();
      r.resize(n);
      r.setZero();
      Y = X.col(m);

      Eigen::ArrayXd Xb_master(n);
      Eigen::ArrayXd w1_master(n);
      std::vector<int> actset_indcat(d, 0);
      std::vector<int> actset_indcat_master(d, 0);
      std::vector<int> actset_idx;
      std::vector<double> old_coef(d, 0);
      std::vector<double> grad(d, 0);
      std::vector<double> grad_master(d, 0);

      double a = 0, g = 0, L = 0, sum_r = 0, sum_r2 = 0;
      double tmp_change = 0, local_change = 0;

      r = Y - Xb;
      sum_r = r.sum();
      sum_r2 = r.matrix().dot(r.matrix());
      L = sqrt(sum_r2 / n);

      double dev_thr = fabs(L) * prec;

      //cout<<dev_thr<<endl;


      for(int i = 0; i < d; i++)
      {
        grad[i] = (r * X.col(i)).sum() / (n*L);
      }
      for(int i = 0; i < d; i++) gr[i] = abs(grad[i]);
      w1_master = w1;
      Xb_master = Xb;
      for (int i = 0; i < d; i++) grad_master[i] = gr[i];

      std::vector<double> stage_lambdas(d, 0);

      for(int i=0;i<nlambda;i++)
      {
        double ilambda = lambda[i];
        w1 = w1_master;
        Xb = Xb_master;

        for (int j = 0; j < d; j++)
        {
          gr[j] = grad_master[j];
          actset_indcat[j] = actset_indcat_master[j];
        }

        // init the active set
        double threshold;
        if (i > 0)
          threshold = 2 * lambda[i] - lambda[i - 1];
        else
          threshold = 2 * lambda[i];

        for (int j = 0; j < m; ++j)
        {
          stage_lambdas[j] = lambda[i];

          if (gr[j] > threshold) actset_indcat[j] = 1;
        }
        for (int j = m+1; j < d; ++j)
        {
          stage_lambdas[j] = lambda[i];

          if (gr[j] > threshold) actset_indcat[j] = 1;
        }
        stage_lambdas[m] = lambda[i];
        r = Y - Xb;
        sum_r = r.sum();
        sum_r2 = r.matrix().dot(r.matrix());
        L = sqrt(sum_r2 / n);
        // loop level 0: multistage convex relaxation
        int loopcnt_level_0 = 0;
        int idx;
        double old_w1, updated_coord;
        while (loopcnt_level_0 < num_relaxation_round)
        {
          loopcnt_level_0++;

          // loop level 1: active set update
          int loopcnt_level_1 = 0;
          bool terminate_loop_level_1 = true;
          while (loopcnt_level_1 < max_iter)
          {
            loopcnt_level_1++;
            terminate_loop_level_1 = true;

            for (int j = 0; j < d; j++) old_coef[j] = w1[j];

            // initialize actset_idx
            actset_idx.clear();
            for (int j = 0; j < m; j++)
              if (actset_indcat[j])
              {
                g = 0.0;
                a = 0.0;

                double tmp;

                sum_r2 = r.matrix().dot(r.matrix());
                L = sqrt(sum_r2 / n);

                Eigen::ArrayXd wXX  = (1 - r*r/sum_r2) * X.col(j) * X.col(j);
                g = (wXX * w1[j] + r * X.col(j)).sum()/(n*L);
                a = wXX.sum()/(n*L);

                tmp = w1[j];
                w1[j] = thresholdl1(g, stage_lambdas[j]) / a;

                tmp = w1[j] - tmp;
                // Xb += delta*X[idx*n]
                Xb = Xb + tmp * X.col(j);

                sum_r = 0.0;
                sum_r2 = 0.0;
                // r -= delta*X
                r = r - tmp * X.col(j);
                sum_r = r.sum();

                sum_r2 = r.matrix().dot(r.matrix());
                L = sqrt(sum_r2 / n);

                updated_coord = w1[j];

                if (fabs(updated_coord) > 0) actset_idx.push_back(j);
              }

            for (int j = m+1; j < d; j++)
              if (actset_indcat[j])
              {
                  g = 0.0;
                  a = 0.0;

                  double tmp;

                  sum_r2 = r.matrix().dot(r.matrix());
                  L = sqrt(sum_r2 / n);

                  Eigen::ArrayXd wXX  = (1 - r*r/sum_r2) * X.col(j) * X.col(j);
                  g = (wXX * w1[j] + r * X.col(j)).sum()/(n*L);
                  a = wXX.sum()/(n*L);

                  tmp = w1[j];
                  w1[j] = thresholdl1(g, stage_lambdas[j]) / a;

                  tmp = w1[j] - tmp;
                  // Xb += delta*X[idx*n]
                  Xb = Xb + tmp * X.col(j);

                  sum_r = 0.0;
                  sum_r2 = 0.0;
                  // r -= delta*X
                  r = r - tmp * X.col(j);
                  sum_r = r.sum();

                  sum_r2 = r.matrix().dot(r.matrix());
                  L = sqrt(sum_r2 / n);

                  updated_coord = w1[j];

                  if (fabs(updated_coord) > 0) actset_idx.push_back(j);
                }

              // loop level 2: proximal newton on active set
            int loopcnt_level_2 = 0;
            bool terminate_loop_level_2 = true;
            while (loopcnt_level_2 < max_iter)
            {
              loopcnt_level_2++;
              terminate_loop_level_2 = true;

              for (int k = 0; k < actset_idx.size(); k++)
              {
                  idx = actset_idx[k];

                  old_w1 = w1[idx];
                  g = 0.0;
                  a = 0.0;

                  double tmp;

                  sum_r2 = r.matrix().dot(r.matrix());
                  L = sqrt(sum_r2 / n);

                  Eigen::ArrayXd wXX  = (1 - r*r/sum_r2) * X.col(idx) * X.col(idx);
                  g = (wXX * w1[idx] + r * X.col(idx)).sum()/(n*L);
                  a = wXX.sum()/(n*L);

                  tmp = w1[idx];
                  w1[idx] = thresholdl1(g, stage_lambdas[idx]) / a;

                  tmp = w1[idx] - tmp;
                  // Xb += delta*X[idx*n]
                  Xb = Xb + tmp * X.col(idx);

                  sum_r = 0.0;
                  sum_r2 = 0.0;
                  // r -= delta*X
                  r = r - tmp * X.col(idx);
                  sum_r = r.sum();

                  sum_r2 = r.matrix().dot(r.matrix());
                  L = sqrt(sum_r2 / n);

                  updated_coord = w1[idx];
                  tmp_change = old_w1 - w1[idx];
                  double a =  (X.col(idx) * X.col(idx) * (1 - r * r/(L*L*n))).sum()/(n*L);
                  local_change = a * tmp_change * tmp_change / (2 * L * n);
                  if (local_change > dev_thr)
                    terminate_loop_level_2 = false;
                }
              if (terminate_loop_level_2)
                break;
            }

            terminate_loop_level_1 = true;
              // check stopping criterion 1: fvalue change
            for (int k = 0; k < actset_idx.size(); ++k)
            {
              idx = actset_idx[k];
              tmp_change = old_w1 - w1[idx];
              double a =  (X.col(idx) * X.col(idx) * (1 - r * r/(L*L*n))).sum()/(n*L);
              local_change = a * tmp_change * tmp_change / (2 * L * n);
              if (local_change > dev_thr)
                terminate_loop_level_1 = false;
            }

            r = Y - Xb;
            sum_r = r.sum();
            sum_r2 = r.matrix().dot(r.matrix());
            L = sqrt(sum_r2 / n);

            if (terminate_loop_level_1)
              break;


              // check stopping criterion 2: active set change
            bool new_active_idx = false;
            for (int k = 0; k < m; k++)
              if (actset_indcat[k] == 0)
              {
                grad[idx] = (r * X.col(idx)).sum() / (n*L);
                //cout<<grad[idx];
                gr[k] = fabs(grad[k]);
                if (gr[k] > stage_lambdas[k])
                {
                  actset_indcat[k] = 1;
                  new_active_idx = true;
                }
              }
            for (int k = m+1; k < d; k++)
              if (actset_indcat[k] == 0)
              {
                grad[idx] = (r * X.col(idx)).sum() / (n*L);
                //cout<<grad[idx]
                gr[k] = fabs(grad[k]);
                if (gr[k] > stage_lambdas[k])
                {
                  actset_indcat[k] = 1;
                  new_active_idx = true;
                }
              }
            if(!new_active_idx)
              break;
          }
          if (loopcnt_level_0 == 1)
          {
            for (int j = 0; j < d; j++)
            {
              w1_master[j] = w1[j];

              grad_master[j] = gr[j];
              actset_indcat_master[j] = actset_indcat[j];
            }

            for (int j = 0; j < n; j++) Xb_master[j] = Xb[j];
          }
        }
        for(int j=0;j<actset_idx.size();j++)
        {
          int w_idx = actset_idx[j];
          x[cnz] = w1[w_idx];
          row_idx[cnz] = i*d+w_idx;
          cnz++;
          //cout<<cnz<<"    ";
        }
        double tal = 0;
        Eigen::MatrixXd temp;
        temp.resize(n, 1);
        for(int j = 0; j < n; j++)
        {
          temp(j, 0) = 0;
          for(int k = 0; k < d; k++)
            temp(j, 0) += X.matrix()(j, k)*w1[k];
          temp(j, 0) = Y[j] - temp(j, 0);
        }
        //temp = Y.matrix() - X.matrix().transpose()*w1.matrix();
        for(int j = 0; j < n; j++)
          tal += temp(j, 0)*temp(j, 0);
        tal = sqrt(tal)/sqrt(n);

        tmp_icov = tmp_icov_p[i];
        tmp_icov(m, m) = pow(tal, -2);
        for(int j = 0; j < m; j++)
          tmp_icov(j, m) = -tmp_icov(m, m)*w1[j];
        for(int j = m+1; j < d; j++)
          tmp_icov(j, m) = -tmp_icov(m, m)*w1[j];
        tmp_icov_p[i] = tmp_icov;
      }
      col_cnz[m+1]=cnz;
    }
    for(int i = 0; i < nlambda; i++)
      tmp_icov_p[i] = (tmp_icov_p[i].transpose()+tmp_icov_p[i])/2;
	  return List::create(
	    _["col_cnz"] = col_cnz,
	    _["row_idx"] = row_idx,
	    _["x"] = x,
	    _["icov"] = tmp_icov_p
	  );
}