本文整理汇总了C++中eigen::Matrix3d::computeInverseAndDetWithCheck方法的典型用法代码示例。如果您正苦于以下问题：C++ Matrix3d::computeInverseAndDetWithCheck方法的具体用法？C++ Matrix3d::computeInverseAndDetWithCheck怎么用？C++ Matrix3d::computeInverseAndDetWithCheck使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类eigen::Matrix3d的用法示例。
在下文中一共展示了Matrix3d::computeInverseAndDetWithCheck方法的3个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: get_rasterized_fast
DensityGrid get_rasterized_fast(const Gaussian3Ds &gmm, const Floats &weights,
double cell_width, const BoundingBox3D &bb, double factor) {
DensityGrid ret(cell_width, bb, 0);
for (unsigned int ng = 0; ng < gmm.size(); ng++) {
Eigen::Matrix3d covar = get_covariance(gmm[ng]);
Eigen::Matrix3d inverse = Eigen::Matrix3d::Zero(3, 3);
double determinant;
bool invertible;
covar.computeInverseAndDetWithCheck(inverse, determinant, invertible);
IMP_INTERNAL_CHECK((invertible && determinant > 0),
"Tried to invert Gaussian, but it's not proper matrix");
double pre(get_gaussian_eval_prefactor(determinant));
Eigen::Vector3d evals = covar.eigenvalues().real();
double maxeval = sqrt(evals.maxCoeff());
double cutoff = factor * maxeval;
double cutoff2 = cutoff * cutoff;
Vector3D c = gmm[ng].get_center();
Vector3D lower = c - Vector3D(cutoff, cutoff, cutoff);
Vector3D upper = c + Vector3D(cutoff, cutoff, cutoff);
GridIndex3D lowerindex = ret.get_nearest_index(lower);
GridIndex3D upperindex = ret.get_nearest_index(upper);
Eigen::Vector3d center(c.get_data());
IMP_INTERNAL_CHECK(invertible, "matrix wasn't invertible! uh oh!");
IMP_GRID3D_FOREACH_SMALLER_EXTENDED_INDEX_RANGE(ret, upperindex, lowerindex,
upperindex, {
GridIndex3D i(voxel_index[0], voxel_index[1], voxel_index[2]);
Eigen::Vector3d r(get_vec_from_center(i, ret, center));
if (r.squaredNorm() < cutoff2) {
update_value(&ret, i, r, inverse, pre, weights[ng]);
}
})
}
return ret;
}示例2: get_rasterized
DensityGrid get_rasterized(const Gaussian3Ds &gmm, const Floats &weights,
double cell_width, const BoundingBox3D &bb) {
DensityGrid ret(cell_width, bb, 0);
for (unsigned int ng = 0; ng < gmm.size(); ng++) {
Eigen::Matrix3d covar = get_covariance(gmm[ng]);
Eigen::Matrix3d inverse = Eigen::Matrix3d::Zero(3, 3);
double determinant;
bool invertible;
covar.computeInverseAndDetWithCheck(inverse, determinant, invertible);
IMP_INTERNAL_CHECK((invertible && determinant > 0),
"Tried to invert Gaussian, but it's not proper matrix");
double pre(get_gaussian_eval_prefactor(determinant));
Eigen::Vector3d center(gmm[ng].get_center().get_data());
IMP_INTERNAL_CHECK(invertible, "matrix wasn't invertible! uh oh!");
IMP_FOREACH(const DensityGrid::Index & i, ret.get_all_indexes()) {
Eigen::Vector3d r(get_vec_from_center(i, ret, center));
update_value(&ret, i, r, inverse, pre, weights[ng]);
}
}
return ret;
}示例3: local_map
void NDTMCL3D::updateAndPredictEff(Eigen::Affine3d Tmotion, pcl::PointCloud<pcl::PointXYZ> &cloud, double subsample_level){
if(subsample_level < 0 || subsample_level > 1) subsample_level = 1;
Eigen::Vector3d tr = Tmotion.translation();
Eigen::Vector3d rot = Tmotion.rotation().eulerAngles(0,1,2);
double time_start = getDoubleTime();
//pf.predict(Tmotion, tr[0]*0.1, tr[1]*0.1, tr[2]*0.1, rot[0]*0.1, rot[1]*0.1, rot[2]*0.1);
if(rot[2]<(0.5 * M_PI/180.0) && tr[0]>=0){
//pf.predict(Tmotion, tr[0]*0.2 + 0.005, tr[1]*0.1+ 0.005, tr[2]*0.1+0.005 ,rot[0]*0.2+0.001,rot[1]*0.2+0.001, rot[2]*0.2+0.001);
pf.predict(Tmotion, tr[0]*0.2 + 0.105, tr[1]*0.1+ 0.105, tr[2]*0.1+0.005 ,rot[0]*0.2+0.001,rot[1]*0.2+0.001, rot[2]*0.2+0.031);
}else if(tr[0]>=0){
//pf.predict(Tmotion,tr[0]*0.5 + 0.005, tr[1]*0.1+ 0.005, tr[2]*0.1+0.005 ,rot[0]*0.2+0.001,rot[1]*0.2+0.001, rot[2]*0.4+0.001);
pf.predict(Tmotion,tr[0]*0.5 + 0.095, tr[1]*0.1+ 0.095, tr[2]*0.1+0.005 ,rot[0]*0.2+0.001,rot[1]*0.2+0.001, rot[2]*0.4+0.061);
}else{
//pf.predict(Tmotion, tr[0]*0.2 + 0.005, tr[1]*0.1+ 0.005, tr[2]*0.1+0.005 ,rot[0]*0.2+0.001,rot[1]*0.2+0.001, rot[2]*0.2+0.001);
pf.predict(Tmotion, tr[0]*0.2 + 0.055, tr[1]*0.1+ 0.055, tr[2]*0.1+0.005 ,rot[0]*0.2+0.001,rot[1]*0.2+0.001, rot[2]*0.2+0.021);
}
double t_pred = getDoubleTime() - time_start;
std::cerr<<"cloud points "<<cloud.points.size()<<" res :"<<resolution<<" sres: "<<resolution_sensor<<std::endl;
lslgeneric::NDTMap local_map(new lslgeneric::LazyGrid(resolution));
//local_map.guessSize(0,0,0,30,30,10); //sensor_range,sensor_range,map_size_z);
local_map.loadPointCloud(cloud);//,30); //sensor_range);
local_map.computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);
/*lslgeneric::NDTMap<PointT> local_map(new lslgeneric::LazyGrid<PointT>(resolution_sensor));
local_map.addPointCloudSimple(cloud);
//local_map.computeNDTCells();
local_map.computeNDTCellsSimple();
*/
std::vector<lslgeneric::NDTCell*> ndts0 = local_map.getAllCells();
std::vector<lslgeneric::NDTCell*> ndts;
std::cerr<<"ndts: "<<ndts0.size()<<std::endl;
if(subsample_level != 1) {
srand((int)(t_pred*10000));
for(int i=0; i<ndts0.size(); ++i) {
double p = ((double)rand())/RAND_MAX;
if(p < subsample_level) {
ndts.push_back(ndts0[i]);
} else {
delete ndts0[i];
}
}
} else {
ndts = ndts0;
}
std::cerr<<"resampled ndts: "<<ndts.size()<<std::endl;
int Nn = 0;
// #pragma omp parallel for
double t_pseudo = 0;
#pragma omp parallel num_threads(4)
{
#pragma omp for
for(int i=0;i<pf.size();i++){
Eigen::Affine3d T = pf.pcloud[i].T;
//ndts = local_map.pseudoTransformNDT(T);
double score=1;
if(ndts.size()==0) fprintf(stderr,"ERROR no gaussians in measurement!!!\n");
Nn = ndts.size();
for(int n=0;n<ndts.size();n++){
Eigen::Vector3d m = T*ndts[n]->getMean();
if(m[2]<zfilt_min) continue;
lslgeneric::NDTCell *cell;
pcl::PointXYZ p;
p.x = m[0];p.y=m[1];p.z=m[2];
if(map.getCellAtPoint(p,cell)){
//if(map.getCellForPoint(p,cell)){
if(cell == NULL) continue;
if(cell->hasGaussian_){
Eigen::Matrix3d covCombined = cell->getCov() + T.rotation()*ndts[n]->getCov() *T.rotation().transpose();
Eigen::Matrix3d icov;
bool exists;
double det = 0;
covCombined.computeInverseAndDetWithCheck(icov,det,exists);
if(!exists) continue;
double l = (cell->getMean() - m).dot(icov*(cell->getMean() - m));
if(l*0 != 0) continue;
score += 0.1 + 0.9 * exp(-0.05*l/2.0);
}else{
}
}
}
pf.pcloud[i].lik = score;
}
}///#pragma
//.........这里部分代码省略.........