void NaiveMotionPredict::objectsCallback(const autoware_msgs::DetectedObjectArray& input) { autoware_msgs::DetectedObjectArray output; visualization_msgs::MarkerArray predicted_lines; output.header = input.header; for (const auto &object : input.objects) { std::vector<autoware_msgs::DetectedObject> predicted_objects_vec; visualization_msgs::Marker predicted_line; makePrediction(object, predicted_objects_vec, predicted_line); // concate to output object array output.objects.insert(output.objects.end(), predicted_objects_vec.begin(), predicted_objects_vec.end()); // visualize only stably tracked objects if (!object.pose_reliable) { continue; } predicted_lines.markers.push_back(predicted_line); } for (auto &object : output.objects) { object.valid = true; } predicted_objects_pub_.publish(output); predicted_paths_pub_.publish(predicted_lines); }
//prediction for external data Eigen::VectorXd Tree::predict(DataFrame &testSet, const bool verbose) { //starting at root level, then call makePredictions recursively double pi = 0.0; Eigen::VectorXd p(testSet.nrrows); for (int obs = 0; obs < testSet.nrrows; obs++) { //in some rare case we do have a terminal node at the 0th level if (root->isTerminal) { pi = root->cm; } else if (testSet.matrix(obs, root->feature) < root->splitvalue) { //makePrediction leftDF pi = makePrediction(testSet, root->left, obs, verbose); } else { //makePrediction rightDF pi = makePrediction(testSet, root->right, obs, verbose); } if (!probability && !regression) { //cout<<"Rounding "<<pi<<" to "<<LUtils::round(pi)<<endl; pi = LUtils::round(pi); } p(obs) = pi; if (verbose) { cout << "p(" << obs << "): " << p(obs) << " order:" << testSet.order[obs] <<endl; } } //original order for p Eigen::VectorXd tmp(testSet.nrrows); int idx_orig = 0; for (int i = 0; i < testSet.nrrows; i++) { idx_orig = testSet.order.at(i); tmp(idx_orig) = p(i); } for (int i = 0; i < testSet.nrrows; i++) { p(i) = tmp(i); } //after prediction we have to re-establish the original order testSet.restoreOrder(); if (verbose) cout << "###Tree size:" << tree_size + 1 << " nodes." << endl; return p; }
//make prediction for a certain row, recursive until terminal node is reached double Tree::makePrediction(const DataFrame &testset, const boost::shared_ptr<Node> localNode, const int obs, const bool verbose) { double t = 0.0; if (localNode->isTerminal) { if (verbose) { cout << " cm(Node" << localNode->nodeID << "):"; } return localNode->cm; } if (testset.matrix(obs, localNode->feature) < localNode->splitvalue) { //makePrediction leftDF t = makePrediction(testset, localNode->left, obs, verbose); } else { //makePrediction rightDF t = makePrediction(testset, localNode->right, obs, verbose); } //if we reach this end we go upwards again... return t; }
/*=============================================================== routine : main return val : int input par : int argc char *argv[] function : main function ===============================================================*/ int main(int argc, char *argv[]) { double psnr; int headframe, tailframe, skipframe; char *srcname, c, *dd, filename[255]; register int i, j; clock_t start, end; fprintf(stderr, "\nstarting program...\n"); system("date"); start = clock(); /* パラメータの初期化 */ srcname = SRC_NAME; headframe = HEAD_FRAME; tailframe = TAIL_FRAME; skipframe = SKIP_FRAME; /* パラメータの設定 */ if (argc < 1) { fprintf(stderr, "usage : %s [option]\n", argv[0]); fprintf(stderr, " -s <string> source image filename\n"); fprintf(stderr, " -h help\n"); exit(1); } while (--argc > 0) { c = (*++argv)[0]; if (!(c == '-')) fprintf(stderr, "\nunknown option...\n"); else { c = (*argv)[1]; dd = &(*argv)[1]; switch (c) { case 'h': fprintf(stderr, "\nusage : command [option]\n"); fprintf(stderr, " -s <string> source image filename\n"); fprintf(stderr, " -h help\n"); exit(1); break; case 's': if (--argc > 0) { c = (*++argv)[0]; if (c == '-') fprintf(stderr, "\noption error...\n"); else srcname = (*argv); } else { fprintf(stderr, "\noption error \"%s\"...\n", (*argv)); exit(1); } break; default: fprintf(stderr, "\nunknown option...\n"); break; } } } fprintf(stderr, "image size = %dx%d pixel\n", SRC_X_SIZE, SRC_Y_SIZE); fprintf(stderr, "macro block size = %dx%d pixel\n", MB_SIZE, MB_SIZE); fprintf(stderr, "search window x = [-%d:+%d], y = [-%d:+%d]\n", SW_SIZE, SW_SIZE, SW_SIZE, SW_SIZE); fprintf(stderr, "head frame number = %d, tail frame number = %d (skip:%d)\n", headframe, tailframe, skipframe); /* 先頭フレーム名の作成 */ makeFileName(filename, srcname, headframe, ".cif"); fprintf(stderr, "\n[frame no.%d]\n", headframe); fprintf(stderr, "now loading frame data from \"%s\"...\n", filename); /* 先頭フレームの読み込み */ loadRawFloat(filename, premap); for (j = headframe + skipframe; j <= tailframe; j += skipframe) { /* 動きベクトルの初期化 */ for (i = 0; i < MB_X_NUM*MB_Y_NUM; i++) vecx[i] = vecy[i] = 0.0; /* フレーム番号情報の表示 */ fprintf(stderr, "\n[frame no.%d]-[frame no.%d]\n", j - skipframe, j); /* 次フレーム名の作成 */ makeFileName(filename, srcname, j, ".cif"); fprintf(stderr, "now loading frame data from \"%s\"...\n", filename); /* 次フレームの読み込み */ loadRawFloat(filename, crtmap); /* Three Step Search */ greedySearch(premap, crtmap, vecy, vecx); /* 予測画像の作成 */ makePrediction(premap, mcmap, vecy, vecx); /* PSNR の算出 */ psnr = getPsnrFloat(crtmap, mcmap); fprintf(stderr, "psnr = %f [dB]\n", psnr); /* フレームの複写 */ for (i = 0; i < SRC_Y_SIZE*SRC_X_SIZE; i++) premap[i] = crtmap[i]; } fprintf(stderr, "\ndone!!\n"); system("date"); end = clock(); fprintf(stderr, "TIME=%d[ms]\n",end-start); return 0; }
void CEconomyTracker::frameUpdate(int frame) { if (trackerOff) { return; } /* // iterate over all the BuildTasks for (UnitCategory category = 0; category < CAT_LAST; category++) { for (std::list<BuildTask>::iterator i = ai->uh->BuildTasks[category]->begin(); i != ai->uh->BuildTasks[category]->end(); i++) { BuildTask bt = *i; updateUnitUnderConstruction(&bt); } } */ for (int category = 0; category < CAT_LAST; category++) { for (std::list<BuildingTracker>::iterator i = allTheBuildingTrackers[category].begin(); i != allTheBuildingTrackers[category].end(); i++) { BuildingTracker* bt = &(*i); updateUnitUnderConstruction(bt); } } constructionEnergySum += constructionEnergy; constructionMetalSum += constructionMetal; // move the new EconomyUnitTrackers std::list<EconomyUnitTracker*> removeList; for (std::list<EconomyUnitTracker*>::iterator i = newEconomyUnitTrackers.begin(); i != newEconomyUnitTrackers.end(); i++) { EconomyUnitTracker* bt = *i; assert(frame - bt->createFrame <= 16); if (frame - bt->createFrame == 16) { // move it to the active list assert(bt->alive); activeEconomyUnitTrackers.push_back(bt); removeList.push_back(bt); } } // remove them from newEconomyUnitTrackers for (std::list<EconomyUnitTracker*>::iterator i = removeList.begin(); i != removeList.end(); i++) { newEconomyUnitTrackers.remove(*i); } // update the units in activeEconomyUnitTrackers, add their production/usage to total float energyProduction = 0.0f; float metalProduction = 0.0f; // these are exclusive of what is used by builders float energyUsage = 0.0f; float metalUsage = 0.0f; if (frame % 16 == 0) { for (std::list<EconomyUnitTracker*>::iterator i = activeEconomyUnitTrackers.begin(); i != activeEconomyUnitTrackers.end(); i++) { EconomyUnitTracker* bt = *i; assert(bt->alive); // if the unit is a builder then we track its resource usage already // if it's using more than its weapon fire or upkeep UnitResourceInfo resourceInfo; bool isAlive = ai->cb->GetUnitResourceInfo(bt->economyUnitId, &resourceInfo); assert(isAlive); // add the change from last update bt->totalEnergyMake += bt->lastUpdateEnergyMake = resourceInfo.energyMake - bt->lastUpdateEnergyMake; bt->totalMetalMake += bt->lastUpdateMetalMake = resourceInfo.metalMake - bt->lastUpdateMetalMake; bt->totalEnergyUsage += bt->lastUpdateEnergyUsage = resourceInfo.energyUse - bt->lastUpdateEnergyUsage; bt->totalMetalUsage += bt->lastUpdateMetalUsage = resourceInfo.metalUse - bt->lastUpdateMetalUsage; energyProduction += bt->lastUpdateEnergyMake; metalProduction += bt->lastUpdateMetalMake; if (!bt->unitDef->builder) { energyUsage += bt->lastUpdateEnergyUsage; metalUsage += bt->lastUpdateMetalUsage; } } } float energy = ai->cb->GetEnergy(); float metal = ai->cb->GetMetal(); // float deltaEnergy = energy - oldEnergy + constructionEnergy; // float deltaMetal = metal - oldMetal + constructionMetal; if (frame % 16 == 0) { makePrediction(frame + 320 + 160); } oldEnergy = energy; oldMetal = metal; constructionEnergy = 0; constructionMetal = 0; }
void CEconomyTracker::frameUpdate() { if(trackerOff) return; int frame=ai->cb->GetCurrentFrame(); //L(frame); // Iterate over all the BuildTasks: /* for(int category = 0; category < LASTCATEGORY; category++ ) { for(list<BuildTask>::iterator i = ai->uh->BuildTasks[category]->begin(); i != ai->uh->BuildTasks[category]->end(); i++){ BuildTask bt = *i; updateUnitUnderConstruction(&bt); } }*/ for(int category = 0; category < LASTCATEGORY; category++ ) { for(list<BuildingTracker>::iterator i = allTheBuildingTrackers[category]->begin(); i != allTheBuildingTrackers[category]->end(); i++) { BuildingTracker *bt = &*i; // This is bad, but needed updateUnitUnderConstruction(bt); } } constructionEnergySum += constructionEnergy; constructionMetalSum += constructionMetal; // Iterate over all the factories: // bool IsUnitActivated (int unitid); // Move the new EconomyUnitTrackers list<EconomyUnitTracker*> removeList; for(list<EconomyUnitTracker*>::iterator i = newEconomyUnitTrackers.begin(); i != newEconomyUnitTrackers.end(); i++) { EconomyUnitTracker *bt = *i; // This is bad, but needed assert(frame - bt->createFrame <= 16); if(frame - bt->createFrame == 16) { // Move it to the active list assert(bt->alive); activeEconomyUnitTrackers.push_back(bt); removeList.push_back(bt); //L("Moved " << bt->unitDef->humanName << " to activeEconomyUnitTrackers"); } } // Remove them from newEconomyUnitTrackers: for(list<EconomyUnitTracker*>::iterator i = removeList.begin(); i != removeList.end(); i++) { newEconomyUnitTrackers.remove(*i); } // Update the units in activeEconomyUnitTrackers, add their production/usage to total float energyProduction = 0; float metalProduction = 0; float energyUsage = 0; // This is without what is used by builders float metalUsage = 0; // This is without what is used by builders if(frame % 16 == 0) { // Update for(list<EconomyUnitTracker*>::iterator i = activeEconomyUnitTrackers.begin(); i != activeEconomyUnitTrackers.end(); i++) { EconomyUnitTracker *bt = *i; // This is bad, but needed assert(bt->alive); ////L("Adding " << bt->unitDef->humanName << " UnitResourceInfo...."); // Hmmm // If the unit is a builder then we track its resource usage already // if its useing more then its weapon fire or upkeep UnitResourceInfo resourceInfo; bool isAlive = ai->cb->GetUnitResourceInfo(bt->economyUnitId, &resourceInfo); assert(isAlive); // Add the change from last update bt->totalEnergyMake += bt->lastUpdateEnergyMake = resourceInfo.energyMake - bt->lastUpdateEnergyMake; bt->totalMetalMake += bt->lastUpdateMetalMake = resourceInfo.metalMake - bt->lastUpdateMetalMake; bt->totalEnergyUsage += bt->lastUpdateEnergyUsage = resourceInfo.energyUse - bt->lastUpdateEnergyUsage; bt->totalMetalUsage += bt->lastUpdateMetalUsage = resourceInfo.metalUse - bt->lastUpdateMetalUsage; energyProduction += bt->lastUpdateEnergyMake; metalProduction += bt->lastUpdateMetalMake; if(!bt->unitDef->builder) { // Hmmmm energyUsage += bt->lastUpdateEnergyUsage; metalUsage += bt->lastUpdateMetalUsage; } } } float energy = ai->cb->GetEnergy(); float metal = ai->cb->GetMetal(); float deltaEnergy = energy - oldEnergy + constructionEnergy; float deltaMetal = metal - oldMetal + constructionMetal; myCalcEnergy -= constructionEnergy; myCalcMetal -= constructionMetal; if(frame % 16 == 0) { // Add the activeEconomyUnitTrackers change now myCalcEnergy += energyProduction - energyUsage; myCalcMetal += metalProduction - metalUsage; if(myCalcEnergy > ai->cb->GetEnergyStorage()) myCalcEnergy = ai->cb->GetEnergyStorage(); if(myCalcMetal > ai->cb->GetMetalStorage()) myCalcMetal = ai->cb->GetMetalStorage(); //L("energy: " << ai->cb->GetEnergy() << ", metal: " << ai->cb->GetMetal()); //L("myCalcEnergy: " << myCalcEnergy << ", myCalcMetal: " << myCalcMetal); //L("energyProduction: " << energyProduction << ", energyUsage: " << energyUsage); //L("metalProduction: " << metalProduction << ", metalUsage: " << metalUsage); makePrediction(frame + 320 + 160); } //if(frame % 30 == 0) // //L("energy: " << ai->cb->GetEnergy() << ", metal: " << ai->cb->GetMetal()); ////L("myCalcEnergy: " << myCalcEnergy << ", myCalcMetal: " << myCalcMetal); if(fabs(deltaEnergy) > 0.001 || fabs(deltaMetal) > 0.001) //L("deltaEnergy: " << deltaEnergy << ", deltaMetal: " << deltaMetal); if(frame % 32 == 0) //L("constructionEnergy: " << constructionEnergy << ", constructionMetal: " << constructionMetal); if(frame % 32 == 0) { //L("constructionEnergySum: " << constructionEnergySum << ", constructionMetalSum: " << constructionMetalSum); } oldEnergy = energy; oldMetal = metal; constructionEnergy = 0; constructionMetal = 0; //L(frame << ", end"); //L(""); }