/**
* Convert covariance matrix @param in to correlation matrix, and
* put lower triangle to @param out
*/
int cov2cor(const Eigen::MatrixXf& in, std::vector<double>* out) {
int n = in.rows();
if ( n != in.cols()) return -1;
if (n == 0) return -1;
(*out).resize(n * (n-1)/2);
int k = 0;
for (int i = 0 ; i < n; ++i) {
for (int j = 0; j < i; ++j){
(*out)[k++] = in(i, j ) / sqrt(in(i,i) * in(j, j));
}
}
return 0;
}
/**
* rescale this->upper, this->lower, and set this->cor
*/
int makeCov(const Eigen::MatrixXf cov) {
if (cov.rows() != cov.cols()) return -1;
if (cov.rows() != (int) upper.size()) return -1;
if (cov.rows() != (int) lower.size()) return -1;
for (size_t i = 0; i < upper.size(); ++i) {
upper[i] /= sqrt(cov(i, i));
lower[i] /= sqrt(cov(i, i));
}
cov2cor(cov, &this->cor);
return 0;
}
// Compute the KL-divergence of a set of marginals
double DenseCRF::klDivergence( const Eigen::MatrixXf & Q ) const {
double kl = 0;
// Add the entropy term
for( int i=0; i<Q.cols(); i++ )
for( int l=0; l<Q.rows(); l++ )
kl += Q(l,i)*log(std::max( Q(l,i), 1e-20f) );
// Add the unary term
if( unary_ ) {
Eigen::MatrixXf unary = unary_->get();
for( int i=0; i<Q.cols(); i++ )
for( int l=0; l<Q.rows(); l++ )
kl += unary(l,i)*Q(l,i);
}
// Add all pairwise terms
Eigen::MatrixXf tmp;
for( unsigned int k=0; k<pairwise_.size(); k++ ) {
pairwise_[k]->apply( tmp, Q );
kl += (Q.array()*tmp.array()).sum();
}
return kl;
}
int Conversion::convert(const Eigen::MatrixXf & depthMat, vpImage<float>&dmap)
{
int height = depthMat.rows();
int width = depthMat.cols();
dmap.resize(height, width);
for(int i = 0 ; i< height ; i++){
for(int j=0 ; j< width ; j++){
dmap[i][j]=depthMat(i,j);
}
}
return 1;
}
Eigen::MatrixXf safeResize(Eigen::MatrixXf A, int nRows, int nCols) {
Eigen::MatrixXf B(nRows,nCols);
for (int i = 0; i < nRows; i++) {
for (int j = 0; j < nCols; j++) {
if (i < A.rows() && j < A.cols()) {
B(i,j) = A(i,j);
} else {
B(i,j) = std::sqrt(-1);
}
}
}
return B;
}
void LaserscanMerger::pointcloud_to_laserscan(Eigen::MatrixXf points, pcl::PCLPointCloud2 *merged_cloud)
{
sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
output->header = pcl_conversions::fromPCL(merged_cloud->header);
output->header.frame_id = destination_frame.c_str();
output->header.stamp = ros::Time::now(); //fixes #265
output->angle_min = this->angle_min;
output->angle_max = this->angle_max;
output->angle_increment = this->angle_increment;
output->time_increment = this->time_increment;
output->scan_time = this->scan_time;
output->range_min = this->range_min;
output->range_max = this->range_max;
uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) / output->angle_increment);
output->ranges.assign(ranges_size, output->range_max + 1.0);
for(int i=0; i<points.cols(); i++)
{
const float &x = points(0,i);
const float &y = points(1,i);
const float &z = points(2,i);
if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
{
ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
continue;
}
double range_sq = y*y+x*x;
double range_min_sq_ = output->range_min * output->range_min;
if (range_sq < range_min_sq_) {
ROS_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range_sq, range_min_sq_, x, y, z);
continue;
}
double angle = atan2(y, x);
if (angle < output->angle_min || angle > output->angle_max)
{
ROS_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output->angle_min, output->angle_max);
continue;
}
int index = (angle - output->angle_min) / output->angle_increment;
if (output->ranges[index] * output->ranges[index] > range_sq)
output->ranges[index] = sqrt(range_sq);
}
laser_scan_publisher_.publish(output);
}
TEST(LeastSquaresTransform, Transform) {
Eigen::MatrixXf A(7, 3), B(7,3);
/*
A:
4:
|\ A
|'\
| '\,
| \,
....|____\.............
: 2
:
:
_,-, 2
_,--'-' | B
: ,-/~' |
....:/'...............|.......
: 4
*/
A << 0, 4, 1,
0, 3, 1,
0, 2, 1,
0, 1, 1,
0, 0, 1,
1, 0, 1,
2, 0, 1;
B << 0, 0, 1,
1, 0, 1,
2, 0, 1,
3, 0, 1,
4, 0, 1,
4, 1, 1,
4, 2, 1;
Eigen::MatrixXf T;
LeastSquaresTransform<cv::Point2f> lst;
lst(A, B, T);
EXPECT_EQ(T.cols(), 3);
EXPECT_EQ(T.rows(), 3);
Eigen::MatrixXf TExpected(3,3);
TExpected << 0, 1, 0,
-1, 0, 0,
4, 0, 1;
EXPECT_EQ(TExpected.isApprox(T, 0.0001f), true);
}
bool ZMeshBilateralFilter::apply(const Eigen::MatrixXf& input, const Eigen::VectorXf& weights, const std::vector<bool>& tags)
{
if (pAnnSearch_==NULL)
return false;
if (getRangeDim()+getSpatialDim()!=input.cols())
return false;
int nSize = input.rows();
output_ = input;
pAnnSearch_->setFlags(tags);
float searchRad = filterPara_.spatial_sigma*sqrt(3.0);
for (int i=0; i<nSize; i++)
{
if (!tags[i]) continue;
Eigen::VectorXf v(input.row(i));
Eigen::VectorXi nnIdx;
Eigen::VectorXf nnDists;
// query
Eigen::VectorXf queryV(v.head(spatialDim_));
Eigen::VectorXf rangeV(v.tail(rangeDim_));
int queryNum = pAnnSearch_->queryFRPoint(queryV, searchRad, 0, nnIdx, nnDists);
//int queryNum = queryMeshTool_->query(i, 20, nnIdx, nnDists);
// convolute
Eigen::VectorXf sumRange(rangeDim_);
sumRange.fill(0);
float sumWeight = 0;
for (int j=1; j<queryNum; j++)
{
int idx = nnIdx[j];
if (!tags[idx]) continue;
Eigen::VectorXf rangeU(input.row(idx).tail(rangeDim_));
float distWeight = ZKernelFuncs::GaussianKernelFunc(sqrt(nnDists(j)), filterPara_.spatial_sigma);
// if kernelFuncA_==NULL, then only using spatial filter
float rangeWeidht = kernelFuncA_ ? kernelFuncA_(rangeV, rangeU, filterPara_.range_sigma) : 1.f;
float weight = rangeWeidht*distWeight*weights(idx);
//if (i==1)
// std::cout << rangeU << " * " << distWeight << "*" << rangeWeidht << "*" << weights(idx) << "\n";
sumWeight += weight;
sumRange += rangeU*weight;
}
if (!g_isZero(sumWeight))
output_.row(i).tail(rangeDim_) = sumRange*(1.f/sumWeight);
}
return true;
}
/**
* @function fillEq
*/
void trifocalTensor::fillEq() {
mPointer = 0;
mNumTotalEq = 9*mPPP.size() + 3*mLLL.size() + 3*mPLP.size() + 1*mPLL.size();
printf("Total number of equations (some of them redundant): %d \n", mNumTotalEq );
mEq = Eigen::MatrixXf::Zero( mNumTotalEq, 27 );
mB = Eigen::VectorXf::Zero( mNumTotalEq );
Eigen::FullPivLU<Eigen::MatrixXf> lu(mEq);
printf("* [Beginning] Rank of mEq is: %d \n", lu.rank() );
// Fill Point-Point-Point ( 9 Equations - 4 DOF )
for( int i = 0; i < mPPP.size(); i++ ) {
fillEq_PPP( mPPP[i][0], mPPP[i][1], mPPP[i][2] );
//Eigen::MatrixXf yam = mEq.block(mPointer - 9, 0, 9, mEq.cols() );
Eigen::MatrixXf yam = mEq;
Eigen::FullPivLU<Eigen::MatrixXf> lu(yam);
printf("* [PPP][%d] Rank of mEq(%d x %d) is: %d \n", i, yam.rows(), yam.cols(), lu.rank() );
}
Eigen::FullPivLU<Eigen::MatrixXf> luA(mEq);
printf("* After [PPP] Rank of mEq is: %d \n",luA.rank() );
// Fill Line -Line - Line ( 3 Equations - 2 DOF )
for( int j = 0; j < mLLL.size(); j++ ) {
fillEq_LLL( mLLL[j][0], mLLL[j][1], mLLL[j][2] );
Eigen::FullPivLU<Eigen::MatrixXf> lu(mEq);
printf("* [LLL][%d] Rank of mEq is: %d \n", j, lu.rank() );
}
// Fill Point - Line - Line ( 1 Equations - 1 DOF )
for( int i = 0; i < mPLL.size(); ++i ) {
fillEq_PLL( mPLL[i][0], mPLL[i][1], mPLL[i][2] );
Eigen::FullPivLU<Eigen::MatrixXf> lu(mEq);
printf("* [PLL][%d] Rank of mEq is: %d \n", i, lu.rank() );
}
// Fill Point - Line - Point ( 3 Equations - 2 DOF )
for( int i = 0; i < mPLP.size(); ++i ) {
fillEq_PLP( mPLP[i][0], mPLP[i][1], mPLP[i][2] );
Eigen::FullPivLU<Eigen::MatrixXf> lu(mEq);
printf("* [PLP][%d] Rank of mEq is: %d \n", i, lu.rank() );
}
}
void PointProjector::unProject(HomogeneousPoint3fVector &points,
Eigen::MatrixXi &indexImage,
const Eigen::MatrixXf &depthImage) const {
points.resize(depthImage.rows()*depthImage.cols());
int count = 0;
indexImage.resize(depthImage.rows(), depthImage.cols());
HomogeneousPoint3f* point = &points[0];
int cpix=0;
for (int c=0; c<depthImage.cols(); c++){
const float* f = &depthImage(0,c);
int* i =&indexImage(0,c);
for (int r=0; r<depthImage.rows(); r++, f++, i++){
if (!unProject(*point, r,c,*f)){
*i=-1;
continue;
}
point++;
cpix++;
*i=count;
count++;
}
}
points.resize(count);
}
void DenseCRF::stepInference( Eigen::MatrixXf & Q, Eigen::MatrixXf & tmp1, Eigen::MatrixXf & tmp2 ) const{
tmp1.resize( Q.rows(), Q.cols() );
tmp1.fill(0);
if( unary_ )
tmp1 -= unary_->get();
// Add up all pairwise potentials
for( unsigned int k=0; k<pairwise_.size(); k++ ) {
pairwise_[k]->apply( tmp2, Q );
tmp1 -= tmp2;
}
// Exponentiate and normalize
expAndNormalize( Q, tmp1 );
}
std::vector<Eigen::Vector2f> Random(const Eigen::MatrixXf& density)
{
boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > die(
impl::Rnd(),
boost::uniform_real<float>(0.0f, 1.0f));
std::vector<Eigen::Vector2f> seeds;
for(unsigned int iy=0; iy<density.cols(); iy++) {
for(unsigned int ix=0; ix<density.rows(); ix++) {
if(die() < density(ix,iy))
seeds.push_back(Eigen::Vector2f(ix, iy));
}
}
return seeds;
}
// draw all the lines between aPts and Pts that have a corr>threshold.
// if aPtInd is in the range of aPts, then draw only the lines that comes from aPts[aPtInd]
void drawCorrLines(PlotLines::Ptr lines, const vector<btVector3>& aPts, const vector<btVector3>& bPts, const Eigen::MatrixXf& corr, float threshold, int aPtInd) {
vector<btVector3> linePoints;
vector<btVector4> lineColors;
float max_corr = corr.maxCoeff(); // color lines by corr, where corr has been mapped [threshold,max_corr] -> [0,1]
for (int i=0; i<corr.rows(); ++i)
for (int j=0; j<corr.cols(); ++j)
if (corr(i,j) > threshold) {
if (aPtInd<0 || aPtInd>=corr.rows() || aPtInd==i) {
linePoints.push_back(aPts[i]);
linePoints.push_back(bPts[j]);
float color_factor = (corr(i,j)-threshold)/(max_corr-threshold); //basically, it ranges from 0 to 1
lineColors.push_back(btVector4(color_factor, color_factor,0,1));
}
}
lines->setPoints(linePoints, lineColors);
}
int ComputationGraph::matrix(Eigen::MatrixXf matrix) {
std::vector<int> output;
for (int i = 0; i < matrix.rows(); i++) {
for (int j = 0; j < matrix.cols(); j++) {
output.push_back(*((int *)&matrix(i, j)));
}
}
nodes.push_back({
-1,
{},
output
});
return nodes.size() - 1;
}
void HierarchicalWalkingIK::computeWalkingTrajectory(const Eigen::Matrix3Xf& comTrajectory,
const Eigen::Matrix6Xf& rightFootTrajectory,
const Eigen::Matrix6Xf& leftFootTrajectory,
std::vector<Eigen::Matrix3f>& rootOrientation,
Eigen::MatrixXf& trajectory)
{
int rows = outputNodeSet->getSize();
trajectory.resize(rows, rightFootTrajectory.cols());
rootOrientation.resize(rightFootTrajectory.cols());
Eigen::Vector3f com = colModelNodeSet->getCoM();
Eigen::VectorXf configuration;
int N = trajectory.cols();
Eigen::Matrix4f leftFootPose = Eigen::Matrix4f::Identity();
Eigen::Matrix4f rightFootPose = Eigen::Matrix4f::Identity();
Eigen::Matrix4f chestPose = chest->getGlobalPose();
Eigen::Matrix4f pelvisPose = pelvis->getGlobalPose();
for (int i = 0; i < N; i++)
{
VirtualRobot::MathTools::posrpy2eigen4f(1000 * leftFootTrajectory.block(0, i, 3, 1), leftFootTrajectory.block(3, i, 3, 1), leftFootPose);
VirtualRobot::MathTools::posrpy2eigen4f(1000 * rightFootTrajectory.block(0, i, 3, 1), rightFootTrajectory.block(3, i, 3, 1), rightFootPose);
// FIXME the orientation of the chest and chest is specific to armar 4
// since the x-Axsis points in walking direction
Eigen::Vector3f xAxisChest = (leftFootPose.block(0, 1, 3, 1) + rightFootPose.block(0, 1, 3, 1))/2;
xAxisChest.normalize();
chestPose.block(0, 0, 3, 3) = Bipedal::poseFromXAxis(xAxisChest);
pelvisPose.block(0, 0, 3, 3) = Bipedal::poseFromYAxis(-xAxisChest);
std::cout << "Frame #" << i << ", ";
robot->setGlobalPose(leftFootPose);
computeStepConfiguration(1000 * comTrajectory.col(i),
rightFootPose,
chestPose,
pelvisPose,
configuration);
trajectory.col(i) = configuration;
rootOrientation[i] = leftFootPose.block(0, 0, 3, 3);
}
}
Eigen::MatrixXf JacobiProvider::computePseudoInverseJacobianMatrix(const Eigen::MatrixXf &m) const
{
#ifdef CHECK_PERFORMANCE
clock_t startT = clock();
#endif
MatrixXf pseudo;
switch (inverseMethod)
{
case eTranspose:
{
if (jointWeights.rows() == m.cols())
{
Eigen::MatrixXf W = jointWeights.asDiagonal();
Eigen::MatrixXf W_1 = W.inverse();
pseudo = W_1 * m.transpose() * (m*W_1*m.transpose()).inverse();
}
else
{
pseudo = m.transpose() * (m*m.transpose()).inverse();
}
break;
}
case eSVD:
{
float pinvtoler = 0.00001f;
pseudo = MathTools::getPseudoInverse(m, pinvtoler);
break;
}
case eSVDDamped:
{
float pinvtoler = 0.00001f;
pseudo = MathTools::getPseudoInverseDamped(m,pinvtoler);
break;
}
default:
THROW_VR_EXCEPTION("Inverse Jacobi Method nyi...");
}
#ifdef CHECK_PERFORMANCE
clock_t endT = clock();
float diffClock = (float)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0f);
//if (diffClock>10.0f)
cout << "Inverse Jacobi time:" << diffClock << endl;
#endif
return pseudo;
}
IGL_INLINE void igl::fit_rotations_AVX(
const Eigen::MatrixXf & S,
Eigen::MatrixXf & R)
{
const int cStep = 8;
assert(S.cols() == 3);
const int dim = 3; //S.cols();
const int nr = S.rows()/dim;
assert(nr * dim == S.rows());
// resize output
R.resize(dim,dim*nr); // hopefully no op (should be already allocated)
Eigen::Matrix<float, 3*cStep, 3> siBig;
// using SSE decompose cStep matrices at a time:
int r = 0;
for( ; r<nr; r+=cStep)
{
int numMats = cStep;
if (r + cStep >= nr) numMats = nr - r;
// build siBig:
for (int k=0; k<numMats; k++)
{
for(int i = 0;i<dim;i++)
{
for(int j = 0;j<dim;j++)
{
siBig(i + 3*k, j) = S(i*nr + r + k, j);
}
}
}
Eigen::Matrix<float, 3*cStep, 3> ri;
polar_svd3x3_avx(siBig, ri);
for (int k=0; k<cStep; k++)
assert(ri.block(3*k, 0, 3, 3).determinant() >= 0);
// Not sure why polar_dec computes transpose...
for (int k=0; k<numMats; k++)
{
R.block(0, (r + k)*dim, dim, dim) = ri.block(3*k, 0, dim, dim).transpose();
}
}
}
//uses cauchy as cost function instead of squared error
//observations is a matrix of nx1 where n is the number of landmarks observed
//each value in the matrix represents the angle at which the landmark was observed
//params is a matrix of nx2 where n is the number of landmarks
//for each landmark, the x and y pose of where it is
//pose is a matrix of 2x1 containing the initial guess of the robot pose
//error is a nx1 matrix for the difference between the measurement and the estimated angle
double LMAlgr::computeError(Eigen::MatrixXf observations, Eigen::MatrixXf params, Eigen::MatrixXf pose, Eigen::MatrixXf &error){
Eigen::MatrixXf estimated_angle;
estimated_angle.resize(observations.rows(), observations.cols());
for(int i = 0; i < observations.rows(); i++){
//compute the estimated angle for each landmark
estimated_angle(i, 0) = atan2(params(i, 1) - pose(1, 0), params(i, 0) - pose(0, 0));
//std::cout << params(i, 1) << " " << params(i, 0) << " " << pose(1, 0) << " " << pose(0, 0) << " " << estimated_angle(i, 0) << " " << observations(i, 0) << std::endl;
//compute the error for each landmark
error(i, 0) = atan2(sin(observations(i, 0) - estimated_angle(i, 0)), cos(observations(i, 0) - estimated_angle(i, 0)));//normalize_angle(observations(i, 0) - estimated_angle(i, 0));
}
/*std::cout << "estimated angle: \n" << estimated_angle << std::endl;*/
/*std::cout << "error matrix: \n " << error << std::endl;*/
//std::cout << "final error value:\n" << error << std::endl;
double cost = outlier_threshold * outlier_threshold * log10(1 + (error.squaredNorm() / (outlier_threshold * outlier_threshold)));
double weight = sqrt(cost) / error.norm();
error = weight * error;
return error.squaredNorm();
}
int Conversion::convert(const Eigen::MatrixXf & depthMat,
pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
double &fx,
double &fy,
double &cx,
double &cy)
{
int index(0);
for (int i=0; i<depthMat.rows();i++)
for (int j=0 ; j<depthMat.cols();j++)
{
if (fabs(depthMat(i,j) + 1.f) > std::numeric_limits<float>::epsilon()){
pcl::PointXYZ basic_point;
basic_point.x = depthMat(i,j);
basic_point.y = (i-cx)*depthMat(i,j)/fx;
basic_point.z = (j-cy)*depthMat(i,j)/fy;
cloud->push_back(basic_point);
index++;
}
}
return 1;
}
void run(Mat& A, const int rank){
if (A.cols() == 0 || A.rows() == 0) return;
int r = (rank < A.cols()) ? rank : A.cols();
r = (r < A.rows()) ? r : A.rows();
// Gaussian Random Matrix for A^T
Eigen::MatrixXf O(A.rows(), r);
Util::sampleGaussianMat(O);
// Compute Sample Matrix of A^T
Eigen::MatrixXf Y = A.transpose() * O;
// Orthonormalize Y
Util::processGramSchmidt(Y);
// Range(B) = Range(A^T)
Eigen::MatrixXf B = A * Y;
// Gaussian Random Matrix
Eigen::MatrixXf P(B.cols(), r);
Util::sampleGaussianMat(P);
// Compute Sample Matrix of B
Eigen::MatrixXf Z = B * P;
// Orthonormalize Z
Util::processGramSchmidt(Z);
// Range(C) = Range(B)
Eigen::MatrixXf C = Z.transpose() * B;
Eigen::JacobiSVD<Eigen::MatrixXf> svdOfC(C, Eigen::ComputeThinU | Eigen::ComputeThinV);
// C = USV^T
// A = Z * U * S * V^T * Y^T()
matU_ = Z * svdOfC.matrixU();
matS_ = svdOfC.singularValues();
matV_ = Y * svdOfC.matrixV();
}
void initLattice( const Eigen::MatrixXf & f ) {
const int N = f.cols();
lattice_.init( f );
norm_ = lattice_.compute( Eigen::VectorXf::Ones( N ).transpose() ).transpose();
if ( ntype_ == NO_NORMALIZATION ) {
float mean_norm = 0;
for ( int i=0; i<N; i++ )
mean_norm += norm_[i];
mean_norm = N / mean_norm;
for ( int i=0; i<N; i++ )
norm_[i] = mean_norm;
}
else if ( ntype_ == NORMALIZE_SYMMETRIC ) {
for ( int i=0; i<N; i++ )
norm_[i] = 1.0 / sqrt(norm_[i]+1e-20);
}
else {
for ( int i=0; i<N; i++ )
norm_[i] = 1.0 / (norm_[i]+1e-20);
}
}
void SaveDensity(const std::string& filename, const Eigen::MatrixXf& mat)
{
bool is_image =
filename.substr(filename.size()-3, 3) == ".png" ||
filename.substr(filename.size()-3, 3) == ".jpg";
if(is_image) {
}
else {
const int rows = mat.rows();
std::ofstream ofs(filename);
for(int y=0; y<mat.cols(); y++) {
for(int x=0; x<rows; x++) {
ofs << mat(x,y);
if(x+1 != rows) {
ofs << "\t";
}
else {
ofs << std::endl;
}
}
}
}
}
bool
writeDescrToFile (const std::string &file, const Eigen::MatrixXf &matrix)
{
std::ofstream out (file.c_str ());
if (!out)
{
std::cout << "Cannot open file.\n";
return false;
}
size_t i ,j;
for (i = 0; i < matrix.rows(); i++)
{
for (j = 0; j < matrix.cols(); j++)
{
out << matrix (i, j);
out << " ";
}
out<<'\n';
}
out.close ();
return true;
}
template<typename PointT> inline void
pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag)
{
if (!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");
Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
const size_t n = eigenvectors_.cols ();// number of eigen vectors
Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
Eigen::VectorXf h = y - input;
if (h.norm() > 0)
h.normalize ();
else
h.setZero ();
float gamma = h.dot(input - mean_.head<3>());
Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
D.block(0,0,n,n) = a * a.transpose();
D /= float(n)/float((n+1) * (n+1));
for(std::size_t i=0; i < a.size(); i++) {
D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
D(i,D.cols()-1) = D(D.rows()-1,i);
D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
}
Eigen::MatrixXf R(D.rows(), D.cols());
Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
Eigen::VectorXf alphap = D_evd.eigenvalues().real();
eigenvalues_.resize(eigenvalues_.size() +1);
for(std::size_t i=0; i<eigenvalues_.size(); i++) {
eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
R.col(i) = D.col(D.cols()-i-1);
}
Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
Up.rightCols<1>() = h;
eigenvectors_ = Up*R;
if (!basis_only_) {
Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
coefficients_(coefficients_.rows()-1,i) = 0;
coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
}
a.resize(a.size()+1);
a(a.size()-1) = 0;
coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
}
mean_.head<3>() = meanp;
switch (flag)
{
case increase:
if (eigenvectors_.rows() >= eigenvectors_.cols())
break;
case preserve:
if (!basis_only_)
coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
eigenvalues_.resize(eigenvalues_.size()-1);
break;
default:
PCL_ERROR("[pcl::PCA] unknown flag\n");
}
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::weightPointSPFHSignature (
const Eigen::MatrixXf &hist_f1, const Eigen::MatrixXf &hist_f2, const Eigen::MatrixXf &hist_f3,
const std::vector<int> &indices, const std::vector<float> &dists, Eigen::VectorXf &fpfh_histogram)
{
assert (indices.size () == dists.size ());
double sum_f1 = 0.0, sum_f2 = 0.0, sum_f3 = 0.0;
float weight = 0.0, val_f1, val_f2, val_f3;
// Get the number of bins from the histograms size
int nr_bins_f1 = static_cast<int> (hist_f1.cols ());
int nr_bins_f2 = static_cast<int> (hist_f2.cols ());
int nr_bins_f3 = static_cast<int> (hist_f3.cols ());
int nr_bins_f12 = nr_bins_f1 + nr_bins_f2;
// Clear the histogram
fpfh_histogram.setZero (nr_bins_f1 + nr_bins_f2 + nr_bins_f3);
// Use the entire patch
for (size_t idx = 0, data_size = indices.size (); idx < data_size; ++idx)
{
// Minus the query point itself
if (dists[idx] == 0)
continue;
// Standard weighting function used
weight = 1.0f / dists[idx];
// Weight the SPFH of the query point with the SPFH of its neighbors
for (int f1_i = 0; f1_i < nr_bins_f1; ++f1_i)
{
val_f1 = hist_f1 (indices[idx], f1_i) * weight;
sum_f1 += val_f1;
fpfh_histogram[f1_i] += val_f1;
}
for (int f2_i = 0; f2_i < nr_bins_f2; ++f2_i)
{
val_f2 = hist_f2 (indices[idx], f2_i) * weight;
sum_f2 += val_f2;
fpfh_histogram[f2_i + nr_bins_f1] += val_f2;
}
for (int f3_i = 0; f3_i < nr_bins_f3; ++f3_i)
{
val_f3 = hist_f3 (indices[idx], f3_i) * weight;
sum_f3 += val_f3;
fpfh_histogram[f3_i + nr_bins_f12] += val_f3;
}
}
if (sum_f1 != 0)
sum_f1 = 100.0 / sum_f1; // histogram values sum up to 100
if (sum_f2 != 0)
sum_f2 = 100.0 / sum_f2; // histogram values sum up to 100
if (sum_f3 != 0)
sum_f3 = 100.0 / sum_f3; // histogram values sum up to 100
// Adjust final FPFH values
for (int f1_i = 0; f1_i < nr_bins_f1; ++f1_i)
fpfh_histogram[f1_i] *= static_cast<float> (sum_f1);
for (int f2_i = 0; f2_i < nr_bins_f2; ++f2_i)
fpfh_histogram[f2_i + nr_bins_f1] *= static_cast<float> (sum_f2);
for (int f3_i = 0; f3_i < nr_bins_f3; ++f3_i)
fpfh_histogram[f3_i + nr_bins_f12] *= static_cast<float> (sum_f3);
}
/////////////////////////////////
///// Pairwise Potentials /////
/////////////////////////////////
void DenseCRF::addPairwiseEnergy (const Eigen::MatrixXf & features, LabelCompatibility * function, KernelType kernel_type, NormalizationType normalization_type) {
assert( features.cols() == N_ );
addPairwiseEnergy( new PairwisePotential( features, function, kernel_type, normalization_type ) );
}
inline vcl_size_t size2(Eigen::MatrixXf const & m) { return m.cols(); }
// visualizzazione doppia
void VisualizationUtils::vis_doppia( string name1, string name2) {
std::stringstream firstCloud;
firstCloud<<"./../registrazione/cloud"<<name1<<".ply";
std::stringstream secondCloud;
secondCloud<<"./../registrazione/cloud"<<name2<<".ply";
cout<<firstCloud.str()<<endl;
cout<<secondCloud.str()<<endl;
pcl::PointCloud<POINT_TYPE>::Ptr cloud1 (new pcl::PointCloud<POINT_TYPE>);
pcl::PointCloud<POINT_TYPE>::Ptr cloud2 (new pcl::PointCloud<POINT_TYPE>);
pcl::io::loadPLYFile(firstCloud.str().c_str(),*cloud1);
pcl::io::loadPLYFile(secondCloud.str().c_str(),*cloud2);
pcl::PointCloud<POINT_TYPE>::Ptr clicked_points_app(new pcl::PointCloud<POINT_TYPE>);
pcl::PointCloud<POINT_TYPE>::Ptr clicked_points2_app(new pcl::PointCloud<POINT_TYPE>);
// resetto le variabili globali
clicked_points_app->clear();
clicked_points2_app->clear();
points_left.clear_stack();
points_right.clear_stack();
color_left.restart();
color_right.restart();
clicked_points = clicked_points2_app;
clicked_points2 = clicked_points2_app;
// creo la finestra
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
global_viewer = viewer;
// viewer->initCameraParameters();
// viewer->setSize(1200, 650);
// assegno la prima cloud
viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
viewer->setCameraPosition(0.0, 0.0, 0.0, 0.0, 0.0, 0.15, 0.0, 1.0, 0.0, v1);
viewer->setBackgroundColor(0.2, 0.2, 0.2, v1); // background grigio
viewer->addText("Prossimo colore: " + color_left.getColorName(), 10, 10, "v1_text", v1);
pcl::visualization::PointCloudColorHandlerRGBField<POINT_TYPE> rgb(cloud1);
viewer->addPointCloud<POINT_TYPE>(cloud1, rgb, name1, v1);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, name1);
// assegno la seconda cloud
viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
viewer->setBackgroundColor(0.3, 0.3, 0.3, v2);
viewer->addText("Prossimo colore: " + color_right.getColorName(), 10, 10, "v2_text", v2);
pcl::visualization::PointCloudColorHandlerRGBField<POINT_TYPE> rgb2(cloud2);
// la traslo per poter identificare i suoi punti
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
transform.translation() << 0.0, 0.0, TRANLSATION_Z_SECOND_CLOUD;
//cout << "matrice applicata alla seconda cloud" << endl << transform.matrix() << endl;
pcl::PointCloud<POINT_TYPE>::Ptr transformed_cloud2(new pcl::PointCloud<POINT_TYPE>);
pcl::transformPointCloud(*cloud2, *transformed_cloud2, transform);
viewer->addPointCloud<POINT_TYPE>(transformed_cloud2, rgb2, name2, v2);
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, name2);
// gestione separata della telecamera
viewer->createViewPortCamera(v2);
viewer->setCameraPosition(0.0, 0.0, TRANLSATION_Z_SECOND_CLOUD, 0.0, 0.0, TRANLSATION_Z_SECOND_CLOUD + 0.15, 0.0, 1.0, 0.0, v2);
// viewer->registerKeyboardCallback(keyboardEventOccurred, (void*)&viewer);
// viewer->registerPointPickingCallback(pointPickDoubleViewEvent, (void*)&viewer);
loop_view(viewer);
// chiusa la view, stampo i punti catturati
cout << "Punti selezionati:" << endl << " -> cloud sinistra:" << endl;
//cout << endl << points_left.makeMatrix() << endl;
points_left.print_all();
cout << " -> cloud destra:" << endl;
//cout << endl << points_right.makeMatrix() << endl;
points_right.print_all();
Eigen::MatrixXf Mx = points_left.makeMatrix();
Eigen::MatrixXf My = points_right.makeMatrix();
if (Mx.cols() != My.cols()) {
cout << "Errore... Hai preso un numero diverso di punti tra destra e sinistra?" << endl;
return ;
}
/*if (Mx.cols() < 3) {
cout << "Dai, sforzati di prendere almeno 3 punti..." << endl;
return;
}*/
Eigen::Matrix4f T = TransformationUtils::trovaT(Mx, My);
ofstream outFile("/home/miky/Scrivania/trasformazione.txt");
outFile << T;
cout << " -> matrice di trasformazione salvata nel file trasformazione.txt" << endl;
pcl::PointCloud<POINT_TYPE>::Ptr result(new pcl::PointCloud<POINT_TYPE>);
pcl::transformPointCloud(*cloud1, *result, T);
pcl::io::savePLYFileBinary("/home/miky/Scrivania/Cloud12.ply", *result);
cout << " -> salvata la cloud traslata con nome Cloud12.ply" << endl;
return ;
}
void CameraFrames::ProjectToCameraPlane(Eigen::MatrixXf cloud){
Eigen::MatrixXf P_L(3,4);
Eigen::MatrixXf P_R(3,4);
Eigen::MatrixXf Point2dimensions_right(3,cloud.cols());
Eigen::MatrixXf Point2dimensions_left(3,cloud.cols());
Eigen::MatrixXf u = Eigen::MatrixXf::Ones(1,cloud.cols());
Eigen::MatrixXf translation_l(4,1);
Eigen::MatrixXf translation_r(4,1);
Eigen::Matrix3f rotationMatrix;
Eigen::MatrixXf toLeftAxis(4,cloud.cols());
Eigen::MatrixXf AuxMat(3,cloud.cols());
cv::Point2f aux_l(0,0);
cv::Point2f aux_r(0,0);
modelpoints2dright.clear();
modelpoints2dleft.clear();
//The Projection matrix projects points in the camera frame.
//Construct the Projection Matrix of the Left Camera
P_L(0,0) = leftCamInfo->P[0]; P_L(1,0) = leftCamInfo->P[4]; P_L(2,0) = leftCamInfo->P[8];
P_L(0,1) = leftCamInfo->P[1]; P_L(1,1) = leftCamInfo->P[5]; P_L(2,1) = leftCamInfo->P[9];
P_L(0,2) = leftCamInfo->P[2]; P_L(1,2) = leftCamInfo->P[6]; P_L(2,2) = leftCamInfo->P[10];
P_L(0,3) = leftCamInfo->P[3]; P_L(1,3) = leftCamInfo->P[7]; P_L(2,3) = leftCamInfo->P[11];
//Construct the Projection Matrix of the Right Camera
P_R(0,0) = rightCamInfo->P[0]; P_R(1,0) = rightCamInfo->P[4]; P_R(2,0) = rightCamInfo->P[8];
P_R(0,1) = rightCamInfo->P[1]; P_R(1,1) = rightCamInfo->P[5]; P_R(2,1) = rightCamInfo->P[9];
P_R(0,2) = rightCamInfo->P[2]; P_R(1,2) = rightCamInfo->P[6]; P_R(2,2) = rightCamInfo->P[10];
P_R(0,3) = 0; P_R(1,3) = rightCamInfo->P[7]; P_R(2,3) = rightCamInfo->P[11];
/*Project the 3D point onto the camera planes [u v w] = ProjMat * (3DPoints+Translation).
Translation is neccesary to put the 3D points on the corresponding camera frame.
Baseline = 0.04 and global frame in the midle of the baseline. */
rotationMatrix = Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY())
* Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ())
* Eigen::AngleAxisf(-90, Eigen::Vector3f::UnitX());
translation_l << 0, 0, 0, 0;
translation_r << 0.05, 0, 0, 0;
AuxMat = rotationMatrix*cloud;
toLeftAxis.row(0) = -AuxMat.row(0); //The Leap is left handed
toLeftAxis.row(1) = AuxMat.row(1);
toLeftAxis.row(2) = AuxMat.row(2);
toLeftAxis.row(3).setOnes();
Point2dimensions_left = P_L*(toLeftAxis+(translation_l*u));
Point2dimensions_right = P_R*(toLeftAxis+(translation_r*u));
//Divide by the Homogeneous Coordinates to get the 2D Points
Point2dimensions_left.row(0) = Point2dimensions_left.row(0).cwiseQuotient(Point2dimensions_left.row(2));
Point2dimensions_left.row(1) = Point2dimensions_left.row(1).cwiseQuotient(Point2dimensions_left.row(2));
Point2dimensions_right.row(0) = Point2dimensions_right.row(0).cwiseQuotient(Point2dimensions_right.row(2));
Point2dimensions_right.row(1) = Point2dimensions_right.row(1).cwiseQuotient(Point2dimensions_right.row(2));
for(int i = 0; i < cloud.cols(); i++){
aux_l.x = Point2dimensions_left(0,i);
aux_l.y = Point2dimensions_left(1,i);
aux_r.x = Point2dimensions_right(0,i);
aux_r.y = Point2dimensions_right(1,i);
if(aux_l.x < 280 && aux_l.x >=0 && aux_r.x < 280 && aux_r.x >= 0 && aux_l.y < 220 && aux_l.y >= 0 && aux_r.y < 220 && aux_r.y >=0 ){
modelpoints2dleft.push_back(aux_l);
modelpoints2dright.push_back(aux_r);
}
}
/* cv::Point center(floor(aux_l.x), floor(aux_l.y));
cv::circle(LeftFrame,center,0.1,cv::Scalar(255,255,255),-1);
cv::Point center2(floor(aux_l.x), floor(aux_l.y));
cv::circle(RightFrame,center2,0.1,cv::Scalar(255,255,255),-1);
cv::imshow("pointsleft", LeftFrame);
cv::imshow("pointsright", RightFrame);
cv::waitKey(1);*/
}
int main(int argc, char** argv)
{
std::string p_feature_img = "";
std::string p_density = "";
std::string p_fn_points = "";
std::string p_out = "";
unsigned p_num = 250;
bool p_verbose = false;
unsigned p_repetitions = 1;
bool p_error = false;
bool p_save_density = true;
// parameters
asp::Parameters params = asp::parameters_default();
namespace po = boost::program_options;
po::options_description desc;
desc.add_options()
("help", "produce help message")
("features", po::value(&p_feature_img), "feature image (leave empty for uniform image)")
("density", po::value(&p_density), "density function image (leave empty for test function)")
("points", po::value(&p_fn_points), "file with points to use as seeds (for DDS)")
("out", po::value(&p_out), "filename of result file with samples points")
("num", po::value(&p_num), "number of points to sample")
("p_num_iterations", po::value(¶ms.num_iterations), "number of DALIC iterations")
("p_pds_mode", po::value(¶ms.pds_mode), "Poisson Disk Sampling method")
("p_seed_mean_radius_factor", po::value(¶ms.seed_mean_radius_factor), "size factor for initial cluster mean feature")
("p_coverage", po::value(¶ms.coverage), "DALIC cluster search factor")
("p_weight_compact", po::value(¶ms.weight_compact), "weight for compactness term")
("verbose", po::value(&p_verbose), "verbose")
("repetitions", po::value(&p_repetitions), "repetitions")
("error", po::value(&p_error), "error")
("save_density", po::value(&p_save_density), "save_density")
;
po::variables_map vm;
po::store(po::parse_command_line(argc, argv, desc), vm);
po::notify(vm);
if(vm.count("help")) {
std::cerr << desc << std::endl;
return 1;
}
// load
bool is_features_null = p_feature_img.empty();
bool is_density_null = p_density.empty();
Eigen::MatrixXf rho;
std::vector<Eigen::Vector3f> features;
int width = -1, height = -1;
if(is_density_null && is_features_null) {
std::cerr << "ERROR feature or density must not be both empty!" << std::endl;
return 1;
}
if(!is_features_null) {
features = LoadFeatures(p_feature_img, width, height);
if(p_verbose) std::cout << "Loaded features dim=" << width << "x" << height << "." << std::endl;
}
if(!is_density_null) {
rho = density::LoadDensity(p_density);
if(p_verbose) std::cout << "Loaded density dim=" << rho.rows() << "x" << rho.cols() << ", sum=" << rho.sum() << "." << std::endl;
}
if(is_features_null) {
width = rho.rows();
height = rho.cols();
features.resize(width*height, Eigen::Vector3f{1,1,1});
if(p_verbose) std::cout << "Created uniform features dim=" << rho.rows() << "x" << rho.cols() << "." << std::endl;
}
if(is_density_null) {
//rho = CreateDensity(p_size, p_size, [](float x, float y) { return TestDensityFunction(x,y); } );
rho = CreateDensity(width, height, [](float x, float y) { return 1.0f; } );
if(p_verbose) std::cout << "Created density dim=" << rho.rows() << "x" << rho.cols() << ", sum=" << rho.sum() << "." << std::endl;
}
assert(width == rho.rows());
assert(height == rho.cols());
std::vector<Eigen::Vector2f> seed_points;
if(!p_fn_points.empty()) {
std::ifstream ifs(p_fn_points);
if(!ifs.is_open()) {
std::cerr << "Error opening file '" << p_fn_points << "'" << std::endl;
}
std::string line;
while(getline(ifs, line)) {
std::istringstream ss(line);
std::vector<float> q;
while(!ss.eof()) {
float v;
ss >> v;
q.push_back(v);
}
seed_points.push_back({q[0], q[1]});
}
}