void project(const Template &src, Template &dst) const
{
dst = cv::Mat(1, keep, CV_32FC1);
// Map Eigen into OpenCV
Eigen::Map<const Eigen::MatrixXf> inMap(src.m().ptr<float>(), src.m().rows*src.m().cols, 1);
Eigen::Map<Eigen::MatrixXf> outMap(dst.m().ptr<float>(), keep, 1);
// Do projection
outMap = eVecs.transpose() * (inMap - mean);
}
bool go(costmap_2d::Costmap2D &gradient_, costmap_2d::Costmap2D &original_, int i, int j, int cost)
{
if(inMap(gradient_, i, j) == false){
//ROS_INFO("salgo por no estar en mapa");
return false;
}
if(original_.getCost( i, j) != 0 ){
//ROS_INFO("salgo por ser obstaculo");
return false;
}
if(gradient_.getCost( i, j) <= cost){
//ROS_INFO("salgo por ser igual o menor");
return false;
}
return true;
}
void GameMap::setTile(int x, int y, int n)
{
if (!inMap(x, y)) return;
map[x][y] = n;
hasChanged = true;
}
bool GameMap::isRightBlocking(int x, int y)
{
if (!inMap(x, y)) return false;
return (map[x][y] > 0);
}
bool GameMap::isUpBlocking(int x, int y)
{
if (y < 0) return true;
if (!inMap(x, y)) return false;
return (map[x][y] > 0);
}
int GameMap::getTile(int x, int y)
{
if (!inMap(x, y)) return -1;
return map[x][y];
}
