C++ VectorXf::array方法代码示例

///////////////////////
/////  Inference  /////
///////////////////////
void expAndNormalize ( Eigen::MatrixXf & out, const Eigen::MatrixXf & in ) {
	out.resize( in.rows(), in.cols() );
	for( int i=0; i<out.cols(); i++ ){
		Eigen::VectorXf b = in.col(i);
		b.array() -= b.maxCoeff();
		b = b.array().exp();
		out.col(i) = b / b.array().sum();
	}
}

float felli(const std::vector<float>& xx) {
  Eigen::VectorXf x = Eigen::VectorXf::Zero(xx.size());
  for (size_t i = 0; i < xx.size(); ++i)
    x[i] = xx[i];
  Eigen::VectorXf v = Eigen::VectorXf::Zero(x.size());
  for (size_t i = 0; i < v.size(); ++i)
    v[i] = powf(1e6, i / (x.size() - 1.0f));
  return v.dot((x.array() * x.array()).matrix());
}

float math_util::get_weighted_mean(std::vector<float> population_, std::vector<float> weights_)
{
    //Generate weighted mean using the weights and population values provided
    //Allocate population and weights Eigen vector
    Eigen::VectorXf population = transform_to_eigen(population_);
    Eigen::VectorXf weights = transform_to_eigen(weights_);
    //Coefficient-wise multiplication of each value with its weight
    Eigen::VectorXf weighted_population = population.array() * weights.array();
    //Return the weighted average
    return weighted_population.sum()/weights.sum();
}

std::tuple<Eigen::MatrixXf, Eigen::VectorXf> dart::SimpleWeightedError::computeGNParam(const Eigen::VectorXf &diff) {
    // compute error from joint deviation
    const double e = (_weight*diff).norm();

    // Jacobian of error, e.g. the partial derivation of the error w.r.t. to each joint value
    // For an error of zero, its partial derivative is not defined. Therefore we set its derivative to 0.
    const Eigen::MatrixXf J = (diff.array()==0).select(0, -diff.array()/e);

    const Eigen::VectorXf JTe = - diff.transpose();

    return std::make_tuple(J, JTe);
}

float math_util::get_variance(std::vector<float> distribution_)
{
    // Initialize an Eigen vector representing the distribution
    // In the following the formula for variance is implemented:
    // Var(x) = 1/N * SUM((x-mean(x))^2)
    Eigen::VectorXf distribution;
    distribution = Eigen::Map<Eigen::VectorXf>(&distribution_[0],distribution_.size(),1);
    // Retrieve mean value, mean(x)
    float distribution_mean = distribution.mean();
    // Get deviation vector. (x-mean(x))^2
    distribution = (distribution.array() - distribution_mean) * (distribution.array() - distribution_mean);
    // Get variance from deviation vector. Var(x)
    return distribution.mean();

}

void sumAndNormalize( Eigen::MatrixXf & out, const Eigen::MatrixXf & in, const Eigen::MatrixXf & Q ) {
	out.resize( in.rows(), in.cols() );
	for( int i=0; i<in.cols(); i++ ){
		Eigen::VectorXf b = in.col(i);
		Eigen::VectorXf q = Q.col(i);
		out.col(i) = b.array().sum()*q - b;
	}
}

	Eigen::MatrixXf featureGradient( const Eigen::MatrixXf & a, const Eigen::MatrixXf & b ) const {
		if (ntype_ == NO_NORMALIZATION )
			return kernelGradient( a, b );
		else if (ntype_ == NORMALIZE_SYMMETRIC ) {
			Eigen::MatrixXf fa = lattice_.compute( a*norm_.asDiagonal(), true );
			Eigen::MatrixXf fb = lattice_.compute( b*norm_.asDiagonal() );
			Eigen::MatrixXf ones = Eigen::MatrixXf::Ones( a.rows(), a.cols() );
			Eigen::VectorXf norm3 = norm_.array()*norm_.array()*norm_.array();
			Eigen::MatrixXf r = kernelGradient( 0.5*( a.array()*fb.array() + fa.array()*b.array() ).matrix()*norm3.asDiagonal(), ones );
			return - r + kernelGradient( a*norm_.asDiagonal(), b*norm_.asDiagonal() );
		}
		else if (ntype_ == NORMALIZE_AFTER ) {
			Eigen::MatrixXf fb = lattice_.compute( b );
			
			Eigen::MatrixXf ones = Eigen::MatrixXf::Ones( a.rows(), a.cols() );
			Eigen::VectorXf norm2 = norm_.array()*norm_.array();
			Eigen::MatrixXf r = kernelGradient( ( a.array()*fb.array() ).matrix()*norm2.asDiagonal(), ones );
			return - r + kernelGradient( a*norm_.asDiagonal(), b );
		}
		else /*if (ntype_ == NORMALIZE_BEFORE )*/ {
			Eigen::MatrixXf fa = lattice_.compute( a, true );
			
			Eigen::MatrixXf ones = Eigen::MatrixXf::Ones( a.rows(), a.cols() );
			Eigen::VectorXf norm2 = norm_.array()*norm_.array();
			Eigen::MatrixXf r = kernelGradient( ( fa.array()*b.array() ).matrix()*norm2.asDiagonal(), ones );
			return -r+kernelGradient( a, b*norm_.asDiagonal() );
		}
	}

float mmf::OptSO3MMFvMF::computeAssignment(uint32_t& N)
{
  N = this->cld_.counts().sum();
  // Compute log of the cluster weights and push them to GPU
  Eigen::VectorXf pi = Eigen::VectorXf::Ones(K()*6)*1000;
  std::cout << this->t_ << std::endl;
  std::cout<<"counts: "<<this->cld_.counts().transpose()<<std::endl;
  if (this->t_ == 0) {
    pi.fill(1.);
  } else if (this->t_ >24) {
    std::cout << "truncating noisy MFs: " << std::endl;
    for (uint32_t k=0; k<K()*6; ++k) {
      float count = this->cld_.counts().middleRows((k/6)*6,6).sum();
      pi(k) =  count > 0.10*N ? count : 1.e-20;
      std::cout << count  << " < " << 0.1*N << std::endl;
    }
  } else {
    // estimate the axis and MF proportions
//    pi += this->cld_.counts();
    // estimate only the MF proportions
    for (uint32_t k=0; k<K()*6; ++k)
      pi(k) += this->cld_.counts().middleRows((k/6)*6,6).sum();
    if (estimateTau_) {
      for (uint32_t k=0; k<K()*6; ++k)
        if (this->cld_.counts()(k) == 0) {
          taus_(k) = 0.; // uniform
        } else {
          Eigen::Vector3f mu = Eigen::Vector3f::Zero();
          mu((k%6)/2) = (k%6)%2==0?-1.:1.;
          mu = Rs_[k/6]*mu;
          taus_(k) = jsc::vMF<3>::MLEstimateTau(
              this->cld_.xSums().col(k).cast<double>(),
              mu.cast<double>(), this->cld_.counts()(k));
        }
    } else {
      taus_.fill(60.);
    }
  }
  std::cout<<"pi: "<<pi.transpose()<<std::endl;
  pi = (pi.array() / pi.sum()).array().log();
  std::cout<<"pi: "<<pi.transpose()<<std::endl;
  if (estimateTau_) {
    std::cout << pi.transpose() << std::endl;
    std::cout << taus_.transpose() << std::endl;
    for (uint32_t k=0; k<K()*6; ++k) {
      pi(k) -= jsc::vMF<3>::log2SinhOverZ(taus_(k)) - log(2.*M_PI);
    }
  }
  pi_.set(pi);
  Rot2Device();
  Eigen::VectorXf residuals = Eigen::VectorXf::Zero(K()*6);
  MMFvMFCostFctAssignmentGPU((float*)residuals.data(), d_cost, &N,
      d_N_, cld_.d_x(), d_weights_, cld_.d_z(), d_mu_, pi_.data(),
      cld_.N(), K());
  return residuals.sum();
};

bool ThermalCalibration::BarometerCalibration(Eigen::VectorXf pressure, Eigen::VectorXf temperature, float *result, float *inputSigma, float *calibratedSigma)
{
    // assume the nearest reading to 20°C as the "zero bias" point
    int index20deg = searchReferenceValue(20.0f, temperature);

    qDebug() << "Ref zero is " << index20deg << " T: " << temperature[index20deg] << " P:" << pressure[index20deg];
    float refZero  = pressure[index20deg];
    pressure.array() -= refZero;
    qDebug() << "Rebiased zero is " << pressure[index20deg];

    Eigen::VectorXf solution(BARO_PRESSURE_POLY_DEGREE + 1);
    if (!CalibrationUtils::PolynomialCalibration(&temperature, &pressure, BARO_PRESSURE_POLY_DEGREE, solution, BARO_PRESSURE_MAX_REL_ERROR)) {
        return false;
    }
    copyToArray(result, solution, BARO_PRESSURE_POLY_DEGREE + 1);
    ComputeStats(&temperature, &pressure, &solution, inputSigma, calibratedSigma);
    return (*calibratedSigma) < (*inputSigma);
}

void dart::ReportedJointsPrior::computeContribution(Eigen::SparseMatrix<float> & fullJTJ,
                             Eigen::VectorXf & fullJTe,
                             const int * modelOffsets,
                             const int priorParamOffset,
                             const std::vector<MirroredModel *> & models,
                             const std::vector<Pose> & poses,
                             const OptimizationOptions & opts)
{
    // get mapping of reported joint names and values
    std::map<std::string, float> rep_map;
    for(unsigned int i=0; i<_reported.getReducedArticulatedDimensions(); i++) {
        // apply lower and upper joint limits
        rep_map[_reported.getReducedName(i)] =
                std::min(std::max(_reported.getReducedArticulation()[i], _reported.getReducedMin(i)), _reported.getReducedMax(i));
    }

#ifdef LCM_DEBUG_GRADIENT
    std::vector<std::string> names;
#if FILTER_FIXED_JOINTS
    const bool pub_grad = (_skipped==GRADIENT_SKIP);
#endif
#endif

    // compute difference of reported to estimated joint value
    Eigen::VectorXf diff = Eigen::VectorXf::Zero(_estimated.getReducedArticulatedDimensions());
    for(unsigned int i=0; i<_estimated.getReducedArticulatedDimensions(); i++) {
        const std::string jname = _estimated.getReducedName(i);
#ifdef LCM_DEBUG_GRADIENT
#if FILTER_FIXED_JOINTS
        if(pub_grad)
            if( !(_estimated.getReducedMin(i)==0 && _estimated.getReducedMin(i)==0) )
#endif
                names.push_back(jname);
#endif
        float rep = rep_map.at(jname);
        float est = _estimated.getReducedArticulation()[i];
        diff[i] = rep_map.at(jname) - _estimated.getReducedArticulation()[i];
    }

    // set nan values to 0, e.g. comparison of nan values always yields false
    diff = (diff.array()!=diff.array()).select(0,diff);

    // get Gauss-Newton parameter for specific objective function
    Eigen::MatrixXf J = Eigen::MatrixXf::Zero(_estimated.getReducedArticulatedDimensions(), 1);
    Eigen::VectorXf JTe = Eigen::VectorXf::Zero(_estimated.getReducedArticulatedDimensions());
    std::tie(J,JTe) = computeGNParam(diff);

    const Eigen::MatrixXf JTJ = J.transpose()*J;

#ifdef LCM_DEBUG_GRADIENT
#if FILTER_FIXED_JOINTS
    if(pub_grad) {
#endif
        // publish gradient (JTe)
        bot_core::joint_angles_t grad;
        grad.num_joints = names.size();
        grad.joint_name = names;
        for(unsigned int i = 0; i<JTe.size(); i++) {
#if FILTER_FIXED_JOINTS
            if(!(_estimated.getReducedMin(i)==0 && _estimated.getReducedMin(i)==0))
#endif
                grad.joint_position.push_back(JTe[i]);
        }
        LCM_CommonBase::publish("DART_GRADIENT", &grad);
#if FILTER_FIXED_JOINTS
        _skipped=0;
    }
    else {
        _skipped++;
    }
#endif
#endif // LCM_DEBUG_GRADIENT

    for(unsigned int r=0; r<JTJ.rows(); r++)
        for(unsigned int c=0; c<JTJ.cols(); c++)
            if(JTJ(r,c)!=0)
                fullJTJ.coeffRef(modelOffsets[_modelID]+6+r, modelOffsets[_modelID]+6+c) += JTJ(r,c);

    for(unsigned int r=0; r<JTe.rows(); r++)
            if(JTe[r]!=0)
                fullJTe[modelOffsets[_modelID]+6+r] += JTe[r];
}

void TraceStoreCol::WellProj(TraceStoreCol &store,
                             std::vector<KeySeq> & key_vectors,
                             vector<char> &filter,
                             vector<float> &mad) {
  int useable_flows = 0;
  for (size_t i = 0; i < key_vectors.size(); i++) {
    useable_flows = std::max((int)key_vectors[i].usableKeyFlows, useable_flows);
    //    useable_flows = std::max(store.GetNumFlows(), (size_t)useable_flows);
  }
  Eigen::VectorXf norm(store.mFrameStride * useable_flows);
  Eigen::VectorXf sum(store.mFrameStride * useable_flows);

  norm.setZero();
  sum.setZero();
  int start_frame = store.GetNumFrames() - 2;
  int end_frame = store.GetNumFrames();
  int count = 0;
  for (int frame_ix = start_frame; frame_ix < end_frame; frame_ix++) {
    count++;
    int16_t *__restrict data_start = store.GetMemPtr() + frame_ix * store.mFlowFrameStride;
    int16_t *__restrict data_end = data_start + store.mFrameStride * useable_flows;
    float *__restrict norm_start = &norm[0];
    while(data_start != data_end) {
      *norm_start++ += *data_start++;
    }
  }

  // std::vector<ChipReduction> smoothed_avg(useable_flows);
  // int x_clip = mCols;
  // int y_clip = mRows;
  // if (mUseMeshNeighbors == 0) {
  //   x_clip = THUMBNAIL_SIZE;
  //   y_clip = THUMBNAIL_SIZE;
  // }
  // int last_frame = store.GetNumFrames() - 1;
  // for (int flow_ix = 0; flow_ix < useable_flows; flow_ix++) {
  //   smoothed_avg[flow_ix].Init(mRows, mCols, 1,
  //                              SMOOTH_REDUCE_STEP, SMOOTH_REDUCE_STEP,
  //                              y_clip, x_clip,
  //                              SMOOTH_REDUCE_STEP * SMOOTH_REDUCE_STEP * .4);
  //   smoothed_avg[flow_ix].ReduceFrame(&mData[0] + flow_ix * mFrameStride + last_frame  * mFlowFrameStride, &filter[0], 0);
  //   smoothed_avg[flow_ix].SmoothBlocks(SMOOTH_REDUCE_REGION, SMOOTH_REDUCE_REGION);
  // }
  // int x_count = 0;
  // for (int flow_ix = 0; flow_ix < useable_flows; flow_ix++) {
  //   for (size_t row_ix = 0; row_ix < mRows; row_ix++) {
  //     for (size_t col_ix = 0; col_ix < mCols; col_ix++) {
  //       float avg = smoothed_avg[flow_ix].GetSmoothEst(row_ix, col_ix, 0);
  //       norm[x_count] = avg;
  //       x_count++;
  //     }
  //   }
  // }

  norm = norm / count;
  //  start_frame = INTEGRATION_START;
  //  end_frame = INTEGRATION_END;
  start_frame = 5;
  end_frame = store.GetNumFrames() - 6;
  
  for (int frame_ix = start_frame; frame_ix < end_frame; frame_ix++) {
    int16_t *__restrict data_start = store.GetMemPtr() + frame_ix * store.mFlowFrameStride;
    int16_t *__restrict data_end = data_start + store.mFrameStride * useable_flows;
    float *__restrict norm_start = &norm[0];
    float *__restrict sum_start = &sum[0];
    int well_offset = 0;
    while(data_start != data_end) {
      if (*norm_start == 0.0f) {
        *norm_start = 1.0f;
        // avoid divide by zero but mark that something is wrong with this well..
        if (filter[well_offset] == 0) {
          filter[well_offset] = 6;
        }
      }
      *sum_start += *data_start / *norm_start;
      well_offset++;
      // reset each flow
      if (well_offset == (int) store.mFrameStride) {
        well_offset = 0;
      }
      sum_start++;
      data_start++;
      norm_start++;
    }
  }
  
  Eigen::MatrixXf flow_mat(store.mFrameStride, useable_flows);
  for (int flow_ix = 0; flow_ix < flow_mat.cols(); flow_ix++) {
    for (size_t well_ix = 0; well_ix < store.mFrameStride; well_ix++) {
      flow_mat(well_ix, flow_ix) = sum(flow_ix * store.mFrameStride + well_ix);
    }
  }

  Eigen::VectorXf n2;
  n2 = flow_mat.rowwise().squaredNorm();
  n2 = n2.array().sqrt();
  float *n2_start = n2.data();
  float *n2_end = n2_start + n2.rows();
  while (n2_start != n2_end) {
    if (*n2_start == 0.0f) { *n2_start = 1.0f; }
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    const InputImageType * inputy = readery->GetOutput();
    const InputImageType * inputz = readerz->GetOutput();


    //create the output image
    OutputImageType::Pointer image = OutputImageType::New();
    image->SetRegions(inputx->GetLargestPossibleRegion());
    image->SetSpacing(inputx->GetSpacing());
    image->SetOrigin(inputx->GetOrigin());
    image->Allocate();
    image->FillBuffer(0);

    
    InputImageType::SizeType radius;
    radius[0] = neighborhood_radius;
    radius[1] = neighborhood_radius;
    radius[2] = neighborhood_radius;
    
    itk::ConstNeighborhoodIterator<InputImageType> 
            iteratorx(radius, inputx, image->GetRequestedRegion());
    itk::ConstNeighborhoodIterator<InputImageType> 
            iteratory(radius, inputy, image->GetRequestedRegion());
    itk::ConstNeighborhoodIterator<InputImageType> 
            iteratorz(radius, inputz, image->GetRequestedRegion());

    itk::ImageRegionIterator<OutputImageType>  it( image, image->GetRequestedRegion() );
    
    iteratorx.GoToBegin();
    iteratory.GoToBegin();
    iteratorz.GoToBegin();
    it.GoToBegin();
  
    const unsigned int nneighbors = (neighborhood_radius*2+1)*(neighborhood_radius*2+1)*(neighborhood_radius*2+1);
    Eigen::MatrixXf neighbors( nneighbors-1, 3 ); //to store the vectors
    Eigen::Vector3f central_vector;
    
    int c_id = (int) (nneighbors / 2); // get offset of center pixel
    
    
    //check image size for the progress indicator
    OutputImageType::RegionType region = image->GetLargestPossibleRegion();
    OutputImageType::SizeType size = region.GetSize();
    const unsigned long int npixels = size[0]*size[1]*size[2];
    
    unsigned long int k = 0;
    while ( ! iteratorx.IsAtEnd() )
    {
        if(k%10000 == 0)
        {
            std::cout<<"Processed "<<k<<"/"<<npixels<<"\r"<<std::flush;
        }
            
        k++;
            
        unsigned int j = 0;
        for (unsigned int i = 0; i < nneighbors; ++i)
        {    
            if(i!=c_id) //exclude the center
            {
                neighbors(j, 0) = iteratorx.GetPixel(i);  
                neighbors(j, 1) = iteratory.GetPixel(i);  
                neighbors(j, 2) = iteratorz.GetPixel(i);  
                j++;
            }
            else
            {
                central_vector(0) = iteratorx.GetPixel(i);  
                central_vector(1) = iteratory.GetPixel(i);  
                central_vector(2) = iteratorz.GetPixel(i);                  
            }
        }
        
        // calculate the angles 
        Eigen::VectorXf dp = neighbors*central_vector;
        
        const float variance = ((dp.array()-dp.mean()).pow(2)).mean();
        it.Set(variance);        
        
        ++iteratorx;
        ++iteratory;
        ++iteratorz;
        ++it;
    }    
    std::cout<<std::endl;
    
    //  The output of the resampling filter is connected to a writer and the
    //  execution of the pipeline is triggered by a writer update.
    try {
        writer->SetInput(image);
        writer->Update();
    } catch (itk::ExceptionObject & excep) {
        std::cerr << "Exception caught !" << std::endl;
        std::cerr << excep << std::endl;
    }

    

    
    return EXIT_SUCCESS;
}