DLLEXPORT
int foo()
{
#ifdef TARGET_LINUX
if (!pBar) throw(0);
#endif
// May cause exception due to NULL pointer
return pBar();
}
DLLEXPORT
int FASTCALL foo(int a0, int a1, int a2, int a3, int a4, int a5, int a6)
{
#ifdef TARGET_LINUX
if (!pBar) throw(0);
#endif
// May cause exception due to NULL pointer
return pBar(a0, a1, a2, a3, a4, a5, a6);
}
void CRForest::loadForest() {
char buffer[256];
char buffer2[256];
std::cout << "loading forest..." << std::endl;
for(int i = 0; i < vTrees.size(); ++i) {
sprintf(buffer, "%s%03d.txt",conf.treepath.c_str(),i);
sprintf(buffer2, "%s%s%03d.txt", conf.treepath.c_str(), conf.classDatabaseName.c_str(), i);
vTrees[i] = new CRTree(buffer, buffer2, conf);
//std::cout << buffer2 << std::endl;
classDatabase.read(buffer2);
pBar(i,vTrees.size(),50);
}
std::cout << std::endl;
}
//------------------------------------------------------------------------------
void ExtendedKalmanInv::TimeUpdate()
{
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("Performing time update\n");
#endif
Rmatrix cov(stateSize, stateSize), stmt(stateSize, stateSize);
stmt = stm->Transpose();
// P phi^T:
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
{
cov(i,j) = 0.0;
for (UnsignedInt k = 0; k < stateSize; ++k)
{
cov(i,j) += (*stateCovariance)(i,k) * stmt(k,j);
}
}
}
// phi * (P phi^T):
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
{
pBar(i,j) = 0.0;
for (UnsignedInt k = 0; k < stateSize; ++k)
pBar(i,j) += (*stm)(i,k) * cov(k,j);
}
}
// make it symmetric!
Symmetrize(pBar);
}
//------------------------------------------------------------------------------
void ExtendedKalmanInv::UpdateCovarianceSimple()
{
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("Updating covariance using simple "
"method\n");
#endif
// Update the covariance
Real khSum;
Rmatrix temp(stateSize, stateSize);
// First calc (I - K H)
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
{
if (i == j)
temp(i,j) = 1.0;
else
temp(i,j) = 0.0;
khSum = 0.0;
for (UnsignedInt k = 0; k < measSize; ++k)
khSum += kalman(i,k) * hTilde[k][j];
temp(i,j) -= khSum;
}
}
// Now build (I - K H) (\bar P) (I - K H)
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
{
(*stateCovariance)(i,j) = 0.0;
for (UnsignedInt k = 0; k < stateSize; ++k)
(*stateCovariance)(i,j) += temp(i,k) * pBar(k,j);
}
}
}
void foo1()
{
if (!pBar) throw(0);
// May cause exception due to NULL pointer
pBar();
}
void CRForest::growATree(const int treeNum) {
// positive, negative dataset
std::vector<CPosDataset> posSet(0);
std::vector<CNegDataset> negSet(0);
// positive, negative patch
std::vector<CPosPatch> posPatch(0);
std::vector<CNegPatch> negPatch(0);
char buffer[256];
std::cout << "tree number " << treeNum << std::endl;
// initialize random seed
boost::mt19937 gen( treeNum * static_cast<unsigned long>(time(NULL)) );
//boost::timer t;
//loadTrainPosFile(conf, posSet);//, gen);
loadTrainObjFile(conf,posSet);
CClassDatabase tempClassDatabase;
// extract pos features and register classDatabase
for(int i = 0; i < posSet.size(); ++i) {
//std::cout << i << std::endl;
//std::cout << posSet.at(i).rgb << std::endl;
// if(posSet.at(i).loadImage(conf) == -1 && conf.learningMode != 2){
// exit(-1);
// }
//posSet.at(i).extractFeatures(conf);
//std::cout << posSet.size() << std::endl;
tempClassDatabase.add(posSet.at(i).getParam()->getClassName(),cv::Size(),0);
}
// std::vector<CPosDataset> tempPosSet(0);
// int currentClass = treeNum % tempClassDatabase.vNode.size();
//std::cout << "okashiina" << std::endl;
// for(int i = 0; i < posSet.size(); ++i){
// if(tempClassDatabase.search(posSet.at(i).getClassName()) == currentClass){
// tempPosSet.push_back(posSet.at(i));
// //std::cout << "teketeke" << std::endl;
// }else{
// negSet.push_back(convertPosToNeg2(posSet.at(i)));
// //std::cout << "negneg" << std::endl;
// }
// }
//posSet = tempPosSet;
loadTrainNegFile(conf, negSet);
std::cout << "dataset loaded" << std::endl;
// initialize class database
//classDatabase.clear();
std::cout << "generating appearance from 3D model!" << std::endl;
// extract pos features and register classDatabase
for(int i = 0; i < posSet.size(); ++i) {
//std::cout << i << std::endl;
//std::cout << posSet.at(i).rgb << std::endl;
//#pragma omp critical
posSet.at(i).loadImage(conf, posSet.at(i).getModelPath(), posSet.at(i).getParam());
// if(imgload == -1 && conf.learningMode != 2){
// std::cout << "can't load image files" << std::endl;
// exit(-1);
// }
posSet.at(i).extractFeatures(conf);
//std::cout << posSet.at(i).img.at(1)->type() << std::endl;
//std::cout << "detayo" << std::endl;
//std::cout << posSet.size() << std::endl;
classDatabase.add(posSet.at(i).getParam()->getClassName(),posSet.at(i).img.at(0)->size(),0);
pBar(i,posSet.size(),50);
}
std::cout << std::endl;
classDatabase.show();
// extract neg features
for(int i = 0; i < negSet.size(); ++i) {
// if(negSet.at(i).getModel() != NULL)
// negSet.at(i).loadImage(conf, negSet.at(i).getModelPath(), posSet.at(i))
negSet.at(i).loadImage(conf);
negSet.at(i).extractFeatures(conf);
}
CRTree *tree = new CRTree(conf.min_sample, conf.max_depth, classDatabase.vNode.size(),this->classDatabase);
std::cout << "tree created" << std::endl;
extractPosPatches(posSet,posPatch,conf,treeNum,this->classDatabase);
extractNegPatches(negSet,negPatch,conf);
std::cout << "extracted pathes" << std::endl;
std::vector<int> patchClassNum(classDatabase.vNode.size(), 0);
for(int j = 0; j < posPatch.size(); ++j)
patchClassNum.at(classDatabase.search(posPatch.at(j).getClassName()))++;
// grow tree
//vTrees.at(treeNum)->growTree(vPatches, 0,0, (float)(vPatches.at(0).size()) / ((float)(vPatches.at(0).size()) + (float)(vPatches.at(1).size())), conf, gen, patchClassNum);
//#pragma omp parallel
tree->growTree(posPatch,negPatch, 0,0, ((float)posPatch.size() / (float)(posPatch.size() + negPatch.size())), conf, patchClassNum);
// cv::namedWindow("test");
// cv::imshow("test", *posSet.at(0).feature.at(3));
// cv::waitKey(0);
// cv::destroyAllWindows();
// save tree
sprintf(buffer, "%s%03d.txt",
conf.treepath.c_str(), treeNum + conf.off_tree);
std::cout << "tree file name is " << buffer << std::endl;
tree->saveTree(buffer);
// save class database
sprintf(buffer, "%s%s%03d.txt",
conf.treepath.c_str(),
conf.classDatabaseName.c_str(), treeNum + conf.off_tree);
std::cout << "write tree data" << std::endl;
classDatabase.write(buffer);
//double time = t.elapsed();
//std::cout << "tree " << treeNum << " calicuration time is " << time << std::endl;
sprintf(buffer, "%s%03d_timeResult.txt",conf.treepath.c_str(), treeNum + conf.off_tree);
std::fstream lerningResult(buffer, std::ios::out);
if(lerningResult.fail()) {
std::cout << "can't write result" << std::endl;
}
lerningResult << time << std::endl;
lerningResult.close();
delete tree;
posPatch.clear();
negPatch.clear();
// for(int i = 0; i< posSet.size(); ++i){
// cv::namedWindow("test");
// cv::imshow("test", *posSet.at(i).img.at(0));
// cv::waitKey(0);
// cv::destroyWindow("test");
// }
// for(int i = 0; i < posSet.size(); ++i){
// posSet.at(i).releaseImage();
// }
// for(int i = 0; i< posSet.size(); ++i){
// cv::namedWindow("test");
// cv::imshow("test", *posSet.at(i).feature.at(0));
// cv::waitKey(0);
// cv::destroyWindow("test");
// }
// for(int i = 0; i < posSet.size(); ++i){
// posSet.at(i).releaseFeatures();
// }
posSet.clear();
negSet.clear();
}
int __fastcall foo(int a0, int a1, int a2, int a3, int a4, int a5, int a6)
{
// May cause exception due to NULL pointer
return pBar(a0, a1, a2, a3, a4, a5, a6);
}
//------------------------------------------------------------------------------
void ExtendedKalmanInv::UpdateCovarianceJoseph()
{
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("Updating covariance using Joseph "
"method\n");
#endif
// Update the covariance
Real khSum;
Rmatrix temp(stateSize, stateSize), krk(stateSize, stateSize);
Rmatrix pikh(stateSize, stateSize), r(measSize, measSize);
Rmatrix ikh(stateSize, stateSize), rk(measSize, stateSize);
// First calc (I - K H)
#ifdef DEBUG_JOSEPH
MessageInterface::ShowMessage("Calcing I - K H\n K:\n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < measSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", kalman(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n H:\n");
for (UnsignedInt i = 0; i < measSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", hTilde[i][j]);
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
#endif
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
{
if (i == j)
ikh(i,j) = 1.0;
else
ikh(i,j) = 0.0;
khSum = 0.0;
for (UnsignedInt k = 0; k < measSize; ++k)
khSum += kalman(i,k) * hTilde[k][j];
ikh(i,j) -= khSum;
}
}
#ifdef DEBUG_JOSEPH
MessageInterface::ShowMessage(" I - K H:\n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", ikh(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n pBar:\n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", pBar(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n (I - K H) \bar P (I - K H)^T:\n");
#endif
// Build (I - K H) \bar P (I - K H)^T
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
{
pikh(i,j) = 0.0;
for (UnsignedInt k = 0; k < stateSize; ++k)
pikh(i,j) += pBar(i,k) * ikh(j,k);
}
}
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
{
temp(i,j) = 0.0;
for (UnsignedInt k = 0; k < stateSize; ++k)
temp(i,j) += ikh(i,k) * pikh(k,j);
}
}
#ifdef DEBUG_JOSEPH
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", temp(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
#endif
#ifdef DEBUG_JOSEPH
MessageInterface::ShowMessage("Prepping to fake up R\n R:\n");
#endif
// Fake up the measurement covariance if needed
if (measCovariance)
r = *(measCovariance->GetCovariance());
else
for (UnsignedInt i = 0; i < measSize; ++i)
for (UnsignedInt j = 0; j < measSize; ++j)
if (i == j)
r(i,j) = DEFAULT_MEASUREMENT_COVARIANCE;
else
r(i,j) = 0.0;
// Now K R K^T
#ifdef DEBUG_JOSEPH
for (UnsignedInt i = 0; i < measSize; ++i)
{
for (UnsignedInt j = 0; j < measSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", r(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
MessageInterface::ShowMessage("Calcing RK:\n");
#endif
for (UnsignedInt i = 0; i < measSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
{
rk(i,j) = 0.0;
for (UnsignedInt k = 0; k < measSize; ++k)
rk(i,j) += r(i,k) * kalman(j,k);
}
}
#ifdef DEBUG_JOSEPH
for (UnsignedInt i = 0; i < measSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", rk(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
MessageInterface::ShowMessage("Calcing KRK:\n");
#endif
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
{
krk(i,j) = 0.0;
for (UnsignedInt k = 0; k < measSize; ++k)
krk(i,j) += kalman(i,k) * rk(k,j);
}
}
#ifdef DEBUG_JOSEPH
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", krk(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
#endif
// Add 'em up
#ifdef DEBUG_JOSEPH
MessageInterface::ShowMessage("Summing covariance\n");
#endif
for (UnsignedInt i = 0; i < stateSize; ++i)
for (UnsignedInt j = 0; j < stateSize; ++j)
(*stateCovariance)(i,j) = temp(i,j) + krk(i,j);
#ifdef DEBUG_JOSEPH
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf ", (*covariance)(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
throw EstimatorException("Intentional debug break!");
#endif
}
//------------------------------------------------------------------------------
void ExtendedKalmanInv::ComputeGain()
{
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("Computing Kalman Gain\n");
#endif
// Finally, the Kalman gain
kalman.SetSize(stateSize, measSize);
#ifdef DEBUG_ESTIMATION_DETAILS
MessageInterface::ShowMessage("Calculating P H^T\n");
#endif
Rmatrix ph(stateSize, measSize), hph(measSize, measSize);
// P H^T
for (UnsignedInt i = 0; i < stateSize; ++i)
for (UnsignedInt j = 0; j < measSize; ++j)
{
ph(i,j) = 0.0;
for (UnsignedInt k = 0; k < stateSize; ++k)
ph(i,j) += pBar(i,k) * hTilde[j][k];
}
// H P H^T
#ifdef DEBUG_ESTIMATION_DETAILS
MessageInterface::ShowMessage("Calculating H P H^T\n");
#endif
for (UnsignedInt i = 0; i < measSize; ++i)
for (UnsignedInt j = 0; j < measSize; ++j)
{
hph(i,j) = 0.0;
for (UnsignedInt k = 0; k < stateSize; ++k)
hph(i,j) += hTilde[i][k] * ph(k,j);
}
// H P H^T + R; hard coded for now, since the obs covariances aren't available
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("Adding R -- Constant 0.1 diag for now\n");
#endif
for (UnsignedInt i = 0; i < measSize; ++i)
{
for (UnsignedInt j = 0; j < measSize; ++j)
{
if (measCovariance == NULL)
{
if (i == j)
{
#ifdef DEBUG_WEIGHTS
MessageInterface::ShowMessage("Measurement covariance is "
"%le\n", (*(measCovariance))(i,j));
#endif
hph(i,j) += DEFAULT_MEASUREMENT_COVARIANCE;
}
}
else
{
#ifdef DEBUG_WEIGHTS
MessageInterface::ShowMessage("Measurement covariance (%d, %d) "
"is %le\n", i, j, (*(measCovariance))(i,j));
#endif
hph(i,j) += (*measCovariance)(i,j);
}
// Save the std deviation data
sigma.push_back(sqrt(hph(i,j)));
}
}
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("Gain denominator = \n");
for (UnsignedInt i = 0; i < measSize; ++i)
{
for (UnsignedInt j = 0; j < measSize; ++j)
{
MessageInterface::ShowMessage(" %.12le", hph(i,j));
}
MessageInterface::ShowMessage("\n");
}
#endif
// Finally, Kalman = P H^T (H P H^T + R)^{-1}
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("Calculating the Kalman gain\n");
#endif
Rmatrix inv = hph.Inverse();
for (UnsignedInt i = 0; i < stateSize; ++i)
for (UnsignedInt j = 0; j < measSize; ++j)
{
kalman(i,j) = 0.0;
for (UnsignedInt k = 0; k < measSize; ++k)
kalman(i,j) += ph(i,k) * inv(k,j);
}
}
//------------------------------------------------------------------------------
void ExtendedKalmanInv::Estimate()
{
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("\n\n---------------------\n");
MessageInterface::ShowMessage("Current covariance:\n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf", (*covariance)(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
MessageInterface::ShowMessage("Current stm:\n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf", (*stm)(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
MessageInterface::ShowMessage("Current State: [ ");
for (UnsignedInt i = 0; i < stateSize; ++i)
MessageInterface::ShowMessage(" %.12lf ", (*estimationState)[i]);
MessageInterface::ShowMessage("\n");
#endif
// Perform the time update of the covariances, phi P phi^T, and the state
TimeUpdate();
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("Time updated matrix \\bar P:\n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf", pBar(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
#endif
// Construct the O-C data and H tilde
ComputeObs();
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("hTilde:\n");
for (UnsignedInt i = 0; i < measSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf", hTilde[i][j]);
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
#endif
// Then the Kalman gain
ComputeGain();
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("The Kalman gain is: \n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < measSize; ++j)
MessageInterface::ShowMessage(" %.12lf", kalman(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
#endif
// Finally, update everything
UpdateElements();
// Plot residuals if set
if (showAllResiduals)
PlotResiduals();
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("Updated covariance:\n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf", (*covariance)(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
MessageInterface::ShowMessage("Updated State: [ ");
for (UnsignedInt i = 0; i < stateSize; ++i)
MessageInterface::ShowMessage(" %.12lf ", (*estimationState)[i]);
MessageInterface::ShowMessage("\n\n---------------------\n");
#endif
// ReportProgress();
// Advance MeasMan to the next measurement and get its epoch
measManager.AdvanceObservation();
nextMeasurementEpoch = measManager.GetEpoch();
FindTimeStep();
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("CurrentEpoch = %.12lf, next "
"epoch = %.12lf, timeStep = %.12lf\n", currentEpoch,
nextMeasurementEpoch, timeStep);
#endif
if (currentEpoch < nextMeasurementEpoch)
{
// Reset the STM
for (UnsignedInt i = 0; i < stateSize; ++i)
for (UnsignedInt j = 0; j < stateSize; ++j)
if (i == j)
(*stm)(i,j) = 1.0;
else
(*stm)(i,j) = 0.0;
esm.MapSTMToObjects();
esm.MapVectorToObjects();
PropagationStateManager *psm = propagator->GetPropStateManager();
psm->MapObjectsToVector();
// Flag that a new current state has been loaded in the objects
resetState = true;
currentState = PROPAGATING;
}
else
currentState = CHECKINGRUN; // Should this just go to FINISHED?
}
