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";
}
/** \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;
}
}
Eigen::Vector3f RobotNodeSet::getCoM()
{
Eigen::Vector3f res;
res.setZero();
float m = getMass();
if (m<=0)
return res;
for (size_t i=0;i<this->robotNodes.size();i++)
res += robotNodes[i]->getCoMGlobal() * robotNodes[i]->getMass() / m;
return res;
}
/**
* ComputeCentroid
*/
void RigidTransformationRANSAC::ComputeCentroid(
const std::vector<Eigen::Vector3f > &pts,
const std::vector<int> &indices,
Eigen::Vector3f ¢roid)
{
// Initialize to 0
centroid.setZero ();
if (pts.size()==0 || indices.size()==0)
return;
for (unsigned i = 0; i < indices.size (); i++)
centroid += pts[indices[i]];
centroid /= indices.size ();
}
int PatternDetector::detectPattern(cv::Mat& image_in, Eigen::Vector3f& translation, Eigen::Quaternionf& orientation,
cv::Mat& image_out)
{
translation.setZero();
orientation.setIdentity();
bool found = false;
observation_pts_t observation_points;
switch (pattern_type)
{
case ASYMMETRIC_CIRCLES_GRID:
found = cv::findCirclesGrid(image_in, grid_size, observation_points,
cv::CALIB_CB_ASYMMETRIC_GRID | cv::CALIB_CB_CLUSTERING);
break;
case CHESSBOARD:
found = cv::findChessboardCorners(image_in, grid_size, observation_points, cv::CALIB_CB_ADAPTIVE_THRESH);
break;
case CIRCLES_GRID:
found = cv::findCirclesGrid(image_in, grid_size, observation_points, cv::CALIB_CB_SYMMETRIC_GRID);
break;
}
if (found)
{
// Do subpixel ONLY IF THE PATTERN IS A CHESSBOARD
if (pattern_type == CHESSBOARD)
{
cv::cornerSubPix(image_in, observation_points, cv::Size(5, 5), cv::Size(-1, -1),
cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 100, 0.01));
}
cv::solvePnP(cv::Mat(ideal_points), cv::Mat(observation_points), K, D, rvec, tvec, false);
cv::Rodrigues(rvec, R); //take the 3x1 rotation representation to a 3x3 rotation matrix.
cv::drawChessboardCorners(image_out, grid_size, cv::Mat(observation_points), found);
convertCVtoEigen(tvec, R, translation, orientation);
}
return found;
}
/**
* ComputeNormal
*/
float ZAdaptiveNormals::computeNormal(const v4r::DataMatrix2D<Eigen::Vector3f> &cloud, std::vector<int> &indices, Eigen::Matrix3f &eigen_vectors)
{
if (indices.size()<4)
return NaN;
Eigen::Vector3f mean;
mean.setZero();
for (unsigned j=0; j<indices.size(); j++)
mean += cloud.data[indices[j]];
mean /= (float)indices.size();
Eigen::Matrix3f cov;
computeCovarianceMatrix (cloud, indices, mean, cov);
Eigen::Vector3f eigen_values;
v4r::eigen33 (cov, eigen_vectors, eigen_values);
float eigsum = eigen_values.sum();
if (eigsum != 0)
return fabs (eigen_values[0] / eigsum );
return NaN;
}
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::planeFitting (
Eigen::Matrix<float,
Eigen::Dynamic, 3> const &points,
Eigen::Vector3f ¢er,
Eigen::Vector3f &norm)
{
// -----------------------------------------------------
// Plane Fitting using Singular Value Decomposition (SVD)
// -----------------------------------------------------
int n_points = static_cast<int> (points.rows ());
if (n_points == 0)
{
return;
}
//find the center by averaging the points positions
center.setZero ();
for (int i = 0; i < n_points; ++i)
{
center += points.row (i);
}
center /= static_cast<float> (n_points);
//copy points - average (center)
Eigen::Matrix<float, Eigen::Dynamic, 3> A (n_points, 3); //PointData
for (int i = 0; i < n_points; ++i)
{
A (i, 0) = points (i, 0) - center.x ();
A (i, 1) = points (i, 1) - center.y ();
A (i, 2) = points (i, 2) - center.z ();
}
Eigen::JacobiSVD<Eigen::MatrixXf> svd (A, Eigen::ComputeFullV);
norm = svd.matrixV ().col (2);
}
/**
* estimatePlaneLS
* least squares estimation of a plan using points defined by indices
*/
void PlaneEstimationRANSAC::estimatePlaneLS(const std::vector<Eigen::Vector3f> &pts, const std::vector<int> &indices, Eigen::Vector3f &pt, Eigen::Vector3f &n)
{
Eigen::Vector3f mean;
EIGEN_ALIGN16 Eigen::Matrix3f cov;
mean.setZero();
for (unsigned j=0; j<indices.size(); j++)
mean += pts[indices[j]];
mean /= (float)indices.size();
computeCovarianceMatrix(pts, indices, mean, cov);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> sv(cov);
Eigen::Matrix3f eigen_vectors = sv.eigenvectors();
n[0] = eigen_vectors(0,0);
n[1] = eigen_vectors(1,0);
n[2] = eigen_vectors(2,0);
}
Eigen::Vector3f& MyPointCloud::getCentroid() {
DeviceArray2D<pcl::PointXYZ> cloudDevice;
pcl::PointCloud<PointXYZ>::Ptr hostFrameCloud;
this->getLastFrameCloud(cloudDevice);
int c;
hostFrameCloud = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ>);
cloudDevice.download (hostFrameCloud->points, c);
hostFrameCloud->width = cloudDevice.cols ();
hostFrameCloud->height = cloudDevice.rows ();
Eigen::Vector3f centroid;
centroid.setZero();
int count = 0;
for(int point = 0; point < hostFrameCloud->points.size(); point++) {
if(hostFrameCloud->points[point].z == hostFrameCloud->points[point].z) {
if(hostFrameCloud->points[point].z != 0) {
centroid(0) += hostFrameCloud->points[point].x;
centroid(1) += hostFrameCloud->points[point].y;
centroid(2) += hostFrameCloud->points[point].z;
count++;
}
}
}
centroid /= count;
return centroid;
}
void PointWithNormalStatistcsGenerator::computeNormalsAndSVD(PointWithNormalVector& points, PointWithNormalSVDVector& svds, const Eigen::MatrixXi& indices,
const Eigen::Matrix3f& cameraMatrix, const Eigen::Isometry3f& transform){
_integralImage.compute(indices,points);
int q=0;
int outerStep = _numThreads * _step;
PixelMapper pixelMapper;
pixelMapper.setCameraMatrix(cameraMatrix);
pixelMapper.setTransform(transform);
Eigen::Isometry3f inverseTransform = transform.inverse();
#pragma omp parallel
{
#ifdef _PWN_USE_OPENMP_
int threadNum = omp_get_thread_num();
#else // _PWN_USE_OPENMP_
int threadNum = 0;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver;
#endif // _PWN_USE_OPENMP_
for (int c=threadNum; c<indices.cols(); c+=outerStep) {
#ifdef _PWN_USE_OPENMP_
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver;
#endif // _PWN_USE_OPENMP_
for (int r=0; r<indices.rows(); r+=_step, q++){
int index = indices(r,c);
//cerr << "index(" << r <<"," << c << ")=" << index << endl;
if (index<0)
continue;
// determine the region
PointWithNormal& point = points[index];
PointWithNormalSVD& originalSVD = svds[index];
PointWithNormalSVD svd;
Eigen::Vector3f normal = point.normal();
Eigen::Vector3f coord = pixelMapper.projectPoint(point.point()+Eigen::Vector3f(_worldRadius, _worldRadius, 0));
svd._z=point(2);
coord.head<2>()*=(1./coord(2));
int dx = abs(c - coord[0]);
int dy = abs(r - coord[1]);
if (dx>_imageRadius)
dx = _imageRadius;
if (dy>_imageRadius)
dy = _imageRadius;
PointAccumulator acc = _integralImage.getRegion(c-dx, c+dx, r-dy, r+dy);
svd._mean=point.point();
if (acc.n()>_minPoints){
Eigen::Vector3f mean = acc.mean();
svd._mean = mean;
svd._n = acc.n();
Eigen::Matrix3f cov = acc.covariance();
eigenSolver.compute(cov);
svd._U=eigenSolver.eigenvectors();
svd._singularValues=eigenSolver.eigenvalues();
if (svd._singularValues(0) <0)
svd._singularValues(0) = 0;
/*
cerr << "\t svd.singularValues():" << svd.singularValues() << endl;
cerr << "\t svd.U():" << endl << svd.U() << endl;
//cerr << "\t svd.curvature():" << svd.curvature() << endl;
*/
normal = eigenSolver.eigenvectors().col(0).normalized();
if (normal.dot(inverseTransform * mean) > 0.0f)
normal =-normal;
svd.updateCurvature();
//cerr << "n(" << index << ") c:" << svd.curvature() << endl << point.tail<3>() << endl;
if (svd.curvature()>_maxCurvature){
//cerr << "region: " << c-dx << " " << c+dx << " " << r-dx << " " << r+dx << " points: " << acc.n() << endl;
normal.setZero();
}
} else {
normal.setZero();
svd = PointWithNormalSVD();
}
if (svd.n() > originalSVD.n()){
originalSVD = svd;
point.setNormal(normal);
}
}
}
}
}
template <typename PointInT, typename PointNT, typename PointOutT, typename IntensitySelectorT> void
pcl::IntensityGradientEstimation<PointInT, PointNT, PointOutT, IntensitySelectorT>::computeFeature (PointCloudOut &output)
{
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
output.is_dense = true;
// If the data is dense, we don't need to check for NaN
if (surface_->is_dense)
{
#ifdef _OPENMP
#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
#endif
// Iterating over the entire index vector
for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
{
PointOutT &p_out = output.points[idx];
if (!this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
{
p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
Eigen::Vector3f centroid;
float mean_intensity = 0;
// Initialize to 0
centroid.setZero ();
for (size_t i = 0; i < nn_indices.size (); ++i)
{
centroid += surface_->points[nn_indices[i]].getVector3fMap ();
mean_intensity += intensity_ (surface_->points[nn_indices[i]]);
}
centroid /= static_cast<float> (nn_indices.size ());
mean_intensity /= static_cast<float> (nn_indices.size ());
Eigen::Vector3f normal = Eigen::Vector3f::Map (normals_->points[(*indices_) [idx]].normal);
Eigen::Vector3f gradient;
computePointIntensityGradient (*surface_, nn_indices, centroid, mean_intensity, normal, gradient);
p_out.gradient[0] = gradient[0];
p_out.gradient[1] = gradient[1];
p_out.gradient[2] = gradient[2];
}
}
else
{
#ifdef _OPENMP
#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
#endif
// Iterating over the entire index vector
for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
{
PointOutT &p_out = output.points[idx];
if (!isFinite ((*surface_) [(*indices_)[idx]]) ||
!this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
{
p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
Eigen::Vector3f centroid;
float mean_intensity = 0;
// Initialize to 0
centroid.setZero ();
unsigned cp = 0;
for (size_t i = 0; i < nn_indices.size (); ++i)
{
// Check if the point is invalid
if (!isFinite ((*surface_) [nn_indices[i]]))
continue;
centroid += surface_->points [nn_indices[i]].getVector3fMap ();
mean_intensity += intensity_ (surface_->points [nn_indices[i]]);
++cp;
}
centroid /= static_cast<float> (cp);
mean_intensity /= static_cast<float> (cp);
Eigen::Vector3f normal = Eigen::Vector3f::Map (normals_->points[(*indices_) [idx]].normal);
Eigen::Vector3f gradient;
computePointIntensityGradient (*surface_, nn_indices, centroid, mean_intensity, normal, gradient);
p_out.gradient[0] = gradient[0];
p_out.gradient[1] = gradient[1];
p_out.gradient[2] = gradient[2];
}
}
}
bool PositionBasedElasticRod::ProjectEdgeConstraints(
const Eigen::Vector3f& pA, const float wA,
const Eigen::Vector3f& pB, const float wB,
const Eigen::Vector3f& pG, const float wG,
const float edgeKs, const float edgeRestLength, const float ghostEdgeRestLength,
Eigen::Vector3f& corrA, Eigen::Vector3f& corrB, Eigen::Vector3f& corrC)
{
corrA.setZero(); corrB.setZero(); corrC.setZero();
//Edge distance constraint
Eigen::Vector3f dir = pA - pB;
float len = dir.norm();
float wSum = wA + wB;
if (len > EPSILON && wSum > EPSILON)
{
Eigen::Vector3f dP = (1.0f / wSum) * (len - edgeRestLength) * (dir / len) * edgeKs;
corrA -= dP * wA;
corrB += dP * wB;
corrC = Eigen::Vector3f(0, 0, 0);
}
//Bisector constraint
Eigen::Vector3f pm = 0.5f * (pA + pB);
Eigen::Vector3f p0p2 = pA - pG;
Eigen::Vector3f p2p1 = pG - pB;
Eigen::Vector3f p1p0 = pB - pA;
Eigen::Vector3f p2pm = pG - pm;
float lambda;
wSum = wA * p0p2.squaredNorm() + wB * p2p1.squaredNorm() + wG * p1p0.squaredNorm();
if (wSum > EPSILON)
{
lambda = p2pm.dot(p1p0) / wSum * edgeKs;
corrA -= p0p2 * lambda * wA;
corrB -= p2p1 * lambda * wB;
corrC -= p1p0 * lambda * wG;
}
////Ghost-Edge constraint
wSum = 0.25f * wA + 0.25f * wB + 1.0f * wG;
if (wSum > EPSILON)
{
//need to use updated positions
pm = 0.5f * (pA + corrA + pB + corrB);
p2pm = pG + corrC - pm;
float p2pm_mag = p2pm.norm();
p2pm *= 1.0f / p2pm_mag;
lambda = (p2pm_mag - ghostEdgeRestLength) / wSum * edgeKs;
corrA += 0.5f * wA * lambda * p2pm;
corrB += 0.5f * wB * lambda * p2pm;
corrC -= 1.0f * wG * lambda * p2pm;
}
return true;
}