void GmGridMapView::AddNeighbor(int node, float d, FastVector<AStar::EdgeInfo<int,float>>& neighbors) { if (IsCellTraversable(node)) { neighbors.push_back(AStar::EdgeInfo<int,float>(node, d)); } }
/* ************************************************************************* */ HessianFactor::HessianFactor() : info_(matrix_) { // The empty HessianFactor has only a constant error term of zero FastVector<size_t> dims; dims.push_back(1); info_.resize(dims.begin(), dims.end(), false); info_(0,0)(0,0) = 0.0; assertInvariants(); }