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