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
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);
}

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);
		}
	}