Warp MT::Lucas_Kanade(Warp warp)
{
for (int iter = 0; iter < max_iteration; ++iter) {
Matx61f G;
Matx<float, 6, 6> H;
G = 0.0f;
H = 0.0f;
float E = 0.0f;
for (int i = 0; i < fine_samples.size(); ++i) {
Matx<float, L4, 1> T(fine_model.ptr<float>(i)), F;
Matx<float, 2, L4> dF;
Matx<float, 2, 6> dW = warp.gradient(fine_samples[i]);
Point2f p = warp.transform2(fine_samples[i]);
feature.gradient4(p.x, p.y, F.val, dF.val, dF.val + L4);
T -= F;
float e = sigmoid(T.dot(T));
E += e;
float w = sigmoid_factor * e * (1.0f - e);
G += w * (dW.t() * (dF * T));
H += w * (dW.t() * (dF * dF.t()) * dW);
}
E = E / fine_samples.size();
Matx61f D;
solve(H, G, D, DECOMP_SVD);
warp.steepest(D);
if (iter > 1 && D(3) * D(3) + D(4) * D(4) + D(5) * D(5) < translate_eps) {
if (log != NULL)
(*log) << "\terror in iteration " << iter << " = " << E << endl;
break;
}
}
return warp;
}
void Player::Update(float dt) {
Actor::Update(dt);
if(_distanceTraveled > 32.0f) {
Warp* warp = _level->CheckWarp(x, y, GetMaxWidth(), GetMaxHeight());
if(warp) {
_level->GetGame()->Warp(warp->GetTargetMap(), warp->GetTargetX(), warp->GetTargetY());
_distanceTraveled = 0;
}
}
}
Warp* Level::CheckWarp(float x, float y, float w, float h) const {
Rect objectArea(x, y, w, h);
for(std::list<Warp*>::const_iterator i = _warps.begin(); i != _warps.end(); ++i) {
Warp* warp = (*i);
Rect warpArea(
warp->GetX(), warp->GetY(),
warp->GetWidth(), warp->GetHeight());
if(warpArea.CollidesWith(objectArea)) {
return warp;
}
}
return NULL;
}
rect_t MT::retrack(const unsigned char *gray, const vector<rect_t> &detections)
{
number_coarse = number_MLK = number_iteration = 0;
if (log != NULL) {
(*log) << "roll = " << roll * 90.0f / PI_2 << endl;
(*log) << "yaw = " << yaw * 90.0f / PI_2 << endl;
(*log) << "pitch = " << pitch * 90.0f / PI_2 << endl;
}
feature.process(gray, 0.0f);
//the first attempt
Warp w = warp;
w.setr(Vector3f(0.0f, 0.0f, 0.0f));
if (log != NULL)
(*log) << "track at " << w.t.transpose() << " " << window(w.t) << endl;
w = fine_test(w);
float e = evaluate(w);
//use coarse searching
Warp w2 = warp;
w2.setr(Vector3f(0.0f, 0.0f, 0.0f));
w2.sett(fast_test(warp));
if (log != NULL)
(*log) << "search at " << w2.t.transpose() << " " << window(w2.t) << endl;
w2 = fine_test(w2);
float e2 = evaluate(w2);
if (e2 < e) {
w = w2;
e = e2;
}
//use re-detection
for (auto d : detections) {
Warp w3 = warp;
w3.setr(Vector3f(0.0f, 0.0f, 0.0f));
w3.sett(locate(d));
if (log != NULL)
(*log) << "detect at " << w3.t.transpose() << " " << window(w3.t) << endl;
w3 = fine_test(w3);
float e3 = evaluate(w3);
if (e3 < e) {
w = w3;
e = e3;
}
}
update(w, e);
return window(w.t);
}
void samplePatch(const Warp& warp,
const cv::Mat& image,
cv::Mat& patch,
int width,
bool invert,
int interpolation) {
cv::Mat M = warp.matrix();
samplePatchAffine(image, patch, M, width, invert, interpolation);
}
// Returns false if the optimization did not converge.
bool trackPatch(Warp& warp,
const cv::Mat& reference,
const cv::Mat& image,
const cv::Mat& ddx_image,
const cv::Mat& ddy_image,
const cv::Mat& mask,
const FlowOptions& options) {
CHECK(reference.rows == reference.cols) << "Template must be square";
// Set up non-linear optimization problem.
ceres::CostFunction* objective = new WarpCost(*warp.warper(), reference,
image, ddx_image, ddy_image, mask, options.interpolation,
options.check_condition, options.max_condition);
ceres::Problem problem;
problem.AddResidualBlock(objective, NULL, warp.params());
// Solve!
ceres::Solver::Summary summary;
ceres::Solve(options.solver_options, &problem, &summary);
// Ensure there was no catastrophic failure.
CHECK(summary.termination_type != ceres::DID_NOT_RUN);
// Numerical failure can be caused by e.g. numbers going to infinity.
if (summary.termination_type == ceres::NUMERICAL_FAILURE) {
DLOG(INFO) << "Numerical failure";
return false;
}
// Iteration limit was reached before any of the convergece criteria?
if (options.iteration_limit_is_fatal &&
summary.termination_type == ceres::NO_CONVERGENCE) {
DLOG(INFO) << "Reached iteration limit";
return false;
}
return true;
}
Warp MT::Lucas_Kanade(Warp warp)
{
//See "Lucas-Kanade 20 years on A unifying framework"
float last_E = 1.0f;
for (int iter = 0; iter < max_iteration; ++iter) {
++number_iteration;
Matrix<float, 6, 1> G = Matrix<float, 6, 1>::Constant(0.0f);
Matrix<float, 6, 6> H = Matrix<float, 6, 6>::Constant(0.0f);
float E = 0.0f;
for (int i = 0; i < fine_samples.size(); ++i) {
Matrix<float, 2, 6> dW;
Vector2f p = warp.gradient(fine_samples[i], dW);
Vector32f F;
Matrix<float, 32, 2> dF;
feature.gradient4(p.x(), p.y(), F.data(), dF.col(0).data(), dF.col(1).data());
F -= fine_model.col(i);
float e = sigmoid(F.squaredNorm());
float w = sigmoid_factor * e * (1.0f - e);
G += w * (dW.transpose() * (dF.transpose() * -F));
//H.triangularView<Upper> += w * (dW.transpose() * (dF.transpose() * dF) * dW);
hessian(H, w, dW, dF);
E += e;
}
E = E / fine_samples.size();
H.triangularView<Lower>() = H.transpose();
Matrix<float, 6, 1> D = H.fullPivHouseholderQr().solve(G);
warp.steepest(D);
if (log != NULL)
(*log) << E << " ";
if (iter > 1 && D.segment<3>(3).squaredNorm() < translate_eps && last_E - E < error_eps)
break;
last_E = E;
}
if (log != NULL)
(*log) << endl;
return warp;
}
float MT::evaluate(Warp warp)
{
rect_t rect = window(warp.t);
float fine_cell = sqrt(rectArea(rect) / cell_n);
feature.set_cell(fine_cell);
feature.set_step(1);
float E = 0.0f;
for (int i = 0; i < fine_samples.size(); ++i) {
Vector2f p = warp.transform2(fine_samples[i]);
Vector32f F;
feature.descriptor4(p.x(), p.y(), F.data());
E = E + sigmoid((F - fine_model.col(i)).squaredNorm());
}
return E / fine_samples.size();
}
float MT::evaluate(Warp warp)
{
Rect2f rect = window(warp.t);
float fine_cell = sqrt(rect.area() / cell_n);
feature.set_cell(fine_cell);
feature.set_step(1);
float E = 0.0f;
for (int i = 0; i < fine_samples.size(); ++i) {
Matx<float, L4, 1> T(fine_model.ptr<float>(i)), I;
Point2f p = warp.transform2(fine_samples[i]);
feature.descriptor4(p.x, p.y, I.val);
T -= I;
E = E + sigmoid(T.dot(T));
}
return E / fine_samples.size();
}
void MT::fine_train(Warp warp)
{
Rect2f rect = window(warp.t);
float fine_cell = sqrt(rect.area() / cell_n);
feature.set_cell(fine_cell);
feature.set_step(1);
Mat model(fine_samples.size(), L4, CV_32FC1);
for (int i = 0; i < fine_samples.size(); ++i) {
Point2f p = warp.transform2(fine_samples[i]);
feature.descriptor4(p.x, p.y, model.ptr<float>(i));
}
if (fine_model.empty()) {
N = 1;
fine_model = model;
}
else {
++N;
fine_model = (float(N - 1) / N) * fine_model + (1.0f / N) * model;
}
}
void MT::fine_train(Warp warp)
{
rect_t rect = window(warp.t);
float fine_cell = sqrt(rectArea(rect) / cell_n);
feature.set_cell(fine_cell);
feature.set_step(1);
//compute the features on the grid
MatrixXf model(32, fine_samples.size());
for (int i = 0; i < fine_samples.size(); ++i) {
Vector2f p = warp.transform2(fine_samples[i]);
feature.descriptor4(p.x(), p.y(), model.col(i).data());
}
//update using a moving averaging manner
if (N == 0) {
N = 1;
fine_model = model;
}
else {
++N;
fine_model = (float(N - 1) / N) * fine_model + (1.0f / N) * model;
}
}
int main()
{
start:
vector<string> fileNames; //stores all the file names in the directory
string dir_name = "./maps"; // current directory
DIR *dir;
struct dirent *ent;
struct stat filestat;
int i=1;
// for warping, stores the warps positions
int warpRow1[10];
int warpCol1[10];
int countOcc[10];
int warpRow2[10];
int warpCol2[10];
for(int a=0 ; a<10 ; a++)
{
warpRow1[a] = 10;
warpCol1[a] = 10;
countOcc[a] = 0;
warpRow2[a] = 10;
warpCol2[a] = 10;
}
int rows,columns,time; // to initialize the grid and world
int play_row,play_col; // tracks player position
int host = 0; // tracks number of hostages in the maze
bool bombDeployed = false; // checks of bomb is deployed or not. value is true if deployed
int deployedAtX, deployedAtY; // position of the bomb deployed on the maze
// for counting the moves so that bomb can be exploded after 3 moves
int count = 0;
string move_choice; // to input the moves of the user
ifstream gameMap;
World myWorld;
Grid myGrid;
Player *player = new Player();
int choice;
cout<<"Welcome to the 'Hostage Rescue' game. Press enter to continue"<<endl;
start1:
cin.clear();
cin.ignore();
cout<<"________________________________________"<<endl;
cout<<"Welcome to the well-made game"<<endl;
cout<<"----------------------------------------"<<endl;
cout<<"1. Load the maze"<<endl;
cout<<"2. Help" <<endl;
cout<<"3. About"<<endl;
cout<<"_________________________________________"<<endl;
cin>>choice;
cout<<endl;
if(choice == 1)
{
int maze_choice;
cout<<"__________________________________________________________________"<<endl;
cout<<"Load the maze menu. select your maze "<<endl;
cout<<"------------------------------------------------------------------"<<endl;
if ((dir = opendir (dir_name.c_str())) != NULL) {
/* print all the files and directories within directory */
cout << "Printing all the files in current directory..." << endl << endl;
while ((ent = readdir (dir)) != NULL) {
// Skip the file if it is invalid or it is a directory
string filepath = dir_name + "/" + ent->d_name;
if (stat( filepath.c_str(), &filestat ))
continue;
if (S_ISDIR( filestat.st_mode ))
continue;
cout << i<<")"<<ent->d_name << endl;
i++;
fileNames.push_back(ent->d_name); // storing in the vector
}
closedir (dir);
}
else
{
/* could not open directory */
cerr << strerror(errno) << endl;
return EXIT_FAILURE;
}
cout<<"__________________________________________________________________"<<endl;
cin>>maze_choice;
//Loading the required level form maps folder
for(int j=0 ; j<fileNames.size() ; j++)
{
if(j == maze_choice-1)
{
string fileName = ".\\maps\\"+fileNames.at(j);
gameMap.open(fileName.c_str());
}
}
// check bounds of file
if(gameMap.fail())
{
cout << "error opening the file" << endl;
exit(1);
}
else
{
// taking the rows and column of the grid as well as the maximum moves a player can
//make in the form of time
gameMap >> rows >> columns >> time;
// declaring a 2-d array to store the map
char **map = new char*[rows];
// storing the entire maze on the grid from the map array
for(int i=0; i<rows; i++)
{
map[i] = new char[columns];
}
char ch = gameMap.get();
// Initialising the map to declared 2-d array
for (int r=0; r<rows; r++)
{
int c=0;
char ch = gameMap.get();
while(ch!='\n' && !gameMap.eof())
{
map[r][c] = ch;
c++;
ch = gameMap.get();
}
}
gameMap.close();
// creating a world object and initialising its rows, columns and time-limit values
myWorld.setTime(time);
myWorld.setMaxTime(time);
// creating the required grid
myGrid.setGridSize(rows,columns);
// reading the map point by point
for(int i=0 ; i<rows ; i++)
{
for(int j=0 ; j<columns ; j++)
{
// positioning the actors and putting them onto the grid
if(map[i][j] == 'X')
{
Wall *wall = new Wall();
wall->setGrid(&myGrid);
myGrid.getLocation(i, j)->setWall(wall);
}
else if(map[i][j] == 'B')
{
Box *box = new Box();
box->setGrid(&myGrid);
myGrid.getLocation(i, j)->setBox(box);
}
else if(map[i][j] == 'o')
{
Bomb *bomb = new Bomb() ;
bomb->setGrid(&myGrid);
myGrid.getLocation(i, j)->setBomb(bomb);
}
else if(map[i][j] == 'E')
{
Exit *exitt = new Exit() ;
exitt->setGrid(&myGrid);
myGrid.getLocation(i, j)->setExit(exitt);
}
else if(map[i][j] == 'P')
{
myGrid.getLocation(i, j)->setPlayer(player);
play_row = i;
play_col = j;
player->setGrid(&myGrid);
player->setLocation(i,j);
}
else if(map[i][j] == 'H')
{
Hostage *hostage = new Hostage();
hostage->setGrid(&myGrid);
myGrid.getLocation(i, j)->setHost(hostage);
host++;
}
else if(map[i][j] == '.')
{
EmptySpace *emptyspace = new EmptySpace();
emptyspace->setGrid(&myGrid);
myGrid.getLocation(i, j)->setEmptySpace(emptyspace);
}
else
{
Warp *warp = new Warp(map[i][j]);
warp->setSymbol(map[i][j]);
warp->setGrid(&myGrid);
myGrid.getLocation(i, j)->setWarp(warp);
if(countOcc[map[i][j] - '0'] > 0)
{
warpRow2[map[i][j] - '0'] = i;
warpCol2[map[i][j] - '0'] = j;
}
else
{
warpRow1[map[i][j] - '0'] = i;
warpCol1[map[i][j] - '0'] = j;
}
countOcc[map[i][j] - '0']++;
}
}
}
myGrid.display(cout);
for(int t=0;t<10;t++)
{
if(countOcc[t]>0)
{
Warp *warp = new Warp();
warp->setSymbol(t+'0');
warp->setsrcRow(warpRow1[t]);
warp->setsrcCol(warpCol1[t]);
warp->setdestRow(warpRow2[t]);
warp->setdestCol(warpCol2[t]);
player->setWarps(warp);
}
}
myWorld.setGrid(&myGrid); // setting the grid on the world
// intitializing the status of the game
myWorld.setTime(time);
myWorld.setMoves(0);
myWorld.setPlayer(player);
player->setBombCollected(0);
player->setMaxBomb(3);
player->setHostRemain(host);
player->setHostRescued(0);
}
}
bool Level::Load(const std::string& filename) {
Tmx::Map map;
map.ParseFile(filename);
if(map.HasError()) {
Debug::logger->message("Error while loading level %s: %s\n", filename.c_str(), map.GetErrorText().c_str());
return false;
}
_width = map.GetWidth();
_height = map.GetHeight();
_tileWidth = map.GetTileWidth();
_tileHeight = map.GetTileHeight();
std::map<const Tmx::Tileset*, Tileset*> tilesetMap;
for(int i = 0; i < map.GetNumTilesets(); i++) {
const Tmx::Tileset* tmxTileset = map.GetTileset(i);
Tileset* tileset = new Tileset(_tileWidth, _tileHeight);
tileset->LoadImage(map.GetFilepath() + tmxTileset->GetImage()->GetSource());
_tilesets.push_back(tileset);
tilesetMap.insert(std::pair<const Tmx::Tileset*, Tileset*>(tmxTileset, tileset));
}
_collisions = new bool[_width * _height];
for(int i = 0; i < (_width * _height); i++) {
_collisions[i] = false;
}
for(int i = 0; i < map.GetNumLayers(); i++) {
const Tmx::Layer* tmxLayer = map.GetLayer(i);
if(!strcasecmp(tmxLayer->GetName().c_str(), "collision")) {
for(int x = 0; x < _width; x++) {
for(int y = 0; y < _height; y++) {
Tmx::MapTile tile = tmxLayer->GetTile(x, y);
_collisions[y * _width + x] = tile.tilesetId > -1;
}
}
continue;
}
else if(!strcasecmp(tmxLayer->GetName().c_str(), "middle")) {
_middleLayer = i;
}
Layer* layer = new Layer(
tmxLayer->GetWidth(), tmxLayer->GetHeight(),
_tileWidth, _tileHeight);
for(int x = 0; x < layer->GetWidth(); x++) {
for(int y = 0; y < layer->GetHeight(); y++) {
Tmx::MapTile tmxTile = tmxLayer->GetTile(x, y);
MapTile tile;
if(tmxTile.tilesetId != -1) {
const Tmx::Tileset* tmxTileset = map.GetTileset(tmxTile.tilesetId);
tile.id = tmxTile.id;
tile.tileset = tilesetMap.find(tmxTileset)->second;
} else {
tile.id = 0;
tile.tileset = NULL;
}
layer->SetTile(x, y, tile);
}
}
_layers.push_back(layer);
}
if(_middleLayer == -1) {
_middleLayer = int(floor(float(_layers.size()) / 2.0f)); // <-- nasty
}
for(int i = 0; i < map.GetNumObjectGroups(); i++) {
const Tmx::ObjectGroup* tmxGroup = map.GetObjectGroup(i);
for(int j = 0; j < tmxGroup->GetNumObjects(); j++) {
const Tmx::Object* tmxObject = tmxGroup->GetObject(j);
if(!strncasecmp(tmxObject->GetName().c_str(), "NPC", 3)) {
NPC* npc = new NPC(this);
npc->LoadSprites(tmxObject->GetProperties().GetLiteralProperty("image").c_str());
npc->SetXY(tmxObject->GetX(), tmxObject->GetY());
_npcs.push_back(npc);
}
else if(!strncasecmp(tmxObject->GetName().c_str(), "Warp", 4)) {
Warp* warp = new Warp();
warp->SetXY(tmxObject->GetX(), tmxObject->GetY());
warp->SetWidthHeight(tmxObject->GetWidth(), tmxObject->GetHeight());
warp->SetTargetMap(tmxObject->GetProperties().GetLiteralProperty("map").c_str());
warp->SetTargetX(tmxObject->GetProperties().GetNumericProperty("x") * 32);
warp->SetTargetY(tmxObject->GetProperties().GetNumericProperty("y") * 32);
_warps.push_back(warp);
}
}
}
std::map<std::string, std::string> mapProps = map.GetProperties().GetList();
for(std::map<std::string, std::string>::iterator i = mapProps.begin(); i != mapProps.end(); ++i) {
if(i->first == "BGM") {
_bgm = musicManager.Load(i->second);
}
}
return true;
}
