//============================================================================
// 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()); }
