コード例 #1
0
ファイル: TbKalmanTrack.cpp プロジェクト: ruby64/Tb
//============================================================================
// Print some debug info
//============================================================================
void TbKalmanTrack::print() const {
  std::cout << "This is a kalman fitted tracks with chi2/ndof=" << chi2() << "/"
            << ndof() << std::endl;
  // compute forward/backward chi2
  LHCb::ChiSquare forwardchi2, backwardchi2;
  double chi2X(0), chi2Y(0);
  std::cout << "These are the nodes, with some residuals: " << std::endl;
  for (const LHCb::TbKalmanNode * node : m_nodes) {
    std::cout << node->index() << " " << node->z() << " ";
    //<< node->hasInfoUpstream( LHCb::TbKalmanNode::Forward) << " "
    //<< node->hasInfoUpstream( LHCb::TbKalmanNode::Backward) << " " ;
    printState(node->state(), std::cout);
    //std::cout << node->filterStatus( LHCb::TbKalmanNode::Forward) << " "
    //<< node->filterStatus( LHCb::TbKalmanNode::Backward) << " " ;
    const TbKalmanPixelMeasurement* pixelhit =
        dynamic_cast<const TbKalmanPixelMeasurement*>(node);
    if (pixelhit) {
      std::cout << "residual x = " << pixelhit->residualX() << " +/- "
                << std::sqrt(pixelhit->residualCovX()) << " "
                << "residual y = " << pixelhit->residualY() << " +/- "
                << std::sqrt(pixelhit->residualCovY()) << " ";
      chi2X += pixelhit->residualX() * pixelhit->residualX() / pixelhit->covX();
      chi2Y += pixelhit->residualY() * pixelhit->residualY() / pixelhit->covY();
    }
    std::cout << std::endl;
    forwardchi2 += node->deltaChi2(LHCb::TbKalmanNode::Forward);
    backwardchi2 += node->deltaChi2(LHCb::TbKalmanNode::Backward);
  }
  std::cout << "Forward/backward chi2: " << forwardchi2.chi2() << "/"
            << backwardchi2.chi2() << std::endl;
  std::cout << "X/Y chi2: " << chi2X << "/" << chi2Y << std::endl;
}
コード例 #2
0
 Status BaseMultiPos::
  update(Model const & model)
  {
    Status st;    
    Vector cur_eepos(Vector::Zero(3));
    Vector delta;
    Vector v_delta;

    size_t ndof(model.getUnconstrainedNDOF());

    for (size_t ii(0); ii < task_table_.size(); ++ii) {
      st = task_table_[ii]->update(model);
      if ( ! st) { return st; }
    }
    for(int ii=0; ii<3; ii++) {
      cur_eepos[ii] = ee_pos_[3*cur_row_+ii];
    }
    
    delta = cur_eepos - ee_task_->getActual();
    jspace::Constraint* constraint = model.getConstraint();
    if (constraint) {
      jspace::State fullState(model.getNDOF(),model.getNDOF(),6);
      constraint->getFullState(model.getState(),fullState);
      v_delta = ee_task_->getJacobian()*fullState.velocity_;
    }
    else {
      v_delta = ee_task_->getJacobian()*model.getState().velocity_;
    }
    
    if (delta.norm() < threshold_ && v_delta.norm() < vel_threshold_) {
      if(forward_) {
	if (cur_row_ < (ee_pos_.rows()/3)-1) {
	  ++cur_row_;
	  for(size_t jj(0); jj<3; ++jj) {
	    cur_eepos[jj] = ee_pos_[3*cur_row_+jj];
	  }
	  st = ee_goal_->set(cur_eepos);
	  if (! st) { return st; } 
	}
	else { forward_ = false; }
      }
      else {
	if (cur_row_ > 0) {
	  --cur_row_;
	  for(size_t jj(0); jj<3; ++jj) {
	    cur_eepos[jj] = ee_pos_[3*cur_row_+jj];
	  }
	  st = ee_goal_->set(cur_eepos);
	  if (! st) { return st; } 
	}
	else { forward_ = true; }
      }
    }
 
      return st;
    }
コード例 #3
0
ファイル: element.C プロジェクト: shkeshavarz/OOF2
// Create a vector mapping the canonical order of the Element's
// degrees of freedom to the DOF's actual indices.  Almost identical
// code to the routine below.
std::vector<int> Element::localDoFmap() const {
  std::vector<int> dofmap(ndof(),-1);
  for(std::vector<Field*>::size_type fi=0; fi< Field::all().size(); fi++) {
    Field &field = *Field::all()[fi];
    // Field components.
    for(IteratorP fcomp=field.iterator(ALL_INDICES); !fcomp.end(); ++fcomp) {
      // Nodes
      for(CleverPtr<ElementFuncNodeIterator> node(funcnode_iterator());
	  !node->end(); ++*node)
	{
	  if(node->hasField(field)) {
	    DegreeOfFreedom *dof = field(*node, fcomp.integer());
	    dofmap[node->localindex(field, fcomp)] = dof->dofindex();
	  }
	}
    }
  }
  return dofmap;
}
コード例 #4
0
ファイル: scenario.cpp プロジェクト: lemiere/Falaise
/*** dump ***/
void scenario::dump(std::ostream &a_out, const std::string &a_title, const std::string &a_indent,
                    bool /* a_inherit */) const {
  {
    std::string indent;
    if (!a_indent.empty()) indent = a_indent;
    if (!a_title.empty()) {
      a_out << indent << a_title << std::endl;
    }

    a_out << indent << appname_ << " -------------- " << std::endl;
    a_out << indent << "helix_chi2 : " << helix_chi2() << "tangent_chi2 : " << tangent_chi2()
          << " ndof " << ndof() << " helix_prob " << helix_Prob() << " tangent_prob "
          << tangent_Prob() << std::endl;
    a_out << indent << "n free families : " << n_free_families() << std::endl;
    a_out << indent << "n overlaps : " << n_overlaps() << std::endl;
    for (std::vector<sequence>::const_iterator iseq = sequences_.begin(); iseq != sequences_.end();
         ++iseq)
      iseq->dump();
    a_out << indent << " -------------- " << std::endl;

    return;
  }
}
コード例 #5
0
ファイル: scenario.cpp プロジェクト: lemiere/Falaise
double scenario::tangent_Prob() const { return probof(helix_chi2(), ndof()); }