/** \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;
}
}
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();
}
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);
}
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";
}
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;
}
/** \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;
}
}
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;
}
/**
* 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);
}
/**
* 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);
}
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();
}
}
}
void
v4r::MultiPlaneSegmentation<PointT>::segment(bool force_unorganized)
{
models_.clear();
if(input_->isOrganized() && !force_unorganized)
{
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
if(!normals_set_)
{
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.02f);
ne.setNormalSmoothingSize (20.0f);
ne.setBorderPolicy (pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::BORDER_POLICY_IGNORE);
ne.setInputCloud (input_);
ne.compute (*normal_cloud);
}
else
{
normal_cloud = normal_cloud_;
}
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
mps.setMinInliers (min_plane_inliers_);
mps.setAngularThreshold (0.017453 * 2.f); // 2 degrees
mps.setDistanceThreshold (0.01); // 1cm
mps.setMaximumCurvature(0.002);
mps.setInputNormals (normal_cloud);
mps.setInputCloud (input_);
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
std::vector<pcl::ModelCoefficients> model_coefficients;
std::vector<pcl::PointIndices> inlier_indices;
pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
std::vector<pcl::PointIndices> label_indices;
std::vector<pcl::PointIndices> boundary_indices;
typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr ref_comp (
new pcl::PlaneRefinementComparator<PointT,
pcl::Normal, pcl::Label> ());
ref_comp->setDistanceThreshold (0.01f, false);
ref_comp->setAngularThreshold (0.017453 * 2.f);
mps.setRefinementComparator (ref_comp);
mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
//mps.segment (model_coefficients, inlier_indices);
//std::cout << model_coefficients.size() << std::endl;
if(merge_planes_)
{
//sort planes by size
//check if the first plane can be merged against the others, if yes, define a new plane combining both and add it to the cue
typedef boost::adjacency_matrix<boost::undirectedS, int> GraphPlane;
GraphPlane mergeable_planes (model_coefficients.size ());
for(size_t i=0; i < model_coefficients.size(); i++)
{
Eigen::Vector3f plane_i = Eigen::Vector3f (model_coefficients[i].values[0], model_coefficients[i].values[1],
model_coefficients[i].values[2]);
plane_i.normalize();
for(size_t j=(i+1); j < model_coefficients.size(); j++)
{
Eigen::Vector3f plane_j = Eigen::Vector3f (model_coefficients[j].values[0], model_coefficients[j].values[1],
model_coefficients[j].values[2]);
plane_j.normalize();
//std::cout << "dot product:" << plane_i.dot(plane_j) << " diff:" << std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) << std::endl;
if(plane_i.dot(plane_j) > 0.95)
{
if(std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) < 0.015)
{
boost::add_edge (static_cast<int>(i), static_cast<int>(j), mergeable_planes);
}
}
}
}
boost::vector_property_map<int> components (boost::num_vertices (mergeable_planes));
int n_cc = static_cast<int> (boost::connected_components (mergeable_planes, &components[0]));
std::vector<int> cc_sizes;
std::vector< std::vector<int> > cc_to_model_coeff;
cc_sizes.resize (n_cc, 0);
cc_to_model_coeff.resize(n_cc);
for (size_t i = 0; i < model_coefficients.size (); i++)
{
cc_sizes[components[i]]++;
cc_to_model_coeff[components[i]].push_back(i);
}
std::vector<pcl::ModelCoefficients> new_model_coefficients;
std::vector<pcl::PointIndices> new_inlier_indices;
for(size_t i=0; i < cc_sizes.size(); i++)
{
if(cc_sizes[i] < 2)
{
new_model_coefficients.push_back(model_coefficients[cc_to_model_coeff[i][0]]);
new_inlier_indices.push_back(inlier_indices[cc_to_model_coeff[i][0]]);
continue;
}
//std::cout << "going to merge CC:" << cc_sizes[i] << std::endl;
pcl::ModelCoefficients model_coeff;
model_coeff.values.resize(4);
for(size_t k=0; k < 4; k++)
model_coeff.values[k] = 0.f;
pcl::PointIndices merged_indices;
for(size_t j=0; j < cc_to_model_coeff[i].size(); j++)
{
for(size_t k=0; k < 4; k++)
model_coeff.values[k] += model_coefficients[cc_to_model_coeff[i][j]].values[k];
merged_indices.indices.insert(merged_indices.indices.end(), inlier_indices[cc_to_model_coeff[i][j]].indices.begin(),
inlier_indices[cc_to_model_coeff[i][j]].indices.end());
}
for(size_t k=0; k < 4; k++)
model_coeff.values[k] /= static_cast<float>(cc_to_model_coeff[i].size());
new_model_coefficients.push_back(model_coeff);
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 );
}
pm.inliers_ = clean_inlier_indices;
models_.push_back(pm);
}
}
else
{
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<PointT> vg;
PointTCloudPtr cloud_filtered (new PointTCloud);
vg.setInputCloud (input_);
float leaf_size_ = 0.005f;
float dist_threshold_ = 0.01f;
vg.setLeafSize (leaf_size_, leaf_size_, leaf_size_);
vg.filter (*cloud_filtered);
std::vector<bool> pixel_has_not_been_labelled( cloud_filtered->points.size(), true );
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<PointT> seg;
pcl::ModelCoefficients coefficients;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (dist_threshold_);
PointTCloudPtr cloud_filtered_leftover (new PointTCloud (*cloud_filtered));
while (1)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered_leftover);
pcl::PointIndices inliers_in_leftover;
seg.segment (inliers_in_leftover, coefficients);
std::cout << "inliers in left over: " << inliers_in_leftover.indices.size() << " of " << cloud_filtered_leftover->points.size() << std::endl;
if ( (int)inliers_in_leftover.indices.size () < min_plane_inliers_) // Could not estimate a(nother) planar model big enough for the given cloud.
break;
// make indices correspond to complete downsampled cloud
pcl::PointIndices indices_in_original_cloud;
size_t current_index_in_leftover = 0;
size_t px=0;
indices_in_original_cloud.indices.resize(inliers_in_leftover.indices.size());
for(size_t inl_id=0; inl_id < inliers_in_leftover.indices.size(); inl_id++) { // assumes indices are sorted in ascending order
bool found = false;
do {
if( pixel_has_not_been_labelled[px] ) {
if( current_index_in_leftover == inliers_in_leftover.indices [inl_id] ) {
indices_in_original_cloud.indices[ inl_id ] = px;
found = true;
}
current_index_in_leftover++;
}
px++;
} while( !found );
}
for(size_t i=0; i<indices_in_original_cloud.indices.size(); i++)
pixel_has_not_been_labelled[ indices_in_original_cloud.indices[i] ] = false;
//save coefficients
PlaneModel<PointT> pm;
pm.coefficients_ = coefficients;
pm.cloud_ = cloud_filtered;
pm.inliers_ = indices_in_original_cloud;
models_.push_back(pm);
pcl::copyPointCloud(*cloud_filtered, pixel_has_not_been_labelled, *cloud_filtered_leftover);
}
std::cout << "Number of planes found:" << models_.size() << "organized:" << static_cast<int>(input_->isOrganized() && !force_unorganized) << std::endl;
}
}
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;
//select the axis that could be disambiguated more easily
float fx, fy;
float max_x = static_cast<float> (std::max (s_xplus, s_xminus));
float min_x = static_cast<float> (std::min (s_xplus, s_xminus));
float max_y = static_cast<float> (std::max (s_yplus, s_yminus));
float min_y = static_cast<float> (std::min (s_yplus, s_yminus));
fx = (min_x / max_x);
fy = (min_y / max_y);
Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
if (normal3f.dot (evz) < 0)
evz = evzminus;
//if fx/y close to 1, it was hard to disambiguate
//what if both are equally easy or difficult to disambiguate, namely fy == fx or very close
float max_axis = std::max (fx, fy);
float min_axis = std::min (fx, fy);
if ((min_axis / max_axis) > axis_ratio_)
{
PCL_WARN("Both axis are equally easy/difficult to disambiguate\n");
Eigen::Vector3f evy_copy = evy;
Eigen::Vector3f evxminus = evx * -1;
Eigen::Vector3f evyminus = evy * -1;
if (min_axis > min_axis_value_)
{
//combination of all possibilities
evy = evx.cross (evz);
Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
evx = evxminus;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
evx = evy_copy;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
evx = evyminus;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
}
else
{
//1-st case (evx selected)
evy = evx.cross (evz);
Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
//2-nd case (evy selected)
evx = evy_copy;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
}
}
else
{
if (fy < fx)
{
evx = evy;
fx = fy;
}
evy = evx.cross (evz);
Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
}
return true;
}
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);
}
