本文整理汇总了C++中eigen::Matrix3f::setZero方法的典型用法代码示例。如果您正苦于以下问题:C++ Matrix3f::setZero方法的具体用法?C++ Matrix3f::setZero怎么用?C++ Matrix3f::setZero使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类eigen::Matrix3f
的用法示例。
在下文中一共展示了Matrix3f::setZero方法的13个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: computeGradientParameters
/** \brief Computes the gradient parameters of the patch (patch gradient components dx_ dy_, Hessian H_, Shi-Tomasi Score s_, Eigenvalues of the Hessian e0_ and e1_).
* The expanded patch patchWithBorder_ must be set.
* Sets validGradientParameters_ afterwards to true.
*/
void computeGradientParameters() const{
if(!validGradientParameters_){
H_.setZero();
const int refStep = patchSize+2;
float* it_dx = dx_;
float* it_dy = dy_;
const float* it;
Eigen::Vector3f J;
J.setZero();
for(int y=0; y<patchSize; ++y){
it = patchWithBorder_ + (y+1)*refStep + 1;
for(int x=0; x<patchSize; ++x, ++it, ++it_dx, ++it_dy){
J[0] = 0.5 * (it[1] - it[-1]);
J[1] = 0.5 * (it[refStep] - it[-refStep]);
J[2] = 1;
*it_dx = J[0];
*it_dy = J[1];
H_ += J*J.transpose();
}
}
const float dXX = H_(0,0)/(patchSize*patchSize);
const float dYY = H_(1,1)/(patchSize*patchSize);
const float dXY = H_(0,1)/(patchSize*patchSize);
e0_ = 0.5 * (dXX + dYY - sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY)));
e1_ = 0.5 * (dXX + dYY + sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY)));
s_ = e0_;
validGradientParameters_ = true;
}
}
示例2: computeCovarianceMatrix
void computeCovarianceMatrix(PointCloudRGB cloud, std::vector<int> indices, Eigen::Vector4f centroid, Eigen::Matrix3f &covariance_matrix)
{
// ROS_INFO("Inside computeCovarianceMatrix() ");
// Initialize to 0
covariance_matrix.setZero ();
if (indices.empty ())
return;
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
// For each point in the cloud
for (size_t i = 0; i < indices.size (); ++i)
{
//Eigen::Vector4f ptx = cloud.points[indices[i]].getVector4fMap ();
///std::cout << "Index : "<< i <<" : "<<ptx<<std::endl;
Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;
covariance_matrix (0, 0) += pt.x () * pt.x ();
covariance_matrix (0, 1) += pt.x () * pt.y ();
covariance_matrix (0, 2) += pt.x () * pt.z ();
covariance_matrix (1, 1) += pt.y () * pt.y ();
covariance_matrix (1, 2) += pt.y () * pt.z ();
covariance_matrix (2, 2) += pt.z () * pt.z ();
}
}
// NaN or Inf values could exist => check for them
else
{
//std::cout<<"Cloud is not dense "<<std::endl;
// For each point in the cloud
for (size_t i = 0; i < indices.size (); ++i)
{
// Check if the point is invalid
if (!pcl_isfinite (cloud.points[indices[i]].x) ||
!pcl_isfinite (cloud.points[indices[i]].y) ||
!pcl_isfinite (cloud.points[indices[i]].z))
continue;
// Eigen::Vector4f ptx = cloud.points[indices[i]].getVector4fMap ();
// std::cout << "Index : "<< i <<" : "<<ptx<<std::endl;
Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;
covariance_matrix (0, 0) += pt.x () * pt.x ();
covariance_matrix (0, 1) += pt.x () * pt.y ();
covariance_matrix (0, 2) += pt.x () * pt.z ();
covariance_matrix (1, 1) += pt.y () * pt.y ();
covariance_matrix (1, 2) += pt.y () * pt.z ();
covariance_matrix (2, 2) += pt.z () * pt.z ();
covariance_matrix (1, 1) += pt.y () * pt.y ();
}
}
covariance_matrix (1, 0) = covariance_matrix (0, 1);
covariance_matrix (2, 0) = covariance_matrix (0, 2);
covariance_matrix (2, 1) = covariance_matrix (1, 2);
covariance_matrix /= indices.size();
}
示例3:
template <typename PointT> inline void
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
const std::vector<int> &indices,
const Eigen::Vector4f ¢roid,
Eigen::Matrix3f &covariance_matrix)
{
// Initialize to 0
covariance_matrix.setZero ();
if (indices.empty ())
return;
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
// For each point in the cloud
for (size_t i = 0; i < indices.size (); ++i)
{
Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;
covariance_matrix (1, 1) += pt.y () * pt.y ();
covariance_matrix (1, 2) += pt.y () * pt.z ();
covariance_matrix (2, 2) += pt.z () * pt.z ();
pt *= pt.x ();
covariance_matrix (0, 0) += pt.x ();
covariance_matrix (0, 1) += pt.y ();
covariance_matrix (0, 2) += pt.z ();
}
}
// NaN or Inf values could exist => check for them
else
{
// For each point in the cloud
for (size_t i = 0; i < indices.size (); ++i)
{
// Check if the point is invalid
if (!pcl_isfinite (cloud.points[indices[i]].x) ||
!pcl_isfinite (cloud.points[indices[i]].y) ||
!pcl_isfinite (cloud.points[indices[i]].z))
continue;
Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;
covariance_matrix (1, 1) += pt.y () * pt.y ();
covariance_matrix (1, 2) += pt.y () * pt.z ();
covariance_matrix (2, 2) += pt.z () * pt.z ();
pt *= pt.x ();
covariance_matrix (0, 0) += pt.x ();
covariance_matrix (0, 1) += pt.y ();
covariance_matrix (0, 2) += pt.z ();
}
}
covariance_matrix (1, 0) = covariance_matrix (0, 1);
covariance_matrix (2, 0) = covariance_matrix (0, 2);
covariance_matrix (2, 1) = covariance_matrix (1, 2);
}
示例4: getRTGeometricLinearSystem
void PointCloudProcessing::getRTGeometricLinearSystem(PointCloudC::Ptr corrRef,
PointCloudC::Ptr corrNew,
Eigen::Matrix4f &transMat)
{
// build linear system Ax = b;
int rows = 3*corrRef->points.size();
int cols = 6;
Eigen::MatrixXf A(rows, cols); A.setZero();
Eigen::MatrixXf b(rows, 1); b.setZero();
for(int i=0; i<corrRef->points.size(); i++)
{
float X1 = corrRef->points.at(i).x;
float Y1 = corrRef->points.at(i).y;
float Z1 = corrRef->points.at(i).z;
float X0 = corrNew->points.at(i).x;
float Y0 = corrNew->points.at(i).y;
float Z0 = corrNew->points.at(i).z;
A(3*i, 0) = 0; A(3*i, 1) = Z0+Z1; A(3*i, 2) = -Y0-Y1; A(3*i, 3) = 1;
A(3*i+1, 0) = -Z0-Z1; A(3*i+1, 1) = 0; A(3*i+1, 2) = X0+X1; A(3*i+1, 4) = 1;
A(3*i+2, 0) = Y0+Y1; A(3*i+1, 1) = -X0-X1; A(3*i+2, 2) = 0; A(3*i+2, 5) = 1;
b(3*i, 0) = X1-X0;
b(3*i+1, 0) = Y1-Y0;
b(3*i+2, 0) = Z1-Z0;
}
// std::cout<<"A = "<<A<<std::endl;
// std::cout<<"b = "<<b<<std::endl;
Eigen::MatrixXf solveX(rows, 1); solveX.setZero();
solveX = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
// std::cout<<"solveX: "<<solveX<<"\n";
// solveX = [Rx, Ry, Rz, T'x, T'y, T'z];
Eigen::Matrix3f I_Vx; I_Vx.setOnes();
I_Vx(0,0) = 1; I_Vx(0,1) = solveX(2); I_Vx(0,2) = -solveX(1);
I_Vx(1,0) = -solveX(2); I_Vx(1,1) = 1; I_Vx(1,2) = solveX(0);
I_Vx(2,0) = solveX(1); I_Vx(2,1) = -solveX(0); I_Vx(2,2) = 1;
Eigen::Vector3f Tx; Tx.setZero();
Tx(0) = solveX(3); Tx(1) = solveX(4); Tx(2) = solveX(5);
Eigen::Vector3f tx; tx.setZero();
tx = I_Vx.inverse()*Tx;
transMat(0,3) = tx(0); transMat(1,3) = tx(1); transMat(2,3) = tx(2);
Eigen::Matrix3f IplusVx; IplusVx.setOnes();
IplusVx(0,0) = 1; IplusVx(0,1) = -solveX(2); IplusVx(0,2) = solveX(1);
IplusVx(1,0) = solveX(2); IplusVx(1,1) = 1; IplusVx(1,2) = -solveX(0);
IplusVx(2,0) = -solveX(1); IplusVx(2,1) = solveX(0); IplusVx(2,2) = 1;
Eigen::Matrix3f Ro; Ro.setZero();
Ro = I_Vx.inverse()*IplusVx;
transMat(0,0) = Ro(0,0); transMat(0,1) = Ro(0,1); transMat(0,2) = Ro(0,2); transMat(0,3) = tx(0);
transMat(1,0) = Ro(1,0); transMat(1,1) = Ro(1,1); transMat(1,2) = Ro(1,2); transMat(1,3) = tx(1);
transMat(2,0) = Ro(2,0); transMat(2,1) = Ro(2,1); transMat(2,2) = Ro(2,2); transMat(2,3) = tx(2);
// std::cout<<"transMat Geometric: "<<transMat<<"\n";
}
示例5:
void btl::image::CTrackerSimpleFreak::track( const Eigen::Matrix3f& eimHomoInit_,
boost::shared_ptr<cv::gpu::GpuMat> acvgmShrPtrPyrBW_[4],
const cv::Mat& cvmMaskCurr_,
Eigen::Matrix3f* peimHomo_ ){
//from coarse to fine
*peimHomo_ = eimHomoInit_;
cv::gpu::GpuMat cvgmMaskPrev; cvgmMaskPrev.upload(_cvmMaskPrev);
cv::gpu::GpuMat cvgmMaskCurr; cvgmMaskCurr.upload(cvmMaskCurr_);
cv::gpu::GpuMat cvgmBuffer;
for (unsigned int uLevel = _uPyrHeight-1; uLevel >= -1; uLevel--){
for (int n = 0; n < 5; n++){ //iterations
btl::device::cudaFullFrame( *_acgvmShrPtrPyrBWPrev[uLevel], cvgmMaskPrev, *acvgmShrPtrPyrBW_[uLevel], cvgmMaskCurr, peimHomo_->data(), &cvgmBuffer);
//solve out the delta homography
Eigen::Matrix3f eimDeltaHomo; eimDeltaHomo.setZero();
extractHomography(cvgmBuffer,&eimDeltaHomo);
*peimHomo_ += eimDeltaHomo;
}//iterations
}//for pyramid
return;
}
示例6: computeMultilevelShiTomasiScore
/** \brief Computes and sets the multilevel Shi-Tomasi Score \ref s_, considering a defined pyramid level interval.
*
* @param l1 - Start level (l1<l2)
* @param l2 - End level (l1<l2)
*/
void computeMultilevelShiTomasiScore(const int l1 = 0, const int l2 = nLevels_-1) const{
H_.setZero();
int count = 0;
for(int i=l1;i<=l2;i++){
if(isValidPatch_[i]){
H_ += pow(0.25,i)*patches_[i].getHessian();
count++;
}
}
if(count > 0){
const float dXX = H_(0,0)/(count*patchSize*patchSize);
const float dYY = H_(1,1)/(count*patchSize*patchSize);
const float dXY = H_(0,1)/(count*patchSize*patchSize);
e0_ = 0.5 * (dXX + dYY - sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY)));
e1_ = 0.5 * (dXX + dYY + sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY)));
s_ = e0_;
} else {
e0_ = 0;
e1_ = 0;
s_ = -1;
}
}
示例7: computeSensorOffsetAndK
void computeSensorOffsetAndK(Eigen::Isometry3f &sensorOffset, Eigen::Matrix3f &cameraMatrix, ParameterCamera *cameraParam, int reduction) {
sensorOffset = Isometry3f::Identity();
cameraMatrix.setZero();
int cmax = 4;
int rmax = 3;
for (int c=0; c<cmax; c++){
for (int r=0; r<rmax; r++){
sensorOffset.matrix()(r, c) = cameraParam->offset()(r, c);
if (c<3)
cameraMatrix(r,c) = cameraParam->Kcam()(r, c);
}
}
sensorOffset.translation() = Vector3f(0.15f, 0.0f, 0.05f);
Quaternionf quat = Quaternionf(0.5, -0.5, 0.5, -0.5);
sensorOffset.linear() = quat.toRotationMatrix();
sensorOffset.matrix().block<1, 4>(3, 0) << 0.0f, 0.0f, 0.0f, 1.0f;
float scale = 1./reduction;
cameraMatrix *= scale;
cameraMatrix(2,2) = 1;
}
示例8: computeCovarianceMatrix
/**
* ComputeCovarianceMatrix
*/
void ZAdaptiveNormals::computeCovarianceMatrix (const v4r::DataMatrix2D<Eigen::Vector3f> &cloud, const std::vector<int> &indices, const Eigen::Vector3f &mean, Eigen::Matrix3f &cov)
{
Eigen::Vector3f pt;
cov.setZero ();
for (unsigned i = 0; i < indices.size (); ++i)
{
pt = cloud.data[indices[i]] - mean;
cov(1,1) += pt[1] * pt[1];
cov(1,2) += pt[1] * pt[2];
cov(2,2) += pt[2] * pt[2];
pt *= pt[0];
cov(0,0) += pt[0];
cov(0,1) += pt[1];
cov(0,2) += pt[2];
}
cov(1,0) = cov(0,1);
cov(2,0) = cov(0,2);
cov(2,1) = cov(1,2);
}
示例9: computeCovarianceMatrix
/**
* computeCovarianceMatrix
*/
void PlaneEstimationRANSAC::computeCovarianceMatrix (const std::vector<Eigen::Vector3f> &pts, const std::vector<int> &indices, const Eigen::Vector3f &mean, Eigen::Matrix3f &cov)
{
Eigen::Vector3f pt;
cov.setZero ();
for (unsigned i = 0; i < indices.size (); ++i)
{
pt = pts[indices[i]] - mean;
cov(1,1) += pt[1] * pt[1];
cov(1,2) += pt[1] * pt[2];
cov(2,2) += pt[2] * pt[2];
pt *= pt[0];
cov(0,0) += pt[0];
cov(0,1) += pt[1];
cov(0,2) += pt[2];
}
cov(1,0) = cov(0,1);
cov(2,0) = cov(0,2);
cov(2,1) = cov(1,2);
}
示例10: load
void ViewerState::load(const std::string& filename){
clear();
listWidget->clear();
graph->clear();
graph->load(filename.c_str());
vector<int> vertexIds(graph->vertices().size());
int k=0;
for (OptimizableGraph::VertexIDMap::iterator it = graph->vertices().begin(); it != graph->vertices().end(); ++it) {
vertexIds[k++] = (it->first);
}
sort(vertexIds.begin(), vertexIds.end());
listAssociations.clear();
size_t maxCount = 20000;
for(size_t i = 0; i < vertexIds.size() && i< maxCount; ++i) {
OptimizableGraph::Vertex* _v = graph->vertex(vertexIds[i]);
g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(_v);
if (! v)
continue;
OptimizableGraph::Data* d = v->userData();
while(d) {
RGBDData* rgbdData = dynamic_cast<RGBDData*>(d);
if (!rgbdData){
d=d->next();
continue;
}
// retrieve from the rgb data the index of the parameter
int paramIndex = rgbdData->paramIndex();
// retrieve from the graph the parameter given the index
g2o::Parameter* _cameraParam = graph->parameter(paramIndex);
// attempt a cast to a parameter camera
ParameterCamera* cameraParam = dynamic_cast<ParameterCamera*>(_cameraParam);
if (! cameraParam){
cerr << "shall thou be damned forever" << endl;
return;
}
// yayyy we got the parameter
Eigen::Matrix3f cameraMatrix;
Eigen::Isometry3f sensorOffset;
cameraMatrix.setZero();
int cmax = 4;
int rmax = 3;
for (int c=0; c<cmax; c++){
for (int r=0; r<rmax; r++){
sensorOffset.matrix()(r,c)= cameraParam->offset()(r, c);
if (c<3)
cameraMatrix(r,c) = cameraParam->Kcam()(r, c);
}
}
char buf[1024];
sprintf(buf,"%d",v->id());
QString listItem(buf);
listAssociations.push_back(rgbdData);
listWidget->addItem(listItem);
d=d->next();
}
}
}
示例11: mergeable_planes
//.........这里部分代码省略.........
new_inlier_indices.push_back(merged_indices);
}
model_coefficients = new_model_coefficients;
inlier_indices = new_inlier_indices;
}
for(size_t i=0; i < model_coefficients.size(); i++)
{
PlaneModel<PointT> pm;
pm.coefficients_ = model_coefficients[i];
pm.cloud_ = input_;
pm.inliers_ = inlier_indices[i];
//recompute coefficients based on distance to camera and normal?
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*pm.cloud_, pm.inliers_, centroid);
Eigen::Vector3f c(centroid[0],centroid[1],centroid[2]);
Eigen::MatrixXf M_w(inlier_indices[i].indices.size(), 3);
float sum_w = 0.f;
for(size_t k=0; k < inlier_indices[i].indices.size(); k++)
{
const int &idx = inlier_indices[i].indices[k];
float d_c = (pm.cloud_->points[idx].getVector3fMap()).norm();
float w_k = std::max(1.f - std::abs(1.f - d_c), 0.f);
//w_k = 1.f;
M_w.row(k) = w_k * (pm.cloud_->points[idx].getVector3fMap() - c);
sum_w += w_k;
}
Eigen::Matrix3f scatter;
scatter.setZero ();
scatter = M_w.transpose() * M_w;
Eigen::JacobiSVD<Eigen::MatrixXf> svd(scatter, Eigen::ComputeFullV);
//std::cout << svd.matrixV() << std::endl;
Eigen::Vector3f n = svd.matrixV().col(2);
//flip normal if required
if(n.dot(c*-1) < 0)
n = n * -1.f;
float d = n.dot(c) * -1.f;
//std::cout << "normal:" << n << std::endl;
//std::cout << "d:" << d << std::endl;
pm.coefficients_.values[0] = n[0];
pm.coefficients_.values[1] = n[1];
pm.coefficients_.values[2] = n[2];
pm.coefficients_.values[3] = d;
pcl::PointIndices clean_inlier_indices;
float dist_threshold_ = 0.01f;
for(size_t k=0; k < inlier_indices[i].indices.size(); k++)
{
const int &idx = inlier_indices[i].indices[k];
Eigen::Vector3f p = pm.cloud_->points[idx].getVector3fMap();
float val = n.dot(p) + d;
if(std::abs(val) <= dist_threshold_)
clean_inlier_indices.indices.push_back( idx );
}
示例12: transformPC
template<typename PointInT, typename PointNT, typename PointOutT> bool
pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::sgurf (Eigen::Vector3f & centroid, Eigen::Vector3f & normal_centroid,
PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
PointInTPtr & grid, pcl::PointIndices & indices)
{
Eigen::Vector3f plane_normal;
plane_normal[0] = -centroid[0];
plane_normal[1] = -centroid[1];
plane_normal[2] = -centroid[2];
Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
plane_normal.normalize ();
Eigen::Vector3f axis = plane_normal.cross (z_vector);
double rotation = -asin (axis.norm ());
axis.normalize ();
Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis));
grid->points.resize (processed->points.size ());
for (size_t k = 0; k < processed->points.size (); k++)
grid->points[k].getVector4fMap () = processed->points[k].getVector4fMap ();
pcl::transformPointCloud (*grid, *grid, transformPC);
Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0);
Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);
centroid4f = transformPC * centroid4f;
normal_centroid4f = transformPC * normal_centroid4f;
Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]);
Eigen::Vector4f farthest_away;
pcl::getMaxDistance (*grid, indices.indices, centroid4f, farthest_away);
farthest_away[3] = 0;
float max_dist = (farthest_away - centroid4f).norm ();
pcl::demeanPointCloud (*grid, centroid4f, *grid);
Eigen::Matrix4f center_mat;
center_mat.setIdentity (4, 4);
center_mat (0, 3) = -centroid4f[0];
center_mat (1, 3) = -centroid4f[1];
center_mat (2, 3) = -centroid4f[2];
Eigen::Matrix3f scatter;
scatter.setZero ();
float sum_w = 0.f;
//for (int k = 0; k < static_cast<intgrid->points[k].getVector3fMap ();> (grid->points.size ()); k++)
for (int k = 0; k < static_cast<int> (indices.indices.size ()); k++)
{
Eigen::Vector3f pvector = grid->points[indices.indices[k]].getVector3fMap ();
float d_k = (pvector).norm ();
float w = (max_dist - d_k);
Eigen::Vector3f diff = (pvector);
Eigen::Matrix3f mat = diff * diff.transpose ();
scatter = scatter + mat * w;
sum_w += w;
}
scatter /= sum_w;
Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
Eigen::Vector3f evx = svd.matrixV ().col (0);
Eigen::Vector3f evy = svd.matrixV ().col (1);
Eigen::Vector3f evz = svd.matrixV ().col (2);
Eigen::Vector3f evxminus = evx * -1;
Eigen::Vector3f evyminus = evy * -1;
Eigen::Vector3f evzminus = evz * -1;
float s_xplus, s_xminus, s_yplus, s_yminus;
s_xplus = s_xminus = s_yplus = s_yminus = 0.f;
//disambiguate rf using all points
for (int k = 0; k < static_cast<int> (grid->points.size ()); k++)
{
Eigen::Vector3f pvector = grid->points[k].getVector3fMap ();
float dist_x, dist_y;
dist_x = std::abs (evx.dot (pvector));
dist_y = std::abs (evy.dot (pvector));
if ((pvector).dot (evx) >= 0)
s_xplus += dist_x;
else
s_xminus += dist_x;
if ((pvector).dot (evy) >= 0)
s_yplus += dist_y;
else
s_yminus += dist_y;
}
if (s_xplus < s_xminus)
evx = evxminus;
if (s_yplus < s_yminus)
evy = evyminus;
//.........这里部分代码省略.........
示例13: return
template <typename PointT> inline unsigned
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
const Eigen::Vector4f ¢roid,
Eigen::Matrix3f &covariance_matrix)
{
if (cloud.points.empty ())
return (0);
// Initialize to 0
covariance_matrix.setZero ();
unsigned point_count;
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
point_count = static_cast<unsigned> (cloud.size ());
// For each point in the cloud
for (size_t i = 0; i < point_count; ++i)
{
Eigen::Vector4f pt = cloud [i].getVector4fMap () - centroid;
covariance_matrix (1, 1) += pt.y () * pt.y ();
covariance_matrix (1, 2) += pt.y () * pt.z ();
covariance_matrix (2, 2) += pt.z () * pt.z ();
pt *= pt.x ();
covariance_matrix (0, 0) += pt.x ();
covariance_matrix (0, 1) += pt.y ();
covariance_matrix (0, 2) += pt.z ();
}
}
// NaN or Inf values could exist => check for them
else
{
point_count = 0;
// For each point in the cloud
for (size_t i = 0; i < cloud.size (); ++i)
{
// Check if the point is invalid
if (!isFinite (cloud [i]))
continue;
Eigen::Vector4f pt = cloud [i].getVector4fMap () - centroid;
covariance_matrix (1, 1) += pt.y () * pt.y ();
covariance_matrix (1, 2) += pt.y () * pt.z ();
covariance_matrix (2, 2) += pt.z () * pt.z ();
pt *= pt.x ();
covariance_matrix (0, 0) += pt.x ();
covariance_matrix (0, 1) += pt.y ();
covariance_matrix (0, 2) += pt.z ();
++point_count;
}
}
covariance_matrix (1, 0) = covariance_matrix (0, 1);
covariance_matrix (2, 0) = covariance_matrix (0, 2);
covariance_matrix (2, 1) = covariance_matrix (1, 2);
return (point_count);
}