std::string Gaussian<ScalarType>::toJSONString(bool styledWriter) const
{
DEBUG_OUT("saving gaussian as JSON", 40);
//uses Jsoncpp as library. Jsoncpp is licensed as MIT, so we may use it without restriction.
Json::Value root;
Json::Writer* writer=NULL;
if (styledWriter)
writer = new Json::StyledWriter();
else
writer = new Json::FastWriter();
//output the weight
root["weight"] = getWeight();
//output the mean as array of doubles
Json::Value mean(Json::arrayValue);
Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> gMean = getMean();
for (int i=0; i<gMean.size(); i++)
mean.append(Json::Value(gMean[i]));
root["mean"] = mean;
//output the variance as array of double. only save the lower
//triangular, as the other values are mirrored.
Json::Value variance(Json::arrayValue);
Eigen::Matrix<ScalarType, Eigen::Dynamic, Eigen::Dynamic> gVariance = getCovarianceMatrix();
if (isDiagonal(gVariance))
{
DEBUG_OUT("save as diagonal covariance matrix", 45);
for (int i=0; i<gVariance.rows(); i++)
{
variance.append(Json::Value(gVariance(i, i)));
}
}
else
{
DEBUG_OUT("save as full covariance matrix", 45);
for (int i=0; i<gVariance.rows(); i++)
{
for (int j=i; j<gVariance.cols(); j++)
{
variance.append(Json::Value(gVariance(i, j)));
}
}
}
root["covariance"] = variance;
std::string str = writer->write(root);
delete writer;
return str;
}
void BeaconKFNode::createFilter( void)
{
MatrixWrapper::Matrix systemA(3,3);
systemA(1,1) = 1.0; systemA(1,2) = 0.0; systemA(1,3) = 0.0;
systemA(2,1) = 0.0; systemA(2,2) = 1.0; systemA(2,3) = 0.0;
systemA(3,1) = 0.0; systemA(3,2) = 0.0; systemA(3,3) = 1.0;
std::vector<MatrixWrapper::Matrix> systemMats(1);
systemMats[0] = systemA;
MatrixWrapper::SymmetricMatrix sysNoiseCovariance(3);
getCovarianceMatrix("system_noise_covariance", sysNoiseCovariance);
ROS_INFO_STREAM("system noise covariance: " << sysNoiseCovariance);
MatrixWrapper::ColumnVector sysNoiseMu(3); // zeros
sysNoiseMu(1) = 0.0;
sysNoiseMu(2) = 0.0;
sysNoiseMu(3) = 0.0;
BFL::Gaussian sysUncertainty(sysNoiseMu, sysNoiseCovariance);
_system_pdf = new BFL::LinearAnalyticConditionalGaussian(systemMats, sysUncertainty);
_system_model = new BFL::LinearAnalyticSystemModelGaussianUncertainty(_system_pdf);
MatrixWrapper::ColumnVector priorMu(3);
priorMu(1) = 0.0;
priorMu(2) = 0.0;
priorMu(3) = 0.0;
MatrixWrapper::SymmetricMatrix priorCov(3);
priorCov(1,1) = 1.0; priorCov(1,2) = 0.0; priorCov(1,3) = 0.0;
priorCov(2,2) = 1.0; priorCov(2,3) = 0.0;
priorCov(3,3) = 0.5;
_prior = new BFL::Gaussian(priorMu, priorCov);
_filter = new BFL::ExtendedKalmanFilter(_prior);
}
void MainAxisPCA::computeMainAxis(const std::vector<ml::vec3>& pointCloud)
{
ML_TRACE_IN("void MainAxisPCA::computeMainAxis()");
float** covaMatrix = matrix(1,3,1,3);
float** jacobiMat = matrix(1,3,1,3);
float** invMatrix = matrix(1,3,1,3);
float* eigenValues = vL_vector(1,3);
const int size = static_cast<int>( pointCloud.size() );
// copy positions to vertices
float* vertices = NULL;
ML_CHECK_NEW(vertices, float[size * 3]);
unsigned int i=0;
unsigned int entry = 0;
for (i = 0; i < size; i++){
const ml::vec3& currPos = pointCloud[i];
vertices[entry++] = static_cast<float>(currPos[0]);
vertices[entry++] = static_cast<float>(currPos[1]);
vertices[entry++] = static_cast<float>(currPos[2]);
}
// Calculate center of mass
ML_DELETE_ARR(_baryCenter);
_baryCenter = calcBaryCenter(vertices, size);
// Compute covariant matrix, so the Jacobian matrix can be computed
getCovarianceMatrix(vertices, static_cast<long int>(size), covaMatrix, _baryCenter);
// Compute the Jacobian matrix
int nrot=0; // dummy variable
jacobi(covaMatrix, 3, eigenValues, jacobiMat, &nrot);
// Calculate main axes
unsigned int counter=0;
for (counter = 0; counter < 3; counter++) {
_xAxis[counter] = jacobiMat[counter + 1][1];
_yAxis[counter] = jacobiMat[counter + 1][2];
_zAxis[counter] = jacobiMat[counter + 1][3];
}
// Multiply each point by the eigenvectors of the Jacobian matrix
const float* points = vertices;
float* newVertices = NULL;
ML_CHECK_NEW(newVertices, float[size * 3]);
unsigned int counter2 = 0;
for (counter = 0; counter < size; counter++) {
newVertices[counter2++] = dotProduct(points, _xAxis); //tempPoint.dot(_xAxis);
newVertices[counter2++] = dotProduct(points, _yAxis); //tempPoint.dot(_yAxis);
newVertices[counter2++] = dotProduct(points, _zAxis); //tempPoint.dot(_zAxis);
points += 3;
}
// Get extends of the bounding box
float minX=0, maxX=0, minY=0, maxY=0, minZ=0, maxZ=0;
getBoundingBox(newVertices, static_cast<long int>(size), minX, maxX, minY, maxY, minZ, maxZ);
// Extends of the object aligned bounding box
_xDiameter = maxX - minX;
_yDiameter = maxY - minY;
_zDiameter = maxZ - minZ;
// Half the extend...
float half_x = _xDiameter / 2.0f;
float half_y = _yDiameter / 2.0f;
float half_z = _zDiameter / 2.0f;
// Rotate all points back by multiplying them with the inverse Jacobian matrix.
getInverseMatrix(jacobiMat, invMatrix);
float* tmp = stretchVector(_xAxis, half_x);
for (counter = 0; counter < 3; counter++) {
_midPoint[counter] =
minX * invMatrix[1][counter + 1] +
(minY + half_y) * invMatrix[2][counter + 1] +
(minZ + half_z) * invMatrix[3][counter + 1] + tmp[counter];
}
ML_DELETE_ARR(tmp);
ML_DELETE_ARR(newVertices);
ML_DELETE_ARR(vertices);
free_matrix(covaMatrix, 1, 3, 1, 3);
free_matrix(jacobiMat, 1, 3, 1, 3);
free_matrix(invMatrix, 1, 3, 1, 3);
free(eigenValues);
}
