void Cpoint3dCov::setDiagonal(const double sx2, const double sy2, const double sz2)
{
covMat = Matrix3f::Zero();
covMat(0,0)=sx2;
covMat(1,1)=sy2;
covMat(2,2)=sz2;
}
void Cpoint3dCov::setXYcov(const double sx2, const double sy2, const double sxy)
{
covMat = Matrix3f::Zero();
covMat(0,0)=sx2;
covMat(1,1)=sy2;
covMat(0,1)=sxy;
covMat(1,0)=sxy;
}
void ifaGroup::setLatentDistribution(double *_mean, double *_cov)
{
if (!mean) {
mean = (double *) R_alloc(itemDims, sizeof(double));
if (itemDims) memset(mean, 0, itemDims * sizeof(double));
} else {
mean = _mean;
}
if (!cov) {
cov = (double *) R_alloc(itemDims * itemDims, sizeof(double));
Eigen::Map< Eigen::MatrixXd > covMat(cov, itemDims, itemDims);
covMat.setIdentity();
} else {
cov = _cov;
}
}
void ifaGroup::setLatentDistribution(int dims, double *_mean, double *_cov)
{
maxAbilities = dims;
if (maxAbilities < 0) Rf_error("maxAbilities must be non-negative");
if (!mean) {
mean = (double *) R_alloc(maxAbilities, sizeof(double));
memset(mean, 0, maxAbilities * sizeof(double));
} else {
mean = _mean;
}
if (!cov) {
cov = (double *) R_alloc(maxAbilities * maxAbilities, sizeof(double));
Eigen::Map< Eigen::MatrixXd > covMat(cov, maxAbilities, maxAbilities);
covMat.setIdentity();
} else {
cov = _cov;
}
}
void ForegroundObjs::calculateDistrParam()
{
for (int i = 0; i < numObjects; i++)
{
double sumX, sumY, sumZ, sumXX, sumZZ, sumXZ;
sumX = sumY = sumZ = sumXX = sumZZ = sumXZ = 0.0;
int sumW = numFp[i];
for (int p = 0; p < sumW; p++)
{
float x = fp3D[i][p].X;
float y = fp3D[i][p].Y;
float z = fp3D[i][p].Z;
sumX += x;
sumY += y;
sumZ += z;
sumXX += x*x;
sumZZ += z*z;
sumXZ += x*z;
}
XnPoint3D mean;
mean.X = sumX/numFp[i];
mean.Y = sumY/numFp[i];
mean.Z = sumZ/numFp[i];
Mat covMat(2,2, CV_64F);
double covXZ = (sumXZ/sumW)-(mean.X*mean.Z);
covMat.at<double>(0,0) = (sumXX/sumW)-powf(mean.X,2);
covMat.at<double>(0,1) = covXZ;
covMat.at<double>(1,0) = covXZ;
covMat.at<double>(1,1) = (sumZZ/sumW)-powf(mean.Z,2);
peopleDistr[i].mean = mean;
peopleDistr[i].cov = covMat;
}
}
double Cpoint3dCov::getCovTrace() const
{
return covMat(0,0)+covMat(1,1)+covMat(2,2);
}
double Cpoint3dCov::getMatrixElement(const unsigned int ii, const unsigned int jj) const
{
return covMat(ii,jj);
}
void ifaGroup::import(SEXP Rlist)
{
SEXP argNames;
Rf_protect(argNames = Rf_getAttrib(Rlist, R_NamesSymbol));
if (Rf_length(Rlist) != Rf_length(argNames)) {
mxThrow("All list elements must be named");
}
std::vector<const char *> dataColNames;
paramRows = -1;
int pmatCols=-1;
int mips = 1;
int dataRows = 0;
SEXP Rmean=0, Rcov=0;
for (int ax=0; ax < Rf_length(Rlist); ++ax) {
const char *key = R_CHAR(STRING_ELT(argNames, ax));
SEXP slotValue = VECTOR_ELT(Rlist, ax);
if (strEQ(key, "spec")) {
importSpec(slotValue);
} else if (strEQ(key, "param")) {
if (!Rf_isReal(slotValue)) mxThrow("'param' must be a numeric matrix of item parameters");
param = REAL(slotValue);
getMatrixDims(slotValue, ¶mRows, &pmatCols);
SEXP dimnames;
Rf_protect(dimnames = Rf_getAttrib(slotValue, R_DimNamesSymbol));
if (!Rf_isNull(dimnames) && Rf_length(dimnames) == 2) {
SEXP names;
Rf_protect(names = VECTOR_ELT(dimnames, 0));
int nlen = Rf_length(names);
factorNames.resize(nlen);
for (int nx=0; nx < nlen; ++nx) {
factorNames[nx] = CHAR(STRING_ELT(names, nx));
}
Rf_protect(names = VECTOR_ELT(dimnames, 1));
nlen = Rf_length(names);
itemNames.resize(nlen);
for (int nx=0; nx < nlen; ++nx) {
itemNames[nx] = CHAR(STRING_ELT(names, nx));
}
}
} else if (strEQ(key, "mean")) {
Rmean = slotValue;
if (!Rf_isReal(slotValue)) mxThrow("'mean' must be a numeric vector or matrix");
mean = REAL(slotValue);
} else if (strEQ(key, "cov")) {
Rcov = slotValue;
if (!Rf_isReal(slotValue)) mxThrow("'cov' must be a numeric matrix");
cov = REAL(slotValue);
} else if (strEQ(key, "data")) {
Rdata = slotValue;
dataRows = Rf_length(VECTOR_ELT(Rdata, 0));
SEXP names;
Rf_protect(names = Rf_getAttrib(Rdata, R_NamesSymbol));
int nlen = Rf_length(names);
dataColNames.reserve(nlen);
for (int nx=0; nx < nlen; ++nx) {
dataColNames.push_back(CHAR(STRING_ELT(names, nx)));
}
Rf_protect(dataRowNames = Rf_getAttrib(Rdata, R_RowNamesSymbol));
} else if (strEQ(key, "weightColumn")) {
if (Rf_length(slotValue) != 1) {
mxThrow("You can only have one %s", key);
}
weightColumnName = CHAR(STRING_ELT(slotValue, 0));
} else if (strEQ(key, "freqColumn")) {
if (Rf_length(slotValue) != 1) {
mxThrow("You can only have one %s", key);
}
freqColumnName = CHAR(STRING_ELT(slotValue, 0));
} else if (strEQ(key, "qwidth")) {
qwidth = Rf_asReal(slotValue);
} else if (strEQ(key, "qpoints")) {
qpoints = Rf_asInteger(slotValue);
} else if (strEQ(key, "minItemsPerScore")) {
mips = Rf_asInteger(slotValue);
} else {
// ignore
}
}
learnMaxAbilities();
if (itemDims < (int) factorNames.size())
factorNames.resize(itemDims);
if (int(factorNames.size()) < itemDims) {
factorNames.reserve(itemDims);
const int SMALLBUF = 24;
char buf[SMALLBUF];
while (int(factorNames.size()) < itemDims) {
snprintf(buf, SMALLBUF, "s%d", int(factorNames.size()) + 1);
factorNames.push_back(CHAR(Rf_mkChar(buf)));
}
}
if (Rmean) {
if (Rf_isMatrix(Rmean)) {
int nrow, ncol;
getMatrixDims(Rmean, &nrow, &ncol);
if (!(nrow * ncol == itemDims && (nrow==1 || ncol==1))) {
mxThrow("mean must be a column or row matrix of length %d", itemDims);
}
} else {
if (Rf_length(Rmean) != itemDims) {
mxThrow("mean must be a vector of length %d", itemDims);
}
}
verifyFactorNames(Rmean, "mean");
}
if (Rcov) {
if (Rf_isMatrix(Rcov)) {
int nrow, ncol;
getMatrixDims(Rcov, &nrow, &ncol);
if (nrow != itemDims || ncol != itemDims) {
mxThrow("cov must be %dx%d matrix", itemDims, itemDims);
}
} else {
if (Rf_length(Rcov) != 1) {
mxThrow("cov must be %dx%d matrix", itemDims, itemDims);
}
}
verifyFactorNames(Rcov, "cov");
}
setLatentDistribution(mean, cov);
setMinItemsPerScore(mips);
if (numItems() != pmatCols) {
mxThrow("item matrix implies %d items but spec is length %d",
pmatCols, numItems());
}
if (Rdata) {
if (itemNames.size() == 0) mxThrow("Item matrix must have colnames");
for (int ix=0; ix < numItems(); ++ix) {
bool found=false;
for (int dc=0; dc < int(dataColNames.size()); ++dc) {
if (strEQ(itemNames[ix], dataColNames[dc])) {
SEXP col = VECTOR_ELT(Rdata, dc);
if (!Rf_isFactor(col)) {
if (TYPEOF(col) == INTSXP) {
mxThrow("Column '%s' is an integer but "
"not an ordered factor",
dataColNames[dc]);
} else {
mxThrow("Column '%s' is of type %s; expecting an "
"ordered factor (integer)",
dataColNames[dc], Rf_type2char(TYPEOF(col)));
}
}
dataColumns.push_back(INTEGER(col));
found=true;
break;
}
}
if (!found) {
mxThrow("Cannot find item '%s' in data", itemNames[ix]);
}
}
if (weightColumnName) {
for (int dc=0; dc < int(dataColNames.size()); ++dc) {
if (strEQ(weightColumnName, dataColNames[dc])) {
SEXP col = VECTOR_ELT(Rdata, dc);
if (TYPEOF(col) != REALSXP) {
mxThrow("Column '%s' is of type %s; expecting type numeric (double)",
dataColNames[dc], Rf_type2char(TYPEOF(col)));
}
rowWeight = REAL(col);
break;
}
}
if (!rowWeight) {
mxThrow("Cannot find weight column '%s'", weightColumnName);
}
}
if (freqColumnName) {
for (int dc=0; dc < int(dataColNames.size()); ++dc) {
if (strEQ(freqColumnName, dataColNames[dc])) {
SEXP col = VECTOR_ELT(Rdata, dc);
if (TYPEOF(col) != INTSXP) {
mxThrow("Column '%s' is of type %s; expecting type integer",
dataColNames[dc], Rf_type2char(TYPEOF(col)));
}
rowFreq = INTEGER(col);
break;
}
}
if (!rowFreq) {
mxThrow("Cannot find frequency column '%s'", freqColumnName);
}
}
rowMap.reserve(dataRows);
for (int rx=0; rx < dataRows; ++rx) rowMap.push_back(rx);
}
Eigen::Map< Eigen::ArrayXXd > Eparam(param, paramRows, numItems());
Eigen::Map< Eigen::VectorXd > meanVec(mean, itemDims);
Eigen::Map< Eigen::MatrixXd > covMat(cov, itemDims, itemDims);
quad.setStructure(qwidth, qpoints, Eparam, meanVec, covMat);
if (paramRows < impliedParamRows) {
mxThrow("At least %d rows are required in the item parameter matrix, only %d found",
impliedParamRows, paramRows);
}
quad.refresh(meanVec, covMat);
}
void omxInitExpectationBA81(omxExpectation* oo) {
omxState* currentState = oo->currentState;
SEXP rObj = oo->rObj;
SEXP tmp;
if(OMX_DEBUG) {
mxLog("Initializing %s.", oo->name);
}
if (!Glibrpf_model) {
#if USE_EXTERNAL_LIBRPF
get_librpf_t get_librpf = (get_librpf_t) R_GetCCallable("rpf", "get_librpf_model_GPL");
(*get_librpf)(LIBIFA_RPF_API_VERSION, &Glibrpf_numModels, &Glibrpf_model);
#else
// if linking against included source code
Glibrpf_numModels = librpf_numModels;
Glibrpf_model = librpf_model;
#endif
}
BA81Expect *state = new BA81Expect;
// These two constants should be as identical as possible
state->name = oo->name;
if (0) {
state->LogLargestDouble = 0.0;
state->LargestDouble = 1.0;
} else {
state->LogLargestDouble = log(std::numeric_limits<double>::max()) - 1;
state->LargestDouble = exp(state->LogLargestDouble);
ba81NormalQuad &quad = state->getQuad();
quad.setOne(state->LargestDouble);
}
state->expectedUsed = false;
state->estLatentMean = NULL;
state->estLatentCov = NULL;
state->type = EXPECTATION_OBSERVED;
state->itemParam = NULL;
state->EitemParam = NULL;
state->itemParamVersion = 0;
state->latentParamVersion = 0;
oo->argStruct = (void*) state;
{ScopedProtect p1(tmp, R_do_slot(rObj, Rf_install("data")));
state->data = omxDataLookupFromState(tmp, currentState);
}
if (strcmp(omxDataType(state->data), "raw") != 0) {
omxRaiseErrorf("%s unable to handle data type %s", oo->name, omxDataType(state->data));
return;
}
{ScopedProtect p1(tmp, R_do_slot(rObj, Rf_install("verbose")));
state->verbose = Rf_asInteger(tmp);
}
int targetQpoints;
{ScopedProtect p1(tmp, R_do_slot(rObj, Rf_install("qpoints")));
targetQpoints = Rf_asInteger(tmp);
}
{ScopedProtect p1(tmp, R_do_slot(rObj, Rf_install("qwidth")));
state->grp.setGridFineness(Rf_asReal(tmp), targetQpoints);
}
{ScopedProtect p1(tmp, R_do_slot(rObj, Rf_install("ItemSpec")));
state->grp.importSpec(tmp);
if (state->verbose >= 2) mxLog("%s: found %d item specs", oo->name, state->numItems());
}
state->_latentMeanOut = omxNewMatrixFromSlot(rObj, currentState, "mean");
state->_latentCovOut = omxNewMatrixFromSlot(rObj, currentState, "cov");
state->itemParam = omxNewMatrixFromSlot(rObj, currentState, "item");
state->grp.param = state->itemParam->data; // algebra not allowed yet TODO
const int numItems = state->itemParam->cols;
if (state->numItems() != numItems) {
omxRaiseErrorf("ItemSpec length %d must match the number of item columns (%d)",
state->numItems(), numItems);
return;
}
if (state->itemParam->rows != state->grp.impliedParamRows) {
omxRaiseErrorf("item matrix must have %d rows", state->grp.impliedParamRows);
return;
}
state->grp.paramRows = state->itemParam->rows;
// for algebra item param, will need to defer until later?
state->grp.learnMaxAbilities();
int maxAbilities = state->grp.itemDims;
state->grp.setFactorNames(state->itemParam->rownames);
{
ProtectedSEXP tmp2(R_do_slot(rObj, Rf_install(".detectIndependence")));
state->grp.detectIndependence = Rf_asLogical(tmp2);
}
{ScopedProtect p1(tmp, R_do_slot(rObj, Rf_install("EstepItem")));
if (!Rf_isNull(tmp)) {
int rows, cols;
getMatrixDims(tmp, &rows, &cols);
if (rows != state->itemParam->rows || cols != state->itemParam->cols) {
Rf_error("EstepItem must have the same dimensions as the item MxMatrix");
}
state->EitemParam = REAL(tmp);
}
}
oo->computeFun = ba81compute;
oo->setVarGroup = ignoreSetVarGroup;
oo->destructFun = ba81Destroy;
oo->populateAttrFun = ba81PopulateAttributes;
oo->componentFun = getComponent;
oo->canDuplicate = false;
// TODO: Exactly identical rows do not contribute any information.
// The sorting algorithm ought to remove them so we get better cache behavior.
// The following summary stats would be cheaper to calculate too.
omxData *data = state->data;
if (data->hasDefinitionVariables()) Rf_error("%s: not implemented yet", oo->name);
std::vector<int> &rowMap = state->grp.rowMap;
int weightCol;
{ScopedProtect p1(tmp, R_do_slot(rObj, Rf_install("weightColumn")));
weightCol = INTEGER(tmp)[0];
}
if (weightCol == NA_INTEGER) {
// Should rowMap be part of omxData? This is essentially a
// generic compression step that shouldn't be specific to IFA models.
state->grp.rowWeight = (double*) R_alloc(data->rows, sizeof(double));
rowMap.resize(data->rows);
int numUnique = 0;
for (int rx=0; rx < data->rows; ) {
int rw = 1;
state->grp.rowWeight[numUnique] = rw;
rowMap[numUnique] = rx;
rx += rw;
++numUnique;
}
rowMap.resize(numUnique);
state->weightSum = state->data->rows;
}
else {
if (omxDataColumnIsFactor(data, weightCol)) {
omxRaiseErrorf("%s: weightColumn %d is a factor", oo->name, 1 + weightCol);
return;
}
state->grp.rowWeight = omxDoubleDataColumn(data, weightCol);
state->weightSum = 0;
for (int rx=0; rx < data->rows; ++rx) { state->weightSum += state->grp.rowWeight[rx]; }
rowMap.resize(data->rows);
for (size_t rx=0; rx < rowMap.size(); ++rx) {
rowMap[rx] = rx;
}
}
// complain about non-integral rowWeights (EAP can't work) TODO
auto colMap = oo->getDataColumns();
for (int cx = 0; cx < numItems; cx++) {
int *col = omxIntDataColumnUnsafe(data, colMap[cx]);
state->grp.dataColumns.push_back(col);
}
// sanity check data
for (int cx = 0; cx < numItems; cx++) {
if (!omxDataColumnIsFactor(data, colMap[cx])) {
data->omxPrintData("diagnostic", 3);
omxRaiseErrorf("%s: column %d is not a factor", oo->name, int(1 + colMap[cx]));
return;
}
}
// TODO the max outcome should be available from omxData
for (int rx=0; rx < data->rows; rx++) {
int cols = 0;
for (int cx = 0; cx < numItems; cx++) {
const int *col = state->grp.dataColumns[cx];
int pick = col[rx];
if (pick == NA_INTEGER) continue;
++cols;
const int no = state->grp.itemOutcomes[cx];
if (pick > no) {
Rf_error("Data for item '%s' has at least %d outcomes, not %d",
state->itemParam->colnames[cx], pick, no);
}
}
if (cols == 0) {
Rf_error("Row %d has all NAs", 1+rx);
}
}
if (state->_latentMeanOut && state->_latentMeanOut->rows * state->_latentMeanOut->cols != maxAbilities) {
Rf_error("The mean matrix '%s' must be a row or column vector of size %d",
state->_latentMeanOut->name(), maxAbilities);
}
if (state->_latentCovOut && (state->_latentCovOut->rows != maxAbilities ||
state->_latentCovOut->cols != maxAbilities)) {
Rf_error("The cov matrix '%s' must be %dx%d",
state->_latentCovOut->name(), maxAbilities, maxAbilities);
}
state->grp.setLatentDistribution(state->_latentMeanOut? state->_latentMeanOut->data : NULL,
state->_latentCovOut? state->_latentCovOut->data : NULL);
{
EigenArrayAdaptor Eparam(state->itemParam);
Eigen::Map< Eigen::VectorXd > meanVec(state->grp.mean, maxAbilities);
Eigen::Map< Eigen::MatrixXd > covMat(state->grp.cov, maxAbilities, maxAbilities);
state->grp.quad.setStructure(state->grp.qwidth, state->grp.qpoints,
Eparam, meanVec, covMat);
}
{ScopedProtect p1(tmp, R_do_slot(rObj, Rf_install("minItemsPerScore")));
state->grp.setMinItemsPerScore(Rf_asInteger(tmp));
}
state->grp.buildRowSkip();
if (isErrorRaised()) return;
{ScopedProtect p1(tmp, R_do_slot(rObj, Rf_install("debugInternal")));
state->debugInternal = Rf_asLogical(tmp);
}
state->ElatentVersion = 0;
if (state->_latentMeanOut) {
state->estLatentMean = omxInitMatrix(maxAbilities, 1, TRUE, currentState);
omxCopyMatrix(state->estLatentMean, state->_latentMeanOut); // rename matrices TODO
}
if (state->_latentCovOut) {
state->estLatentCov = omxInitMatrix(maxAbilities, maxAbilities, TRUE, currentState);
omxCopyMatrix(state->estLatentCov, state->_latentCovOut);
}
}
// Initialization process
void EkfNode::init() {
/**************************************************************************
* Initialize firstRun, time, and frame names
**************************************************************************/
// Set first run to true for encoders. Once a message is received, this will
// be set to false.
firstRunEnc_ = true;
firstRunSys_ = true;
// Set Times
ros::Time currTime = ros::Time::now();
lastEncTime_ = currTime;
lastSysTime_ = currTime;
// Use the ROS parameter server to initilize parameters
if(!private_nh_.getParam("base_frame", base_frame_))
base_frame_ = "base_link";
if(!private_nh_.getParam("odom_frame", base_frame_))
odom_frame_ = "odom";
if(!private_nh_.getParam("map_frame", map_frame_))
map_frame_ = "map";
/**************************************************************************
* Initialize state for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize state
double state_temp[] = {0, 0, 0, 0, 0, 0};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> state (state_temp, state_temp + sizeof(state_temp) / sizeof(double));
// Check the parameter server and initialize state
if(!private_nh_.getParam("state", state)) {
ROS_WARN_STREAM("No state found. Using default.");
}
// Check to see if the size is not equal to 6
if (state.size() != 6) {
ROS_WARN_STREAM("state isn't 6 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 6, 1> Vector6d;
Vector6d stateMat(state.data());
std::cout << "state_map = " << std::endl;
std::cout << stateMat << std::endl;
ekf_map_.initState(stateMat);
// Odom is always initialized at all zeros.
stateMat << 0, 0, 0, 0, 0, 0;
ekf_odom_.initState(stateMat);
std::cout << "state_odom = " << std::endl;
std::cout << stateMat << std::endl;
/**************************************************************************
* Initialize covariance for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize covariance
double cov_temp[] = {0.01, 0, 0, 0, 0, 0,
0, 0.01, 0, 0, 0, 0,
0, 0, 0.01, 0, 0, 0,
0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> cov (cov_temp, cov_temp + sizeof(cov_temp) / sizeof(double));
// Check the parameter server and initialize cov
if(!private_nh_.getParam("covariance", cov)) {
ROS_WARN_STREAM("No covariance found. Using default.");
}
// Check to see if the size is not equal to 36
if (cov.size() != 36) {
ROS_WARN_STREAM("cov isn't 36 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 6, 6, RowMajor> Matrix66;
Matrix66 covMat(cov.data());
std::cout << "covariance = " << std::endl;
std::cout << covMat << std::endl;
ekf_map_.initCov(covMat);
// Initialize odom covariance the same as the map covariance (this isn't
// correct. But, since it is all an estimate anyway, it should be fine.
ekf_odom_.initCov(covMat);
/**************************************************************************
* Initialize Q for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize Q
double Q_temp[] = {0.01, 0, 0, 0, 0, 0,
0, 0.01, 0, 0, 0, 0,
0, 0, 0.01, 0, 0, 0,
0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> Q (Q_temp, Q_temp + sizeof(Q_temp) / sizeof(double));
// Check the parameter server and initialize Q
if(!private_nh_.getParam("Q", Q)) {
ROS_WARN_STREAM("No Q found. Using default.");
}
// Check to see if the size is not equal to 36
if (Q.size() != 36) {
ROS_WARN_STREAM("Q isn't 36 elements long!");
}
// And initialize the Matrix
Matrix66 QMat(Q.data());
std::cout << "Q = " << std::endl;
std::cout << QMat << std::endl;
ekf_map_.initSystem(QMat);
ekf_odom_.initSystem(QMat);
/**************************************************************************
* Initialize Decawave for ekf_map_ (not used in ekf_odom_)
**************************************************************************/
// Create temp array to initialize R for DecaWave
double RDW_temp[] = {0.01, 0, 0, 0,
0, 0.01, 0, 0,
0, 0, 0.01, 0,
0, 0, 0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> RDW (RDW_temp, RDW_temp + sizeof(RDW_temp) / sizeof(double));
// Check the parameter server and initialize RDW
if(!private_nh_.getParam("DW_R", RDW)) {
ROS_WARN_STREAM("No DW_R found. Using default.");
}
// Check to see if the size is not equal to 16
if (RDW.size() != 16) {
ROS_WARN_STREAM("DW_R isn't 16 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 4, 4, RowMajor> Matrix44;
Matrix44 RDWMat(RDW.data());
std::cout << "RDecaWave = " << std::endl;
std::cout << RDWMat << std::endl;
// Create temp array to initialize beacon locations for DecaWave
double DWBL_temp[] = {0, 0,
5, 0,
5, 15,
0, 15};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> DWBL (DWBL_temp, DWBL_temp + sizeof(DWBL_temp) / sizeof(double));
// Check the parameter server and initialize DWBL
if(!private_nh_.getParam("DW_Beacon_Loc", DWBL)) {
ROS_WARN_STREAM("No DW_Beacon_Loc found. Using default.");
}
// Check to see if the size is not equal to 8
if (DWBL.size() != 8) {
ROS_WARN_STREAM("DW_Beacon_Loc isn't 8 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 4, 2, RowMajor> Matrix42;
Matrix42 DWBLMat(DWBL.data());
std::cout << "DW_Beacon_Loc = " << std::endl;
std::cout << DWBLMat << std::endl;
MatrixXd DecaWaveBeaconLoc(4,2);
MatrixXd DecaWaveOffset(2,1);
double DW_offset_x;
double DW_offset_y;
if(!private_nh_.getParam("DW_offset_x", DW_offset_x))
DW_offset_x = 0.0;
if(!private_nh_.getParam("DW_offset_y", DW_offset_y))
DW_offset_y = 0.0;
DecaWaveOffset << DW_offset_x, DW_offset_y;
std::cout << "DecaWaveOffset = " << std::endl;
std::cout << DecaWaveOffset << std::endl;
ekf_map_.initDecaWave(RDWMat, DWBLMat, DecaWaveOffset);
// Decawave is not used in odom, so no need to initialize
/**************************************************************************
* Initialize Encoders for ekf_odom_ and ekf_map_
**************************************************************************/
// Create temp array to initialize R for DecaWave
double REnc_temp[] = {0.01, 0,
0, 0.01};
// And a std::vector, which will be used to initialize an Eigen Matrix
std::vector<double> REnc (REnc_temp, REnc_temp + sizeof(REnc_temp) / sizeof(double));
// Check the parameter server and initialize RDW
if(!private_nh_.getParam("Enc_R", REnc)) {
ROS_WARN_STREAM("No Enc_R found. Using default.");
}
// Check to see if the size is not equal to 4
if (REnc.size() != 4) {
ROS_WARN_STREAM("Enc_R isn't 4 elements long!");
}
// And initialize the Matrix
typedef Matrix<double, 2, 2, RowMajor> Matrix22;
Matrix22 REncMat(REnc.data());
std::cout << "R_Enc = " << std::endl;
std::cout << REncMat << std::endl;
double b, tpmRight, tpmLeft;
if(!private_nh_.getParam("track_width", b))
b = 1;
if(!private_nh_.getParam("tpm_right", tpmRight))
tpmRight = 1;
if(!private_nh_.getParam("tpm_left", tpmLeft))
tpmLeft = 1;
ekf_map_.initEnc(REncMat, b, tpmRight, tpmLeft);
ekf_odom_.initEnc(REncMat, b, tpmRight, tpmLeft);
std::cout << "track_width = " << b << std::endl;
std::cout << "tpm_right = " << tpmRight << std::endl;
std::cout << "tpm_left = " << tpmLeft << std::endl;
/**************************************************************************
* Initialize IMU for ekf_odom_ and ekf_map_
**************************************************************************/
double RIMU;
if(!private_nh_.getParam("R_IMU", RIMU))
RIMU = 0.01;
ekf_map_.initIMU(RIMU);
ekf_odom_.initIMU(RIMU);
std::cout << "R_IMU = " << RIMU << std::endl;
// Publish the initialized state and exit initialization.
publishState(currTime);
ROS_INFO_STREAM("Finished with initialization.");
}
NRLib::Matrix
State4D::GetFullCov()
{
NRLib::Matrix covMat(6,6);
covMat=0.0;
for(int i=0;i<6;i++)
sigma_static_static_[i]->setAccessMode(FFTGrid::READ);
covMat(0,0) = sigma_static_static_[0]->getNextReal(); // [0] = vp_vp, [1] = vp_vs, [2] = vp_rho ,[3] = vs_vs, [4] = vs_rho, [5] = rho_rho (all static)
covMat(0,1) = sigma_static_static_[1]->getNextReal();
covMat(0,2) = sigma_static_static_[2]->getNextReal();
covMat(1,1) = sigma_static_static_[3]->getNextReal();
covMat(1,2) = sigma_static_static_[4]->getNextReal();
covMat(2,2) = sigma_static_static_[5]->getNextReal();
covMat(1,0) = covMat(0,1);
covMat(2,0) = covMat(0,2);
covMat(2,1) = covMat(1,2);
for(int i=0;i<6;i++)
sigma_static_static_[i]->endAccess();
for(int i=0;i<6;i++)
sigma_dynamic_dynamic_[i]->setAccessMode(FFTGrid::READ);
covMat(3,3) = sigma_dynamic_dynamic_[0]->getNextReal(); // [0] = vp_vp, [1] = vp_vs, [2] = vp_rho ,[3] = vs_vs, [4] = vs_rho, [5] = rho_rho (all static)
covMat(3,4) = sigma_dynamic_dynamic_[1]->getNextReal();
covMat(3,5) = sigma_dynamic_dynamic_[2]->getNextReal();
covMat(4,4) = sigma_dynamic_dynamic_[3]->getNextReal();
covMat(4,5) = sigma_dynamic_dynamic_[4]->getNextReal();
covMat(5,5) = sigma_dynamic_dynamic_[5]->getNextReal();
covMat(4,3) = covMat(3,4);
covMat(5,3) = covMat(3,5);
covMat(5,4) = covMat(4,5);
for(int i=0;i<6;i++)
sigma_dynamic_dynamic_[i]->endAccess();
for(int i=0;i<3;i++)
for(int j=0;j<3;j++){
int counter=j+3*i;
sigma_static_dynamic_[counter]->setAccessMode(FFTGrid::READ);
double value = double(sigma_static_dynamic_[counter]->getNextReal()); // [0] = vpStat_vpDyn, [1] = vpStat_vsDyn, [2] = vpStat_rhoDyn ,[3] = vsStat_vpDyn,
sigma_static_dynamic_[counter]->endAccess();
covMat(i,j+3) = value;
covMat(j+3,i) = value;
}
return covMat;
}
