bool
af::loadMatrix (const std::string filename, Eigen::MatrixXf &matrix)
{
std::ifstream file (filename.c_str (), std::ios::binary | std::ios::in);
if (!file.is_open ())
{
PCL_ERROR ("Error reading matrix from file %s.\n", filename.c_str ());
return (false);
}
size_t num_rows, num_cols;
file.read (reinterpret_cast<char*> (&num_rows), sizeof (num_rows));
file.read (reinterpret_cast<char*> (&num_cols), sizeof (num_cols));
printf ("Reading matrix of size %zu %zu from %s\n", num_rows, num_cols, filename.c_str ());
matrix.resize (num_rows, num_cols);
for (size_t i = 0; i < num_rows; ++i)
for (size_t j = 0; j < num_cols; ++j)
{
float val;
file.read (reinterpret_cast<char*> (&val), sizeof (val));
matrix (i, j) = val;
}
file.close ();
return (true);
}
int Conversion::convert(const Eigen::MatrixXf & point3D,
Eigen::MatrixXf & depthMat,
const int &height,
const int &width,
double &fx,
double &fy,
double &cx,
double &cy)
{
if(point3D.rows()==0)
return 0;
depthMat.resize(height,width);
for (int i=0; i<depthMat.rows();i++)
for (int j=0 ; j<depthMat.cols();j++)
depthMat(i,j)=-1;
for (int index=0; index<point3D.rows();index++)
{
float val = point3D(index,2);//*1000;
//if(val>0){
int i = round(fx*point3D(index,0)/point3D(index,2)+cx);
int j = round(fy*point3D(index,1)/point3D(index,2)+cy);
depthMat(i,j)=val;
//}
index++;
}
return 1;
}
int Conversion::convert(const vpImage<float> & dmap, Eigen::MatrixXf & point3D,
double fx, double fy, double cx, double cy)
{
int height = dmap.getHeight();
int width = dmap.getWidth();
point3D.resize(height*width,3);
int index=0;
for(int i=0 ; i< height ; i++){
for(int j=0 ; j< width ; j++){
float z =dmap[i][j];
if (fabs(z + 1.f) > std::numeric_limits<float>::epsilon() & z>0 ){
point3D(index,2) = z;
point3D(index,0) = (float)((i-cx)*point3D(index,2)/fx);
point3D(index,1) = (float)((j-cy)*point3D(index,2)/fy);
index++;
}
}
}
// resize the point max to remove the points that have been pruned du to negative z value
point3D.conservativeResize(index,3);
return 1;
}
void detectSiftMatchWithVLFeat(const char* img1_path, const char* img2_path, Eigen::MatrixXf &match) {
int *m = 0;
double *kp1 = 0, *kp2 = 0;
vl_uint8 *desc1 = 0, *desc2 = 0;
int nkp1 = detectSiftAndCalculateDescriptor(img1_path, kp1, desc1);
int nkp2 = detectSiftAndCalculateDescriptor(img2_path, kp2, desc2);
cout << "num kp1: " << nkp1 << endl;
cout << "num kp2: " << nkp2 << endl;
int nmatch = matchDescriptorWithRatioTest(desc1, desc2, nkp1, nkp2, m);
cout << "num match: " << nmatch << endl;
match.resize(nmatch, 6);
for (int i = 0; i < nmatch; i++) {
int index1 = m[i*2+0];
int index2 = m[i*2+1];
match.row(i) << kp1[index1*4+1], kp1[index1*4+0], 1, kp2[index2*4+1], kp2[index2*4+0], 1;
}
free(kp1);
free(kp2);
free(desc1);
free(desc2);
free(m);
}
void addEigenMatrixRow( Eigen::MatrixXf &m )
{
Eigen::MatrixXf temp=m;
m.resize(m.rows()+1,m.cols());
m.setZero();
m.block(0,0,temp.rows(),temp.cols())=temp;
}
void PixelMapper::mergeProjections(Eigen::MatrixXf& depthImage, Eigen::MatrixXi& indexImage,
Eigen::MatrixXf* depths, Eigen::MatrixXi* indices, int numImages){
assert (numImages>0);
int rows=depths[0].rows();
int cols=depths[0].cols();
depthImage.resize(indexImage.rows(), indexImage.cols());
depthImage.fill(std::numeric_limits<float>::max());
indexImage.resize(rows, cols);
indexImage.fill(-1);
#pragma omp parallel for
for (int c=0; c<cols; c++){
int* destIndexPtr = &indexImage.coeffRef(0,c);
float* destDepthPtr = &depthImage.coeffRef(0,c);
int* srcIndexPtr[numImages];
float* srcDepthPtr[numImages];
for (int i=0; i<numImages; i++){
srcIndexPtr[i] = &indices[i].coeffRef(0,c);
srcDepthPtr[i] = &depths[i].coeffRef(0,c);
}
for (int r=0; r<rows; r++){
for (int i=0; i<numImages; i++){
if (*destDepthPtr>*srcDepthPtr[i]){
*destDepthPtr = *srcDepthPtr[i];
*destIndexPtr = *srcIndexPtr[i];
}
srcDepthPtr[i]++;
srcIndexPtr[i]++;
}
destDepthPtr++;
destIndexPtr++;
}
}
}
template <typename PointInT, typename PointOutT> void
pcl::BilateralUpsampling<PointInT, PointOutT>::computeDistances (Eigen::MatrixXf &val_exp_depth, Eigen::VectorXf &val_exp_rgb)
{
val_exp_depth.resize (2*window_size_+1,2*window_size_+1);
val_exp_rgb.resize (3*255);
int j = 0;
for (int dx = -window_size_; dx < window_size_+1; ++dx)
{
int i = 0;
for (int dy = -window_size_; dy < window_size_+1; ++dy)
{
float val_exp = expf (- (dx*dx + dy*dy) / (2.0f * static_cast<float> (sigma_depth_ * sigma_depth_)));
val_exp_depth(i,j) = val_exp;
i++;
}
j++;
}
for (int d_color = 0; d_color < 3*255; d_color++)
{
float val_exp = expf (- d_color * d_color / (2.0f * sigma_color_ * sigma_color_));
val_exp_rgb(d_color) = val_exp;
}
}
/**
* Create the augmented knots vector and fill in matrix Xt with the spline basis vectors
*/
void basis_splines_endreps_local_v2(float *knots, int n_knots, int order, int *boundaries, int n_boundaries, Eigen::MatrixXf &Xt) {
assert(n_boundaries == 2 && boundaries[0] < boundaries[1]);
int n_rows = boundaries[1] - boundaries[0]; // this is frames so evaluate at each frame we'll need
int n_cols = n_knots + order; // number of basis vectors we'll have at end
Xt.resize(n_cols, n_rows); // swapped to transpose later. This order lets us use it as a scratch space
Xt.fill(0);
int n_std_knots = n_knots + 2 * order;
float std_knots[n_std_knots];
// Repeat the boundary knots on there to ensure linear out side of knots
for (int i = 0; i < order; i++) {
std_knots[i] = boundaries[0];
}
// Our original boundary knots here
for (int i = 0; i < n_knots; i++) {
std_knots[i+order] = knots[i];
}
// Repeat the boundary knots on there to ensure linear out side of knots
for (int i = 0; i < order; i++) {
std_knots[i+order+n_knots] = boundaries[1];
}
// Evaluate our basis splines at each frame.
for (int i = boundaries[0]; i < boundaries[1]; i++) {
int idx = -1;
// find index such that i >= knots[idx] && i < knots[idx+1]
float *val = std::upper_bound(std_knots, std_knots + n_std_knots - 1, 1.0f * i);
idx = val - std_knots - 1;
assert(idx >= 0);
float *f = Xt.data() + i * n_cols + idx - (order - 1); //column offset
basis_spline_xi_v2(std_knots, n_std_knots, order, idx, i, f);
}
// Put in our conventional format where each column is a basis vector
Xt.transposeInPlace();
}
Eigen::MatrixXf
readDescrFromFile(const std::string &file, int padding, int rowSize)
{
// check if file exists
boost::filesystem::path path = file;
if ( ! (boost::filesystem::exists ( path ) && boost::filesystem::is_regular_file(path)) )
throw std::runtime_error ("Given file path to read Matrix does not exist!");
std::ifstream in (file.c_str (), std::ifstream::in);
int bufferSize = 819200;
//int bufferSize = rowSize * 10;
char linebuf[bufferSize];
Eigen::MatrixXf matrix;
matrix.resize(0,rowSize);
int j=0;
while(in.getline (linebuf, bufferSize)){
int start_s=clock();
std::string line (linebuf);
std::vector < std::string > strs_2;
boost::split (strs_2, line, boost::is_any_of (" "));
matrix.conservativeResize(matrix.rows()+1,rowSize);
for (int i = 0; i < strs_2.size()-1; i++)
matrix (j, i) = static_cast<float> (atof (strs_2[i].c_str ()));
j++;
int stop_s=clock();
std::cout << "time: " << (stop_s-start_s)/double(CLOCKS_PER_SEC)*1000 << std::endl;
}
return matrix;
}
//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
void LMAlgr::computeLM(Eigen::MatrixXf observations, Eigen::MatrixXf params, Eigen::MatrixXf &pose){
double lambda = 0.0001;
//compute the squared error
Eigen::MatrixXf error;
error.resize(observations.rows(), 1);
double squared_error = computeError(observations, params, pose, error);
double old_error = std::numeric_limits<double>::max();
while(old_error - squared_error > threshold){
//std::cout << "squared error: " << squared_error << std::endl;
Eigen::MatrixXf delta(2, 1);
computeIncrement(params, pose, lambda, error, delta);
/*std::cout << "delta: \n" << delta << std::endl;*/
Eigen::MatrixXf new_pose = pose + delta;
//std::cout << new_pose.transpose() << std::endl;
Eigen::MatrixXf new_error;
new_error.resize(observations.rows(), 1);
double new_sq_error = computeError(observations, params, new_pose, new_error);
if(new_sq_error < squared_error){
/*std::cout << "case 1: squared error: " << new_sq_error << ", new pose:\n " << new_pose << std::endl;*/
pose = new_pose;
error = new_error;
old_error = squared_error;
squared_error = new_sq_error;
lambda = lambda / 10;
}
else{
/*std::cout << "case 2: squared error: " << squared_error << std::endl;*/
lambda = lambda * 10;
}
}
}
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;
}
void StructureSimilarityDialog::setMolecules(const std::vector<MongoChem::MoleculeRef> &molecules)
{
MongoChem::MongoDatabase *db = MongoChem::MongoDatabase::instance();
m_molecules = molecules;
// calculate similarity matrix
Eigen::MatrixXf similarityMatrix;
similarityMatrix.resize(m_molecules.size(), m_molecules.size());
for(size_t i = 0; i < m_molecules.size(); i++){
const MongoChem::MoleculeRef &refI = m_molecules[i];
boost::shared_ptr<Molecule> molecule = db->createMolecule(refI);
StructureSimilarityDescriptor descriptor;
descriptor.setMolecule(molecule);
for(size_t j = i + 1; j < m_molecules.size(); j++){
const MongoChem::MoleculeRef &refJ = m_molecules[j];
boost::shared_ptr<Molecule> moleculeJ = db->createMolecule(refJ);
float similarity = descriptor.value(moleculeJ.get()).toFloat();
similarityMatrix(i, j) = similarity;
similarityMatrix(j, i) = similarity;
}
}
// recalculate graph
m_graphWidget->setSimilarityMatrix(similarityMatrix);
}
void sumAndNormalize( Eigen::MatrixXf & out, const Eigen::MatrixXf & in, const Eigen::MatrixXf & Q ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<in.cols(); i++ ){
Eigen::VectorXf b = in.col(i);
Eigen::VectorXf q = Q.col(i);
out.col(i) = b.array().sum()*q - b;
}
}
///////////////////////
///// 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();
}
}
int Conversion::convert(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, Eigen::MatrixXf & matrix){
matrix.resize(cloud->points.size(), 3);
for (int i=0; i<cloud->points.size();i++){
pcl::PointXYZ basic_point(cloud->points[i]);
matrix(i,0)=basic_point.x ;
matrix(i,1)=basic_point.y ;
matrix(i,2)=basic_point.z ;
}
return 1;
}
void load( Archive & ar,
Eigen::MatrixXf & t,
const unsigned int file_version )
{
int n0;
ar >> BOOST_SERIALIZATION_NVP( n0 );
int n1;
ar >> BOOST_SERIALIZATION_NVP( n1 );
t.resize( n0, n1 );
ar >> make_array( t.data(), t.rows()*t.cols() );
}
virtual Eigen::VectorXf parameters() const {
if (ktype_ == CONST_KERNEL)
return Eigen::VectorXf();
else if (ktype_ == DIAG_KERNEL)
return parameters_;
else {
Eigen::MatrixXf p = parameters_;
p.resize( p.cols()*p.rows(), 1 );
return p;
}
}
virtual Eigen::VectorXf gradient( const Eigen::MatrixXf & a, const Eigen::MatrixXf & b ) const {
if (ktype_ == CONST_KERNEL)
return Eigen::VectorXf();
Eigen::MatrixXf fg = featureGradient( a, b );
if (ktype_ == DIAG_KERNEL)
return (f_.array()*fg.array()).rowwise().sum();
else {
Eigen::MatrixXf p = fg*f_.transpose();
p.resize( p.cols()*p.rows(), 1 );
return p;
}
}
virtual void setParameters( const Eigen::VectorXf & p ) {
if (ktype_ == DIAG_KERNEL) {
parameters_ = p;
initLattice( p.asDiagonal() * f_ );
}
else if (ktype_ == FULL_KERNEL) {
Eigen::MatrixXf tmp = p;
tmp.resize( parameters_.rows(), parameters_.cols() );
parameters_ = tmp;
initLattice( tmp * f_ );
}
}
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 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;
}
//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;
}
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 );
}
void operator()(
const Eigen::MatrixXf &A,
const Eigen::MatrixXf &B,
Eigen::MatrixXf &Transform){
assert(A.rows() == B.rows());
assert(A.cols() == numColumnElements);
assert(B.cols() == numColumnElements);
Transform.resize(numColumnElements, numColumnElements);
/*
Transform.col(0) = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B.col(0));
Transform.col(1) = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B.col(1));
Transform.col(2) = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B.col(2));
*/
Transform = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);
}
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);
}
}
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();
}
}
}
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;
}
}
}
/**
* @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;
}
// Read matrix from file
void readMatrix(const char *filename, Eigen::MatrixXf& result)
{
File matfile((std::string(filename)));
// get number of rows from file
const int rows = matfile.number_of_line();
/* check whether number of lines in words is equal to the number of words into vocab file */
if (rows != vocab_size){
throw std::runtime_error("Size mismatch between words and vocabulary files!!!\n");
}
// get number of rows from file
const int cols = matfile.number_of_column(' ');
if (verbose) fprintf(stderr, "words vector size = %d\n",cols);
// opening file
matfile.open();
// Populate matrix with numbers.
result.resize(rows,cols);
char *line=NULL;
char delim=' ';
for (int i = 0; i < rows; i++){
line = matfile.getline();
char *ptr = line;
char *olds = line;
char olddelim = delim;
int j=0;
while(olddelim && *line) {
while(*line && (delim != *line)) line++;
*line ^= olddelim = *line; // olddelim = *line; *line = 0;
result(i,j++) = atof(olds);
*line++ ^= olddelim; // *line = olddelim; line++;
olds = line;
}
free(ptr);
}
// closing file
matfile.close();
};
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();
}
