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