コード例 #1
0
  Eigen::Vector3d& VertexPointXYZCov::normal(){
    Eigen::Matrix3d eigvectors;
    Eigen::Vector3d eigvalues;
    if(!_hasNormal && _cov != Eigen::Matrix3d::Identity()){
      pcl::eigen33<Matrix3d, Vector3d> (_cov, eigvectors, eigvalues);

      _normal = Vector3d(0.0, 0.0, 0.0);
      double eig_sum = eigvalues.sum();
      if(eig_sum != 0){
        if(eigvalues(0) < eigvalues(1) && eigvalues(0) < eigvalues(2)){
          _normal = Vector3d(eigvectors(0,0), eigvectors(1,0), eigvectors(2,0));
        }
        if(eigvalues(1) < eigvalues(0) && eigvalues(1) < eigvalues(2)){
          _normal = Vector3d(eigvectors(0,1), eigvectors(1,1), eigvectors(2,1));
        }
        if(eigvalues(2) < eigvalues(0) && eigvalues(2) < eigvalues(1)){
          _normal = Vector3d(eigvectors(0,2), eigvectors(1,2), eigvectors(2,2));
        }
  
        //check direction of normal
        Vector3d laserPoint(_estimate[0], _estimate[1], _estimate[2]);
        Vector3d beam = (parentVertex()->estimate().translation() - laserPoint);
        if(beam.dot(_normal) < 0){
          _normal = -_normal;
        }
      }     
      _hasNormal = true;
    }
    return _normal;
  }
コード例 #2
0
  void VertexPointXYZCov::updateNormal(Eigen::Matrix3d& cov){
    Eigen::Matrix3d eigvectors;
    Eigen::Vector3d eigvalues;
    if(cov != Eigen::Matrix3d::Identity()){
      pcl::eigen33<Matrix3d, Vector3d> (cov, eigvectors, eigvalues);

      _normal = Vector3d(0.0, 0.0, 0.0);
      double eig_sum = eigvalues.sum();
      if(eig_sum != 0){
        if(eigvalues(0) < eigvalues(1) && eigvalues(0) < eigvalues(2)){
          _normal = Vector3d(eigvectors(0,0), eigvectors(1,0), eigvectors(2,0));
        }
        if(eigvalues(1) < eigvalues(0) && eigvalues(1) < eigvalues(2)){
          _normal = Vector3d(eigvectors(0,1), eigvectors(1,1), eigvectors(2,1));
        }
        if(eigvalues(2) < eigvalues(0) && eigvalues(2) < eigvalues(1)){
          _normal = Vector3d(eigvectors(0,2), eigvectors(1,2), eigvectors(2,2));
        }
  
        //check direction of normal
        Eigen::Vector3d laserPoint(_estimate[0], _estimate[1], _estimate[2]);
        Eigen::Vector3d beam = (parentVertex()->estimate().translation() - laserPoint);
        if(beam.dot(_normal) < 0){
          _normal = -_normal;
        }
        
//         Eigen::Matrix3d coordinateFrameRotation;
//         Eigen::Vector3d yDirection(0,1,0);
// 	coordinateFrameRotation.row(2) = _normal;
// 	yDirection = yDirection - _normal(1)*_normal;
// 	yDirection.normalize();/// need to check if y is close to 0
// 	coordinateFrameRotation.row(1) = yDirection;
// 	coordinateFrameRotation.row(0) = _normal.cross(yDirection);
// 	
// 	_cov = Matrix3d::Identity();
// 	_cov(0,0) = 0.0001; ///Maybe we should make this resolution dependent
// 	_cov(1,1) = 0.0001; ///Maybe we should make this resolution dependent
// 	_cov(2,2) = 0.0001; ///Maybe we should make this resolution dependent
// 	_cov = coordinateFrameRotation.transpose()*_cov*coordinateFrameRotation;	
	_normal = parentVertex()->estimate().linear().inverse() * _normal; //rotate in local coordinate frame
      }     
      _hasNormal = true;
    }  
  }
コード例 #3
0
void TrajectoryAnalyzer::analyze(const Eigen::MatrixXd & nma_covariance,
                                 std::vector<std::shared_ptr<ProteinSegment>> & protein_segments,
                                 const double & temperature) {

    LOGD << "Fitting protein segments with NMA covariance matrix and computing force constants.";
    for (std::shared_ptr<ProteinSegment> const & protein_segment : protein_segments) {
        Eigen::MatrixXd covariance = nma_covariance.block((protein_segment->get_start_residuum_nr() - 1) * 3,
                                                                (protein_segment->get_start_residuum_nr() - 1) * 3,
                                                                protein_segment->get_size() * 3,
                                                                protein_segment->get_size() * 3);

        Eigen::VectorXd displacement_vector = covariance.diagonal().array().sqrt().matrix();

        Eigen::MatrixXd kk_matrix = Eigen::MatrixXd::Zero(protein_segment->get_size(), protein_segment->get_size());
        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(covariance);

        for (int i = 0; i < protein_segment->get_size(); i++) {
            for (int j = 0; j < i; j++) {
                Eigen::Vector3d kk = Eigen::Vector3d::Zero();
                Eigen::Vector3d d = displacement_vector.segment<3>(3 * i)-displacement_vector.segment<3>(3 * j);

                for(int k = (protein_segment->get_size() > 2 ? 6 : 5); k < protein_segment->get_size() * 3; k++) {
                    kk += Eigen::Vector3d(
                        eig.eigenvectors()(3 * i + 0, k) * eig.eigenvectors()(3 * j + 0, k),
                        eig.eigenvectors()(3 * i + 1, k) * eig.eigenvectors()(3 * j + 1, k),
                        eig.eigenvectors()(3 * i + 2, k) * eig.eigenvectors()(3 * j + 2, k))
                        / eig.eigenvalues()[k];
                }
                kk_matrix(i, j) = -kk.sum() * 8.31 * temperature * 0.001;
                kk_matrix(j, i) = d.norm();
            }
        }

        protein_segment->add_force_constant(kk_matrix);
        protein_segment->add_displacement_vector(displacement_vector);
        protein_segment->add_mean_square_fluctuation(covariance);
    }
    LOGD << "Finished analyzing trajectory";
}
コード例 #4
0
ファイル: tensor2metric.cpp プロジェクト: emanuele/mrtrix3
 void operator() (Image<value_type>& dt_img)
 {
   /* check mask */ 
   if (mask_img.valid()) {
     assign_pos_of (dt_img, 0, 3).to (mask_img);
     if (!mask_img.value())
       return;
   }
  
   /* input dt */
   Eigen::Matrix<double, 6, 1> dt;
   for (auto l = Loop (3) (dt_img); l; ++l)
     dt[dt_img.index(3)] = dt_img.value();
   
   /* output adc */
   if (adc_img.valid()) {
     assign_pos_of (dt_img, 0, 3).to (adc_img);
     adc_img.value() = DWI::tensor2ADC(dt);
   }
   
   double fa = 0.0;
   if (fa_img.valid() || (vector_img.valid() && (modulate == 1)))
     fa = DWI::tensor2FA(dt);
   
   /* output fa */
   if (fa_img.valid()) {
     assign_pos_of (dt_img, 0, 3).to (fa_img);
     fa_img.value() = fa;
   }
   
   bool need_eigenvalues = value_img.valid() || (vector_img.valid() && (modulate == 2)) || ad_img.valid() || rd_img.valid() || cl_img.valid() || cp_img.valid() || cs_img.valid();
   
   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;
   if (need_eigenvalues || vector_img.valid()) {
     Eigen::Matrix3d M;
     M (0,0) = dt[0];
     M (1,1) = dt[1];
     M (2,2) = dt[2];
     M (0,1) = M (1,0) = dt[3];
     M (0,2) = M (2,0) = dt[4];
     M (1,2) = M (2,1) = dt[5];
     es = Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>(M, vector_img.valid() ? Eigen::ComputeEigenvectors : Eigen::EigenvaluesOnly);
   }
   
   Eigen::Vector3d eigval;
   if (need_eigenvalues)
     eigval = es.eigenvalues();
     
   /* output value */
   if (value_img.valid()) {
     assign_pos_of (dt_img, 0, 3).to (value_img);
     if (vals.size() > 1) {
       auto l = Loop(3)(value_img);
       for (size_t i = 0; i < vals.size(); i++) {
         value_img.value() = eigval(3-vals[i]); l++;
       }
     } else {
       value_img.value() = eigval(3-vals[0]);
     }
   }
   
   /* output ad */
   if (ad_img.valid()) {
     assign_pos_of (dt_img, 0, 3).to (ad_img);
     ad_img.value() = eigval(2);
   }
   
   /* output rd */
   if (rd_img.valid()) {
     assign_pos_of (dt_img, 0, 3).to (rd_img);
     rd_img.value() = (eigval(1) + eigval(0)) / 2;
   }
   
   /* output shape measures */
   if (cl_img.valid() || cp_img.valid() || cs_img.valid()) {
     double eigsum = eigval.sum();
     if (eigsum != 0.0) {
       if (cl_img.valid()) {
         assign_pos_of (dt_img, 0, 3).to (cl_img);
         cl_img.value() = (eigval(2) - eigval(1)) / eigsum;
       }
       if (cp_img.valid()) {
         assign_pos_of (dt_img, 0, 3).to (cp_img);
         cp_img.value() = 2.0 * (eigval(1) - eigval(0)) / eigsum;
       }
       if (cs_img.valid()) {
         assign_pos_of (dt_img, 0, 3).to (cs_img);
         cs_img.value() = 3.0 * eigval(0) / eigsum;
       }
     }
   }
   
   /* output vector */
   if (vector_img.valid()) {
     Eigen::Matrix3d eigvec = es.eigenvectors();
     assign_pos_of (dt_img, 0, 3).to (vector_img);
     auto l = Loop(3)(vector_img);
     for (size_t i = 0; i < vals.size(); i++) {
       double fact = 1.0;
       if (modulate == 1)
         fact = fa;
       else if (modulate == 2)
         fact = eigval(3-vals[i]);
       vector_img.value() = eigvec(0,3-vals[i])*fact; l++;
       vector_img.value() = eigvec(1,3-vals[i])*fact; l++;
       vector_img.value() = eigvec(2,3-vals[i])*fact; l++;
     }
   }                   
 }