//============================================================================ // 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; }
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; }
// 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; }
/*** 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; } }
double scenario::tangent_Prob() const { return probof(helix_chi2(), ndof()); }