int Conversion::convert(const Eigen::MatrixXf & depthMat,
Eigen::MatrixXf & point3D,
double &fx,
double &fy,
double &cx,
double &cy)
{
//max resize
point3D.resize(depthMat.rows()*depthMat.cols(),3);
int index(0);
for (int i=0; i<depthMat.rows();i++)
for (int j=0 ; j<depthMat.cols();j++)
{
float z = depthMat(i,j);
if (fabs( z+ 1.f) > std::numeric_limits<float>::epsilon()
& fabs(z) !=1
& z > std::numeric_limits<float>::epsilon() ){
point3D(index,2) = z;
point3D(index,0) = (i-cx)*point3D(index,2)/fx;
point3D(index,1) = (j-cy)*point3D(index,2)/fy;
index++;
}
}
//min resize
point3D.conservativeResize(index,3);
return 1;
}
template <typename PointSource, typename PointTarget> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
Eigen::Matrix4f &transformation_matrix)
{
size_t nr_points = correspondences.size ();
// Approximate as a linear least squares problem
Eigen::MatrixXf A (nr_points, 6);
Eigen::MatrixXf b (nr_points, 1);
for (size_t i = 0; i < nr_points; ++i)
{
const int & src_idx = correspondences[i].index_query;
const int & tgt_idx = correspondences[i].index_match;
const float & sx = cloud_src.points[src_idx].x;
const float & sy = cloud_src.points[src_idx].y;
const float & sz = cloud_src.points[src_idx].z;
const float & dx = cloud_tgt.points[tgt_idx].x;
const float & dy = cloud_tgt.points[tgt_idx].y;
const float & dz = cloud_tgt.points[tgt_idx].z;
const float & nx = cloud_tgt.points[tgt_idx].normal[0];
const float & ny = cloud_tgt.points[tgt_idx].normal[1];
const float & nz = cloud_tgt.points[tgt_idx].normal[2];
A (i, 0) = nz*sy - ny*sz;
A (i, 1) = nx*sz - nz*sx;
A (i, 2) = ny*sx - nx*sy;
A (i, 3) = nx;
A (i, 4) = ny;
A (i, 5) = nz;
b (i, 0) = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
}
// Solve A*x = b
Eigen::VectorXf x = A.colPivHouseholderQr ().solve (b);
// Construct the transformation matrix from x
constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
}
//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
//delta is a matrix of 2x1 returns the increment in the x and y of the robot
void LMAlgr::computeIncrement(Eigen::MatrixXf params, Eigen::MatrixXf pose, double lambda, Eigen::MatrixXf error, Eigen::MatrixXf &delta){
Eigen::MatrixXf Jac;
Jac.resize(params.rows(), 2);
//initialize the jacobian matrix
for(int i = 0; i < params.rows(); i++){
Jac(i, 0) = (params(i, 1) - pose(1, 0)) / (pow(params(i, 0) - pose(0, 0), 2) + pow(params(i, 1) - pose(1, 0), 2));
Jac(i, 1) = -1 * (params(i, 0) - pose(0, 0)) / (pow(params(i, 0) - pose(0, 0), 2) + pow(params(i, 1) - pose(1, 0), 2));
}
Eigen::MatrixXf I;
I = Eigen::MatrixXf::Identity(2, 2);
Eigen::MatrixXf tmp = (Jac.transpose() * Jac) + (lambda * I);
Eigen::MatrixXf incr = tmp.inverse() * Jac.transpose() * error;
delta = incr;
}
template <typename PointInT, typename PointOutT> void
pcl::SIFTKeypoint<PointInT, PointOutT>::computeScaleSpace (
const PointCloudIn &input, KdTree &tree, const std::vector<float> &scales,
Eigen::MatrixXf &diff_of_gauss)
{
std::vector<int> nn_indices;
std::vector<float> nn_dist;
diff_of_gauss.resize (input.size (), scales.size () - 1);
// For efficiency, we will only filter over points within 3 standard deviations
const float max_radius = 3.0 * scales.back ();
for (size_t i_point = 0; i_point < input.size (); ++i_point)
{
tree.radiusSearch (i_point, max_radius, nn_indices, nn_dist); // *
// * note: at this stage of the algorithm, we must find all points within a radius defined by the maximum scale,
// regardless of the configurable search method specified by the user, so we directly employ tree.radiusSearch
// here instead of using searchForNeighbors.
// For each scale, compute the Gaussian "filter response" at the current point
float filter_response = 0;
float previous_filter_response;
for (size_t i_scale = 0; i_scale < scales.size (); ++i_scale)
{
float sigma_sqr = pow (scales[i_scale], 2);
float numerator = 0.0;
float denominator = 0.0;
for (size_t i_neighbor = 0; i_neighbor < nn_indices.size (); ++i_neighbor)
{
const float &intensity = input.points[nn_indices[i_neighbor]].intensity;
const float &dist_sqr = nn_dist[i_neighbor];
if (dist_sqr <= 9*sigma_sqr)
{
float w = exp (-0.5 * dist_sqr / sigma_sqr);
numerator += intensity * w;
denominator += w;
}
else break; // i.e. if dist > 3 standard deviations, then terminate early
}
previous_filter_response = filter_response;
filter_response = numerator / denominator;
// Compute the difference between adjacent scales
if (i_scale > 0)
{
diff_of_gauss (i_point, i_scale - 1) = filter_response - previous_filter_response;
}
}
}
}
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;
}
int
writeMat2File(const Eigen::MatrixXf &matrix, const std::string &fileName)
{
std::ofstream out( fileName.c_str() );
if (out.is_open())
out << matrix.format(CSVFormat);
else
return 0;
out.close();
return 1;
}
void compressFeature( string filename, std::vector< std::vector<float> > &models, const int dim, bool ascii ){
PCA pca;
pca.read( filename.c_str(), ascii );
Eigen::VectorXf variance = pca.getVariance();
Eigen::MatrixXf tmpMat = pca.getAxis();
Eigen::MatrixXf tmpMat2 = tmpMat.block(0,0,tmpMat.rows(),dim);
const int num = (int)models.size();
for( int i=0; i<num; i++ ){
Eigen::Map<Eigen::VectorXf> vec( &(models[i][0]), models[i].size() );
//vec = tmpMat2.transpose() * vec;
Eigen::VectorXf tmpvec = tmpMat2.transpose() * vec;
models[i].resize( dim );
if( WHITENING ){
for( int t=0; t<dim; t++ )
models[i][t] = tmpvec[t] / sqrt( variance( t ) );
}
else{
for( int t=0; t<dim; t++ )
models[i][t] = tmpvec[t];
}
}
}
void FeatureEval::computeCorrelation(float * data, int vector_size, int size, std::vector<int> & big_to_small, int stride, int offset){
stride = stride ? stride : vector_size;
if(ll_->getSelection().size() == 0)
return;
std::set<uint16_t> labels;
for(boost::weak_ptr<Layer> wlayer: ll_->getSelection()){
for(uint16_t label : wlayer.lock()->getLabelSet())
labels.insert(label);
}
std::vector<float> layer = get_scaled_layer_mask(cl_->active_->labels_,
labels,
big_to_small,
size);
Eigen::MatrixXf correlation_mat = multi_correlate(layer, data, vector_size, size, stride, offset);
Eigen::MatrixXf Rxx = correlation_mat.topLeftCorner(vector_size, vector_size);
Eigen::VectorXf c = correlation_mat.block(0, vector_size, vector_size, 1);
//std::cout << correlation_mat << std::endl;
float R = c.transpose() * (Rxx.inverse() * c);
qDebug() << "R^2: " << R;
//qDebug() << "R: " << sqrt(R);
reportResult(R, c.data(), vector_size);
//Eigen::VectorXf tmp = (Rxx.inverse() * c);
qDebug() << "Y -> X correlation <<<<<<<<<<<<<";
std::cout << c << std::endl;
//qDebug() << "Coefs <<<<<<<<<<<<<";
//std::cout << tmp << std::endl;
}
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 PointProjector::project(Eigen::MatrixXi &indexImage,
Eigen::MatrixXf &depthImage,
const HomogeneousPoint3fVector &points) const {
depthImage.resize(indexImage.rows(), indexImage.cols());
depthImage.fill(std::numeric_limits<float>::max());
indexImage.fill(-1);
const HomogeneousPoint3f* point = &points[0];
for (size_t i=0; i<points.size(); i++, point++){
int x, y;
float d;
if (!project(x, y, d, *point) ||
x<0 || x>=indexImage.rows() ||
y<0 || y>=indexImage.cols() )
continue;
float& otherDistance=depthImage(x,y);
int& otherIndex=indexImage(x,y);
if (otherDistance>d) {
otherDistance = d;
otherIndex = i;
}
}
}
std::tuple<Eigen::MatrixXf, Eigen::VectorXf> dart::QWeightedError::computeGNParam(const Eigen::VectorXf &diff) {
// compute error from joint deviation
const float e = diff.transpose()*_Q*diff;
Eigen::MatrixXf deriv = Eigen::MatrixXf::Zero(diff.size(), 1);
for(unsigned int i=0; i<diff.size(); i++) {
// original derivation
//deriv(i) = diff.dot(_Q.row(i)) + diff.dot(_Q.col(i)) - (diff[i]*_Q(i,i));
// negative direction, this works
//deriv(i) = - diff.dot(_Q.row(i)) + diff.dot(_Q.col(i)) - (diff[i]*_Q(i,i));
deriv(i) = - ( diff.dot(_Q.row(i) + _Q.col(i).transpose()) - (diff[i]*_Q(i,i)) );
}
// Jacobian of error, e.g. the partial derivation of the error w.r.t. to each joint value
// For an error of zero, its partial derivative is not defined. Therefore we set its derivative to 0.
const Eigen::MatrixXf J = (diff.array()==0).select(0, deriv);
const Eigen::VectorXf JTe = J.array()*e;
return std::make_tuple(J, JTe);
}
/**
* @brief Searches in the graph closest points to origin and target
*
* @param origin ...
* @param target ...
* @param originVertex to return selected vertex
* @param targetVertex ...
* @return void
*/
void PlannerPRM::searchClosestPoints(const QVec& origin, const QVec& target, Vertex& originVertex, Vertex& targetVertex)
{
qDebug() << __FUNCTION__ << "Searching from " << origin << "and " << target;
//prepare the query
Eigen::MatrixXi indices;
Eigen::MatrixXf distsTo;
Eigen::MatrixXf query(3,2);
indices.resize(1, query.cols());
distsTo.resize(1, query.cols());
query(0,0) = origin.x();query(1,0) = origin.y();query(2,0) = origin.z();
query(0,1) = target.x();query(1,1) = target.y();query(2,1) = target.z();
nabo->knn(query, indices, distsTo, 1);
originVertex = vertexMap.value(indices(0,0));
targetVertex = vertexMap.value(indices(0,1));
qDebug() << __FUNCTION__ << "Closest point to origin is at" << data(0,indices(0,0)) << data(1,indices(0,0)) << data(2,indices(0,0)) << " and corresponds to " << graph[originVertex].pose;
qDebug() << __FUNCTION__ << "Closest point to target is at" << data(0,indices(0,1)) << data(1,indices(0,1)) << data(2,indices(0,1)) << " and corresponds to " << graph[targetVertex].pose;
}
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);
}
int Conversion::convert(const vpImage<float>&dmap, Eigen::MatrixXf & depthMat)
{
int height = dmap.getHeight();
int width = dmap.getWidth();
// i <-> height and j<->width
depthMat.resize(height,width);
for(int i = 0 ; i< height ; i++){
for(int j=0 ; j< width ; j++){
depthMat(i,j) = dmap[i][j];
}
}
return 1;
}
void
readLDRFile(const std::string& file, Eigen::MatrixXf& data)
{
std::ifstream in(file.c_str(), std::ios::in | std::ios::binary);
in.seekg(0, std::ios::end);
int size = in.tellg();
in.seekg(0, std::ios::beg);
int num_floats = size / (sizeof(float) / sizeof (char));
int num_rows = num_floats / 6;
data.resize(6, num_rows);
float* row_arr = new float[num_floats];
in.read((char*)(row_arr), size);
float* data_arr = data.data();
for (int k = 0; k < num_floats; k++)
data_arr[k] = row_arr[k];
data.transposeInPlace();
in.close();
}
///////////////////////
///// Inference /////
///////////////////////
void expAndNormalize ( Eigen::MatrixXf & out, const Eigen::MatrixXf & in ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<out.cols(); i++ ){
Eigen::VectorXf b = in.col(i);
b.array() -= b.maxCoeff();
b = b.array().exp();
out.col(i) = b / b.array().sum();
}
}
bool ZMeshAlgorithms::runManifoldSmooth(float sigma_r, float sigma_s)
{
SAFE_DELETE(pMeshFilterManifold_);
int nSize = mesh_->get_num_of_faces_list();
//int nSize = mesh_->get_num_of_vertex_list();
int spatialDim = 3;
int rangeDim = 3;
Eigen::MatrixXf faceNormals = MeshTools::getAllFaceNormals(mesh_);
//Eigen::MatrixXf faceNormals = MeshTools::getAllFaceNormalsSpherical(mesh_);
Eigen::MatrixXf faceCenters = MeshTools::getAllFaceCenters(mesh_);
Eigen::MatrixXf verticePos = MeshTools::getAllVerticePos(mesh_);
Eigen::MatrixXf faceAttributes(nSize, spatialDim+rangeDim);
faceAttributes.block(0, 0, nSize, spatialDim) = faceCenters;
faceAttributes.block(0, 3, nSize, rangeDim) = faceNormals;
// faceAttributes.block(0, 0, nSize, spatialDim) = verticePos;
// faceAttributes.block(0, 3, nSize, rangeDim) = verticePos;
//ZMeshFilterManifold filter(mesh_);
pMeshFilterManifold_ = new ZMeshFilterManifold(mesh_);
float sigma_spatial = mesh_->m_minEdgeLength*sigma_s;
float sigma_range = sin(sigma_r*Z_PI/180.f);
pMeshFilterManifold_->setPara(ZMeshFilterParaNames::SpatialSigma, sigma_spatial);
pMeshFilterManifold_->setPara(ZMeshFilterParaNames::RangeSigma, sigma_range);
pMeshFilterManifold_->setRangeDim(rangeDim);
pMeshFilterManifold_->setSpatialDim(spatialDim);
CHECK_FALSE_AND_RETURN(pMeshFilterManifold_->apply(faceAttributes, std::vector<bool>(nSize, true)));
Eigen::MatrixXf output = pMeshFilterManifold_->getResult();
meshNewNormals_ = output.block(0, spatialDim, nSize, rangeDim);
MeshTools::setAllFaceNormals2(mesh_, meshNewNormals_);
MeshTools::setAllFaceColorValue(mesh_, pMeshFilterManifold_->getPointClusterIds());
//MeshTools::setAllVerticePos(mesh_, output.block(0, spatialDim, nSize, rangeDim));
//MeshTools::setAllFaceNormal2Spherical(mesh_, output.block(0, spatialDim, nSize, rangeDim));
ZFileHelper::saveEigenMatrixToFile("oldNormal.txt", faceNormals);
ZFileHelper::saveEigenMatrixToFile("newNormal.txt", output.block(0,spatialDim, nSize, rangeDim));
return true;
}
void plotObs(const Eigen::MatrixXf cloud, PointCloudPlot::Ptr plot) {
MatrixXf centers = cloud.leftCols(3);
MatrixXf colors(cloud.rows(), 4);
if (cloud.cols() >= 6)
colors << cloud.middleCols(3,3).rowwise().reverse(), 0.5*VectorXf::Ones(cloud.rows());
else
colors = Vector4f(1,1,1,1).transpose().replicate(cloud.rows(), 1);
plot->setPoints(util::toVec3Array(centers), util::toVec4Array(colors));
}
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;
}
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;
}
}
}
}
}
int main(int argc, char* argv[])
{
try
{
// Select a device and display arrayfire info
int device = argc > 1 ? std::atoi(argv[1]) : 0;
af::setDevice(device);
af::info();
Eigen::MatrixXf C = Eigen::MatrixXf::Random(1e4, 50); // host array
af::array in(1e4, 50, C.data()); // copy host data to device
af::array u, s_vec, vt;
svd(u, s_vec, vt, in);
}
catch (af::exception& e)
{
std::cout << e.what() << '\n';
return 1;
}
return 0;
}
ConverterPlaneFromTo3d::ConverterPlaneFromTo3d(float fx, float fy, float cx, float cy, int height, int width)
{
Eigen::VectorXf colIndicies = Eigen::VectorXf::LinSpaced(Eigen::Sequential, width, 1, (float)width);
Eigen::VectorXf rowIndicies = Eigen::VectorXf::LinSpaced(Eigen::Sequential, height, 1, (float)height);
Eigen::VectorXf onesColSize = Eigen::VectorXf::Ones(width, 1);
Eigen::VectorXf onesRowSize = Eigen::VectorXf::Ones(height, 1);
Eigen::MatrixXf indiciesMatrixAxisX = onesRowSize * colIndicies.transpose(); //row = 1, 2, 3, 4, ..
Eigen::MatrixXf indiciesMatrixAxisY = rowIndicies * onesColSize.transpose();
xAdjustment_ = (indiciesMatrixAxisX.array() - cx) / fx;
yAdjustment_ = (indiciesMatrixAxisY.array() - cy) / fy;
Eigen::IOFormat matlabFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n", "", "", "", "");
std::ostringstream saveFileNameString0;
saveFileNameString0 << "indiciesMatrixAxisX.csv";
std::ofstream matrixFile;
matrixFile.open(saveFileNameString0.str());
if (matrixFile.is_open())
{
matrixFile << indiciesMatrixAxisX.format(matlabFormat);
}
std::ostringstream saveFileNameString;
saveFileNameString << "xAdjustment.csv";
std::ofstream matrixFile1;
matrixFile1.open(saveFileNameString.str());
if (matrixFile1.is_open())
{
matrixFile1 << xAdjustment_.format(matlabFormat);
}
}
int main(int argc, char** argv)
{
std::string p_density = "";
std::string p_mode = "spds";
std::string p_out = "pnts.tsv";
unsigned p_size = 128;
unsigned p_num = 250;
namespace po = boost::program_options;
po::options_description desc;
desc.add_options()
("help", "produce help message")
("density", po::value(&p_density), "density function image (leave empty for test function)")
("mode", po::value(&p_mode), "sampling method")
("out", po::value(&p_out), "filename of result file with samples points")
("size", po::value(&p_size), "size of image in pixel")
("num", po::value(&p_num), "number of points to sample")
;
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;
}
Eigen::MatrixXf rho;
if(p_density.empty()) {
rho = TestDensity(p_size, p_num);
std::cout << "Created density dim=" << rho.rows() << "x" << rho.cols() << ", sum=" << rho.sum() << "." << std::endl;
}
else {
rho = density::LoadDensity(p_density);
std::cout << "Loaded density dim=" << rho.rows() << "x" << rho.cols() << ", sum=" << rho.sum() << "." << std::endl;
}
// scale density
float scl = static_cast<float>(p_num) / rho.sum();
rho *= scl;
std::vector<Eigen::Vector2f> pnts;
{
boost::timer::auto_cpu_timer t;
pnts = pds::PoissonDiscSampling(p_mode, rho);
}
std::cout << "Generated " << pnts.size() << " points." << std::endl;
std::ofstream ofs(p_out);
for(const Eigen::Vector2f& x : pnts) {
ofs << x[0] << "\t" << x[1] << std::endl;
}
std::cout << "Wrote points to file '" << p_out << "'." << std::endl;
return 1;
}
void saveModel(std::string modelname, std::map<std::string, size_t>& word2id, Eigen::MatrixXf& inner, Eigen::MatrixXf& outer) {
std::ofstream sink(modelname.c_str());
if (!sink.good()) {
std::cerr << "Error opening file " << modelname << std::endl;
std::exit(-1);
}
sink << word2id.size() << std::endl;
for (std::pair<const std::string, size_t>& kv : word2id) {
sink << kv.first << "\t" << kv.second << std::endl;
}
sink << "inner vector" << std::endl;
for (size_t i = 0; i < inner.rows(); ++i) {
sink << inner.row(i) << std::endl;
}
sink << "outer vector" << std::endl;
for (size_t i = 0; i < outer.rows(); ++i) {
sink << outer.row(i) << std::endl;
}
}
template <typename PointT> void
pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const Eigen::Vector4f ¢roid,
Eigen::MatrixXf &cloud_out)
{
size_t npts = cloud_in.points.size ();
cloud_out = Eigen::MatrixXf::Zero (4, npts); // keep the data aligned
for (size_t i = 0; i < npts; ++i)
// One column at a time
cloud_out.block<4, 1> (0, i) = cloud_in.points[i].getVector4fMap () - centroid;
// Make sure we zero the 4th dimension out (1 row, N columns)
cloud_out.block (3, 0, 1, npts).setZero ();
}
Eigen::Vector4f PhotoCamera::center(const Eigen::MatrixXf &camera)
{
Eigen::Matrix3f C1, C2, C3, C4;
C1 << camera.col(1), camera.col(2), camera.col(3);
C2 << camera.col(0), camera.col(2), camera.col(3);
C3 << camera.col(0), camera.col(1), camera.col(3);
C4 << camera.col(0), camera.col(1), camera.col(2);
Eigen::Vector4f C;
C << C1.determinant(), -C2.determinant(), C3.determinant(), -C4.determinant();
return C;
}
int Conversion::convert(const Eigen::MatrixXf & matrix,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const int & r, const int & g, const int & b)
{
// cloud->empty();
for (int i=0; i<matrix.rows();i++){
pcl::PointXYZRGB basic_point;
basic_point.x = matrix(i,0);
basic_point.y = matrix(i,1);
basic_point.z = matrix(i,2);
basic_point.r = r;
basic_point.g = g;
basic_point.b = b;
cloud->push_back(basic_point);
}
return 1;
}
void meanSubtract(Eigen::MatrixXf &data)
{
double x_sum = data.row(0).sum();
double y_sum = data.row(1).sum();
x_sum /= data.cols();
y_sum /= data.cols();
Eigen::MatrixXf xsum = Eigen::MatrixXf::Constant(1, data.cols(), x_sum);
Eigen::MatrixXf ysum = Eigen::MatrixXf::Constant(1, data.cols(), y_sum);
for (int i = 0; i < data.rows(); i++)
{
if (i % 2 == 0)
{
data.row(i) = data.row(i) - xsum;
}
else
{
data.row(i) = ysum - data.row(i);
}
}
}
/** Selects the point in the tree node with highest probability */
inline Eigen::Vector2f OptimalCellPoint(const Eigen::MatrixXf& m0, int scale, int x, int y)
{
x *= scale;
y *= scale;
const auto& b = m0.block(x, y, scale, scale);
unsigned int best_j=scale/2, best_i=scale/2;
float best_val = -1000000.0f;
for(unsigned int i=0; i<b.cols(); ++i) {
for(unsigned int j=0; j<b.rows(); ++j) {
float val = b(j,i);
if(val > best_val) {
best_j = j;
best_i = i;
best_val = val;
}
}
}
return Eigen::Vector2f(x + best_j, y + best_i);
}
/**
* DemeanPoints
*/
void RigidTransformationRANSAC::DemeanPoints (
const std::vector<Eigen::Vector3f > &inPts,
const std::vector<int> &indices,
const Eigen::Vector3f ¢roid,
Eigen::MatrixXf &outPts)
{
if (inPts.size()==0 || indices.size()==0)
{
return;
}
outPts = Eigen::MatrixXf(4, indices.size());
// Subtract the centroid from points
for (unsigned i = 0; i < indices.size(); i++)
outPts.block<3,1>(0,i) = inPts[indices[i]]-centroid;
outPts.block(3, 0, 1, indices.size()).setZero();
}
