Exemplo n.º 1
0
Eigen::MatrixXf CombineMipmaps(const std::vector<Eigen::MatrixXf>& mm)
{
	Eigen::MatrixXf r = Eigen::MatrixXf::Zero(Q*3*mm[0].rows()/2, Q*mm[0].cols());
	r.block(0, 0, Q*mm[0].rows(), Q*mm[0].cols()) = density::ScaleUp(mm[0], Q);
	unsigned int y = 0;
	for(unsigned int i=1; i<mm.size(); ++i) {
		r.block(Q*mm[0].rows(), y, Q*mm[i].rows(), Q*mm[i].cols()) = density::ScaleUp(mm[i], Q);
		y += Q*mm[i].cols();
	}
	return r;
}
Exemplo n.º 2
0
		/** Randomly selects a point in the given tree node by considering probabilities 
		 * Runtime: O(S*S + log(S*S))
		 */
		inline Eigen::Vector2f ProbabilityCellPoint(const Eigen::MatrixXf& m0, int scale, int x, int y)
		{
			x *= scale;
			y *= scale;
			const auto& b = m0.block(x, y, scale, scale);
			// build cdf
			std::vector<float> cdf(scale*scale);
			for(int i=0; i<scale; ++i) {
				for(int j=0; j<scale; ++j) {
					float v = b(j,i);
					int q = scale*i + j;
					if(q > 0) {
						cdf[q] = cdf[q-1] + v;
					}
					else {
						cdf[q] = v;
					}
				}
			}
			// sample in cdf
			boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > rnd(
					Rnd(), boost::uniform_real<float>(0.0f, cdf.back()));
			float v = rnd();
			// find sample
			auto it = std::lower_bound(cdf.begin(), cdf.end(), v);
			int pos = std::distance(cdf.begin(), it);
			return Eigen::Vector2f(x + pos%scale, y + pos/scale);
		}
Exemplo n.º 3
0
	bool ZMeshAlgorithms::runBilateralFiltering(float sigma_r, float sigma_s)
	{

		int nSize = mesh_->get_num_of_faces_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, spatialDim, nSize, rangeDim) = faceNormals;
		AnnWarper_Eigen annSearch;
		annSearch.init(faceCenters);
		ZMeshBilateralFilter filter(mesh_);
		filter.setAnnSearchHandle(&annSearch);
		float sigma_spatial = mesh_->m_minEdgeLength*sigma_s;
		float sigma_range = cos(sigma_r*Z_PI/180.f);
		//filter.setKernelFunc(NULL);
		filter.setPara(ZMeshFilterParaNames::SpatialSigma, sigma_spatial);
		filter.setPara(ZMeshFilterParaNames::RangeSigma, sigma_range);
		filter.apply(faceAttributes, std::vector<bool>(nSize, true));
		Eigen::MatrixXf output = filter.getResult();

		MeshTools::setAllFaceNormals2(mesh_, output.block(0, 3, nSize, 3));
		//MeshTools::setAllFaceNormal2Spherical(mesh_, output.block(0, spatialDim, nSize, rangeDim));

		return true;
	}
Exemplo n.º 4
0
	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;
	}
Exemplo n.º 5
0
	void ZMeshAlgorithms::updateRange()
	{
		pMeshFilterManifold_->updateRange();
		Eigen::MatrixXf output = pMeshFilterManifold_->getResult();
		meshNewNormals_ = output.block(0, pMeshFilterManifold_->getSpatialDim(), pMeshFilterManifold_->getPointSize(), pMeshFilterManifold_->getRangeDim());
		MeshTools::setAllFaceNormals2(mesh_, meshNewNormals_);
		//MeshTools::setAllFaceNormal2Spherical(mesh_, meshNewNormals_);
	}
Exemplo n.º 6
0
	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;
	}
Exemplo n.º 7
0
	void ZMeshFilterManifold::init(const Eigen::MatrixXf& input)
	{
		destroy();
		computeTreeHeight();
		initMinPixelDist2Manifold();
		// init search tool
		pAnnSearch_ = new AnnWarper_Eigen;
		//Eigen::MatrixXf faceCenters = MeshTools::getAllFaceCenters(pMesh_);
		//Eigen::MatrixXf faceNormals = MeshTools::getAllFaceNormals(pMesh_);
		pAnnSearch_->init(input.block(0, 0, input.rows(), spatialDim_));
		// init gaussian filter
		pGaussianFilter_ = new ZMeshBilateralFilter(pMesh_);
		ZMeshBilateralFilter* pFilter = (ZMeshBilateralFilter*)pGaussianFilter_;
		pFilter->setAnnSearchHandle(pAnnSearch_);
	}
Exemplo n.º 8
0
template <typename PointT> void
pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
                       const Eigen::Vector4f &centroid,
                       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 ();
}
Exemplo n.º 9
0
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();
    }    
  }
}
Exemplo n.º 10
0
/**
 * DemeanPoints
 */
void RigidTransformationRANSAC::DemeanPoints (
      const std::vector<Eigen::Vector3f > &inPts, 
      const std::vector<int> &indices,
      const Eigen::Vector3f &centroid,
      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();
}
Exemplo n.º 11
0
		/** 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);
		}
Exemplo n.º 12
0
		/** Randomly selects a point in the given tree node by considering probabilities
		 * Assumes that cdf_sum is the probability sum in the given tree node
		 * Runtime: O(S*S/2)
		 */
		inline Eigen::Vector2f ProbabilityCellPoint(const Eigen::MatrixXf& m0, int scale, int x, int y, float cdf_sum)
		{
			// sample in cdf
			boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > rnd(
					Rnd(), boost::uniform_real<float>(0.0f, cdf_sum));
			float v = rnd();
			// find sample
			x *= scale;
			y *= scale;
			const auto& b = m0.block(x, y, scale, scale);
			for(int i=0; i<scale; ++i) {
				for(int j=0; j<scale; ++j) {
					v -= b(j,i);
					if(v <= 0.0f) {
						return Eigen::Vector2f(x + j, y + i);
					}
				}
			}
			// should never be here
			assert(false);
			return Eigen::Vector2f(x + scale/2, y + scale/2);
		}
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];
    }
  }
}
Eigen::MatrixXf LQRControler::Controler(Eigen::MatrixXf states,Eigen::MatrixXf ref,bool stop){

	if(stop){
		xs.setZero();
		xr.setZero();
		deltaxsi.setZero();
		xsi.setZero();
		xsiant.setZero();
		deltaxsi.setZero();
		deltaxsiant.setZero();
		deltaxs.setZero();
		xs_aumented.setZero();
		deltaxs.setZero();
		xsi.setZero();
		deltaU.setZero();
		xs_aumented.setZero();
		ur.setZero();
		auxu.setZero();
		ur.setZero();
		deltaU.setZero();
		xsiant.setZero();
		xsi.setZero();
		deltaxsiant.setZero();
		deltaxsi.setZero();
	}else{
		//Vectors of reference trajectory and control
		xs<<0,0,states(2),states.block(3,0,5,1),0,0,states(10),states.block(11,0,5,1);
		xr=trajectory->TrajetoryReference_LQR(ref);

		//Vector integration of error(Trapezoidal method)
		deltaxsi<<xs(2,0)-xr(2,0),xs(5,0)-xr(5,0);
		xsi=xsiant+ts*(deltaxsi+deltaxsiant)/2;

		// Error state vector
		deltaxs=xs-xr;
		// augmented error state vector
		xs_aumented<<deltaxs,xsi;
		//Control action variation
		deltaU=Ke*xs_aumented;
		//Control reference
		ur<<9857.54,9837.48,0,0;
		// Total control action
		auxu=ur+deltaU;
		//Variable update
		xsiant=xsi;
		deltaxsiant=deltaxsi;
	}

	if(auxu(0,0)>15000 ){
		auxu(0,0)=15000;
	}
	if(auxu(1,0)>15000 ){
		auxu(1,0)=15000;
	}
	/*The mass in the mathematical model was taken in grams,
	for this reason the controller calculate the forces in g .m/s^2 and the torque in g .m^2/s^2.
	But, the actuators are in the international units N and N. m for this reason the controls
	actions are transforming from g to Kg*/
	u(0,0)=auxu(0,0)/1000;
	u(1,0)=auxu(1,0)/1000;
	u(2,0)=auxu(2,0)/1000;
	u(3,0)=auxu(3,0)/1000;

	return u;
}
Exemplo n.º 15
0
template<typename PointT> void
pcl::registration::LUM<PointT>::compute ()
{
  int n = static_cast<int> (getNumVertices ());
  if (n < 2)
  {
    PCL_ERROR("[pcl::registration::LUM::compute] The slam graph needs at least 2 vertices.\n");
    return;
  }
  for (int i = 0; i < max_iterations_; ++i)
  {
    // Linearized computation of C^-1 and C^-1*D and convergence checking for all edges in the graph (results stored in slam_graph_)
    typename SLAMGraph::edge_iterator e, e_end;
    for (boost::tuples::tie (e, e_end) = edges (*slam_graph_); e != e_end; ++e)
      computeEdge (*e);

    // Declare matrices G and B
    Eigen::MatrixXf G = Eigen::MatrixXf::Zero (6 * (n - 1), 6 * (n - 1));
    Eigen::VectorXf B = Eigen::VectorXf::Zero (6 * (n - 1));

    // Start at 1 because 0 is the reference pose
    for (int vi = 1; vi != n; ++vi)
    {
      for (int vj = 0; vj != n; ++vj)
      {
        // Attempt to use the forward edge, otherwise use backward edge, otherwise there was no edge
        Edge e;
        bool present1, present2;
        boost::tuples::tie (e, present1) = edge (vi, vj, *slam_graph_);
        if (!present1)
        {
          boost::tuples::tie (e, present2) = edge (vj, vi, *slam_graph_);
          if (!present2)
            continue;
        }

        // Fill in elements of G and B
        if (vj > 0)
          G.block (6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_;
        G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_;
        B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_;
      }
    }

    // Computation of the linear equation system: GX = B
    // TODO investigate accuracy vs. speed tradeoff and find the best solving method for our type of linear equation (sparse)
    Eigen::VectorXf X = G.colPivHouseholderQr ().solve (B);

    // Update the poses
    float sum = 0.0;
    for (int vi = 1; vi != n; ++vi)
    {
      Eigen::Vector6f difference_pose = static_cast<Eigen::Vector6f> (-incidenceCorrection (getPose (vi)).inverse () * X.segment (6 * (vi - 1), 6));
      sum += difference_pose.norm ();
      setPose (vi, getPose (vi) + difference_pose);
    }

    // Convergence check
    if (sum <= convergence_threshold_ * static_cast<float> (n - 1))
      return;
  }
}
//********************************
//* main
int main(int argc, char* argv[]) {
  if( (argc != 13) && (argc != 15) ){
    std::cerr << "usage: " << argv[0] << " [path] <rank_num> <exist_voxel_num_threshold> [model_pca_filename] <dim_model> <size1> <size2> <size3> <detect_th> <distance_th> <model_num> /input:=/camera/rgb/points" << std::endl;
    exit( EXIT_FAILURE );
  }
  char tmpname[ 1000 ];
  ros::init (argc, argv, "detect_object_vosch_multi", ros::init_options::AnonymousName);

  // read the length of voxel side
  sprintf( tmpname, "%s/param/parameters.txt", argv[1] );
  voxel_size = Param::readVoxelSize( tmpname );

  detect_th = atof( argv[9] );
  distance_th = atof( argv[10] );
  model_num = atoi( argv[11] );
  rank_num = atoi( argv[2] );

  // set marker color
  const int marker_model_num = 6;
  if( model_num > marker_model_num ){
    std::cerr << "Please set more marker colors for detection of more than " << marker_model_num << " objects." << std::endl;
    exit( EXIT_FAILURE );
  }
  marker_color_r = new float[ marker_model_num ];
  marker_color_g = new float[ marker_model_num ];
  marker_color_b = new float[ marker_model_num ];
  marker_color_r[ 0 ] = 1.0; marker_color_g[ 0 ] = 0.0; marker_color_b[ 0 ] = 0.0;  // red
  marker_color_r[ 1 ] = 0.0; marker_color_g[ 1 ] = 1.0; marker_color_b[ 1 ] = 0.0;  // green
  marker_color_r[ 2 ] = 0.0; marker_color_g[ 2 ] = 0.0; marker_color_b[ 2 ] = 1.0;  // blue
  marker_color_r[ 3 ] = 1.0; marker_color_g[ 3 ] = 1.0; marker_color_b[ 3 ] = 0.0;  // yellow
  marker_color_r[ 4 ] = 0.0; marker_color_g[ 4 ] = 1.0; marker_color_b[ 4 ] = 1.0;  // cyan
  marker_color_r[ 5 ] = 1.0; marker_color_g[ 5 ] = 0.0; marker_color_b[ 5 ] = 1.0;  // magenta
  // marker_color_r[ 0 ] = 0.0; marker_color_g[ 0 ] = 1.0; marker_color_b[ 0 ] = 0.0; // green
  // marker_color_r[ 1 ] = 0.0; marker_color_g[ 1 ] = 0.0; marker_color_b[ 1 ] = 1.0; // blue
  // marker_color_r[ 2 ] = 0.0; marker_color_g[ 2 ] = 1.0; marker_color_b[ 2 ] = 1.0; // cyan
  // marker_color_r[ 3 ] = 1.0; marker_color_g[ 3 ] = 0.0; marker_color_b[ 3 ] = 0.0; // pink

  // read the number of voxels in each subdivision's side of scene
  box_size = Param::readBoxSizeScene( tmpname );

  // read the dimension of compressed feature vectors
  dim = Param::readDim( tmpname );
  const int dim_model = atoi(argv[5]);
  if( dim <= dim_model ){
    std::cerr << "ERR: dim_model should be less than dim(in dim.txt)" << std::endl;
    exit( EXIT_FAILURE );
  }

  // read the threshold for RGB binalize
  sprintf( tmpname, "%s/param/color_threshold.txt", argv[1] );
  Param::readColorThreshold( color_threshold_r, color_threshold_g, color_threshold_b, tmpname );

  // determine the size of sliding box
  region_size = box_size * voxel_size;
  float tmp_val = atof(argv[6]) / region_size;
  int size1 = (int)tmp_val;
  if( ( ( tmp_val - size1 ) >= 0.5 ) || ( size1 == 0 ) ) size1++;
  tmp_val = atof(argv[7]) / region_size;
  int size2 = (int)tmp_val;
  if( ( ( tmp_val - size2 ) >= 0.5 ) || ( size2 == 0 ) ) size2++;
  tmp_val = atof(argv[8]) / region_size;
  int size3 = (int)tmp_val;
  if( ( ( tmp_val - size3 ) >= 0.5 ) || ( size3 == 0 ) ) size3++;
  sliding_box_size_x = size1 * region_size;
  sliding_box_size_y = size2 * region_size;
  sliding_box_size_z = size3 * region_size;

  // set variables
  search_obj.setModelNum( model_num );
#ifdef CCHLAC_TEST
  sprintf( tmpname, "%s/param/max_c.txt", argv[1] );
#else
  sprintf( tmpname, "%s/param/max_r.txt", argv[1] );
#endif
  search_obj.setNormalizeVal( tmpname );
  search_obj.setRange( size1, size2, size3 );
  search_obj.setRank( rank_num * model_num ); // for removeOverlap()
  search_obj.setThreshold( atoi(argv[3]) );

  // read projection axes of the target objects' subspace
  FILE *fp = fopen( argv[4], "r" );
  char **model_file_names = new char* [ model_num ];
  char line[ 1000 ];
  for( int i=0; i<model_num; i++ ){
    model_file_names[ i ] = new char [ 1000 ];
    if( fgets( line, sizeof(line), fp ) == NULL ) std::cerr<<"fgets err"<<std::endl;
    line[ strlen( line ) - 1 ] = '\0';
    //fscanf( fp, "%s\n", model_file_names + i );
    //model_file_names[ i ] = line;
    sprintf( model_file_names[ i ], "%s", line );
    //std::cout << model_file_names[ i ] << std::endl;
  }
  fclose(fp);
  search_obj.readAxis( model_file_names, dim, dim_model, ASCII_MODE_P, MULTIPLE_SIMILARITY );

  // read projection axis for feature compression
  PCA pca;
  sprintf( tmpname, "%s/models/compress_axis", argv[1] );
  pca.read( tmpname, ASCII_MODE_P );
  Eigen::MatrixXf tmpaxis = pca.getAxis();
  Eigen::MatrixXf axis = tmpaxis.block( 0,0,tmpaxis.rows(),dim );
  Eigen::MatrixXf axis_t = axis.transpose();
  Eigen::VectorXf variance = pca.getVariance();
  if( WHITENING )
    search_obj.setSceneAxis( axis_t, variance, dim );
  else
    search_obj.setSceneAxis( axis_t );

  // object detection
  VoxelizeAndDetect vad;
  vad.loop();
  ros::spin();

  return 0;
}
Exemplo n.º 17
0
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");
    }
}
Exemplo n.º 18
0
//********************************
//* main
int main(int argc, char* argv[]) {
  if( (argc != 12) && (argc != 14) ){
    std::cerr << "usage: " << argv[0] << " [path] <rank_num> <exist_voxel_num_threshold> [model_pca_filename] <dim_model> <size1> <size2> <size3> <detect_th> <distance_th> /input:=/camera/rgb/points" << std::endl;
    exit( EXIT_FAILURE );
  }
  char tmpname[ 1000 ];
  ros::init (argc, argv, "detectObj", ros::init_options::AnonymousName);

  // read the length of voxel side
  sprintf( tmpname, "%s/param/parameters.txt", argv[1] );
  voxel_size = Param::readVoxelSize( tmpname );

  detect_th = atof( argv[9] );
  distance_th = atof( argv[10] );
  rank_num = atoi( argv[2] );

  // read the number of voxels in each subdivision's side of scene
  box_size = Param::readBoxSizeScene( tmpname );

  // read the dimension of compressed feature vectors
  dim = Param::readDim( tmpname );

  // set the dimension of the target object's subspace
  const int dim_model = atoi(argv[5]);
  if( dim <= dim_model ){
    std::cerr << "ERR: dim_model should be less than dim(in dim.txt)" << std::endl;
    exit( EXIT_FAILURE );
  }

  // read the threshold for RGB binalize
  sprintf( tmpname, "%s/param/color_threshold.txt", argv[1] );
  Param::readColorThreshold( color_threshold_r, color_threshold_g, color_threshold_b, tmpname );

  // determine the size of sliding box
  region_size = box_size * voxel_size;
  float tmp_val = atof(argv[6]) / region_size;
  int size1 = (int)tmp_val;
  if( ( ( tmp_val - size1 ) >= 0.5 ) || ( size1 == 0 ) ) size1++;
  tmp_val = atof(argv[7]) / region_size;
  int size2 = (int)tmp_val;
  if( ( ( tmp_val - size2 ) >= 0.5 ) || ( size2 == 0 ) ) size2++;
  tmp_val = atof(argv[8]) / region_size;
  int size3 = (int)tmp_val;
  if( ( ( tmp_val - size3 ) >= 0.5 ) || ( size3 == 0 ) ) size3++;
  sliding_box_size_x = size1 * region_size;
  sliding_box_size_y = size2 * region_size;
  sliding_box_size_z = size3 * region_size;

  // set variables
  search_obj.setRange( size1, size2, size3 );
  search_obj.setRank( rank_num );
  search_obj.setThreshold( atoi(argv[3]) );
  search_obj.readAxis( argv[4], dim, dim_model, ASCII_MODE_P, MULTIPLE_SIMILARITY );

  // read projection axis of the target object's subspace
  PCA pca;
  sprintf( tmpname, "%s/models/compress_axis", argv[1] );
  pca.read( tmpname, ASCII_MODE_P );
  Eigen::MatrixXf tmpaxis = pca.getAxis();
  Eigen::MatrixXf axis = tmpaxis.block( 0,0,tmpaxis.rows(),dim );
  Eigen::MatrixXf axis_t = axis.transpose();
  Eigen::VectorXf variance = pca.getVariance();
  if( WHITENING )
    search_obj.setSceneAxis( axis_t, variance, dim );
  else
    search_obj.setSceneAxis( axis_t );

  // object detection
  VoxelizeAndDetect vad;
  vad.loop();
  ros::spin();

  return 0;
}
Exemplo n.º 19
0
void linearTMatrixTest(StrainLin * ene)
{
  ElementRegGrid * grid = new ElementRegGrid(1, 1, 1);
  MaterialQuad * material = new MaterialQuad(ene);
  grid->m.push_back(material);
  grid->x[1][0] += 0.1f;
  grid->x[3][1] += 0.2f;
  MatrixXf K = grid->getStiffness();

  //linear material stiffness
  ElementHex * ele = (ElementHex*)grid->e[0];
  const Quadrature & q = Quadrature::Gauss2;
  Eigen::MatrixXf Ka = Eigen::MatrixXf::Zero(3 * ele->nV(), 3 * ele->nV());
  Eigen::MatrixXf E = ene->EMatrix();
  Eigen::VectorXf U = Eigen::VectorXf::Zero(3 * ele->nV());

  for (int ii = 0; ii < ele->nV(); ii++){
    for (int jj = 0; jj < 3; jj++){
      U[3 * ii + jj] = grid->x[ii][jj] - grid->X[ii][jj];
    }
  }

  for (unsigned int ii = 0; ii < q.x.size(); ii++){
    Eigen::MatrixXf B = ele->BMatrix(q.x[ii], grid->X);
    Eigen::MatrixXf ss = E*B*U;
    //std::cout <<"sigma:\n"<< ss << "\n";

    Matrix3f F = ele->defGrad(q.x[ii], grid->X, grid->x);
    Matrix3f P = ene->getPK1(F);
    //std::cout << "P:\n";
    //P.print();

    Ka += q.w[ii] * B.transpose() * E * B;
    //std::cout << "B:\n" << B << "\n";
  }

  //std::cout << "E:\n" << E << "\n";
  //std::cout << "alt K:\n";
  //std::cout << Ka << "\n";
  float maxErr = 0;
  for (int ii = 0; ii<K.mm; ii++){
    for (int jj = 0; jj<K.nn; jj++){
      float err = (float)std::abs(Ka(ii, jj) - K(ii, jj));
      if (err>maxErr){
        maxErr = err;
      }
    }
  }

  std::cout << "max err " << maxErr << "\n";

  //assemble boundary matrix HNEB
  std::ofstream out("T.txt");

  // 2 point quadrature is accurate enough
  const Quadrature & q2d = Quadrature::Gauss2_2D;
  Eigen::MatrixXf T = Eigen::MatrixXf::Zero(3 * ele->nV(), 3 * ele->nV());
  for (int ii = 0; ii < ele->nF(); ii++){
    Eigen::MatrixXf Tf = Eigen::MatrixXf::Zero(3 * ele->nV(), 3 * ele->nV());
    Eigen::MatrixXf N = ele->NMatrix(ii);
    N.block(0, 3, 3, 3) = Eigen::MatrixXf::Zero(3, 3);
    //std::cout << "N:\n"<<N << "\n";
    for (unsigned int jj = 0; jj < q2d.x.size(); jj++){
      Vector3f quadp = ele->facePt(ii, q2d.x[jj]);
      Eigen::MatrixXf B0 = ele->BMatrix(quadp, grid->X);
      Eigen::MatrixXf B = Eigen::MatrixXf::Zero(6, 3 * ele->nV());
      //only add contributions from the face
      for (int kk = 0; kk < 4; kk++){
        int vidx = faces[ii][kk];
        B.block(0, 3 * vidx, 6, 3) = B0.block(0, 3 * vidx, 6, 3);
      }
      //B=B0;
      Eigen::MatrixXf H = ele->HMatrix(quadp);
      //std::cout << "H:\n" << H.transpose() << "\n";
      //std::cout << "B:\n" << B.transpose() << "\n";
      //std::cout << "traction:\n";
      //Tf += q2d.w[jj] * H.transpose() * N * E * B;
      Tf += q2d.w[jj] * H.transpose() * N * E * N.transpose() * H;
      //Tf += q2d.w[jj] * B.transpose() * E * B;
      Eigen::Vector3f surfF = (N * E * B * U);
      //std::cout << surfF << "\n";
      Matrix3f F = ele->defGrad(quadp, grid->X, grid->x);
      Matrix3f P = ene->getPK1(F);
      Vector3f surfF1 = P * Vector3f(facen[ii][0], facen[ii][1], facen[ii][2]);
      std::cout << surfF1[0] << " " << surfF1[1] << " " << surfF1[2] << "\n";
    }
    //out << Tf << "\n\n";
    T += Tf;
  }
  //out << T << "\n\n";
  //out << Ka << "\n";
  out.close();
}
Exemplo n.º 20
0
	bool ZMeshFilterManifold::buildManifoldAndPerformFiltering(const Eigen::MatrixXf& input, 
		const Eigen::MatrixXf& etaK, const std::vector<bool>& clusterK, 
		float sigma_s, float sigma_r, int currentTreeLevel)
	{
		int inputSize = input.rows();;

		static std::string fileName("etaK.txt");
		fileName += "0";
		ZFileHelper::saveEigenMatrixToFile(fileName.c_str(), etaK);

		// splatting: project the vertices onto the current manifold etaK

		// NOTE: the sigma should be recursive!!
		float sigmaR_over_sqrt_2 = sigma_r/sqrt(2.0);
		Eigen::MatrixXf diffX = input.block(0, spatialDim_, inputSize, rangeDim_) - etaK.block(0, spatialDim_, inputSize, rangeDim_);
		Eigen::VectorXf gaussianDistWeight(inputSize);
		//std::cout << "diffX=\n" << diffX.block(0,0,10,rangeDim_+spatialDim_) << "\n";
		for (int i=0; i<inputSize; i++)
		{
			gaussianDistWeight(i) = ZKernelFuncs::GaussianKernelFunc(diffX.row(i), sigmaR_over_sqrt_2);
		}

		Eigen::MatrixXf psiSplat(inputSize, spatialDim_+rangeDim_+1);
		Eigen::VectorXf psiSplat0 = gaussianDistWeight;
		//////////////////////////////////////////////////////////////////////////
		// for debug
		//std::stringstream ss;
		static int etaI = 1;
		//ss << "gaussianWeights" << etaI << ".txt";
		//std::cout << ZConsoleTools::green << etaI << "\n" << ZConsoleTools::white;
		etaI++;
		//ZFileHelper::saveEigenVectorToFile(ss.str().c_str(), gaussianDistWeight);
		//////////////////////////////////////////////////////////////////////////
		psiSplat.block(0,0,inputSize,spatialDim_) = input.block(0,0,inputSize,spatialDim_);
		for (int i=0; i<inputSize; i++)
		{
			//psiSplat.block(i, spatialDim_, 1, rangeDim_) = input.block(i, spatialDim_, 1, rangeDim_)*gaussianDistWeight(i);
			psiSplat.block(i, spatialDim_, 1, rangeDim_) = etaK.block(i, spatialDim_, 1, rangeDim_)*gaussianDistWeight(i);
		}
		psiSplat.col(spatialDim_+rangeDim_) = gaussianDistWeight;

		// save min distance to later perform adjustment of outliers -- Eq.(10)
		// UNDO
		// update min_pixel_dist_to_manifold_square_
		Eigen::VectorXf pixelDist2Manifold(inputSize);
		for (int i=0; i<inputSize; i++)
		{
			if (clusterK[i])
				pixelDist2Manifold(i) = diffX.row(i).squaredNorm();
			else
				pixelDist2Manifold(i) = FLT_MAX;
		}
		min_pixel_dist_to_manifold_squared_ = min_pixel_dist_to_manifold_squared_.cwiseMin(pixelDist2Manifold);

		// Blurring
		Eigen::MatrixXf wkiPsiBlur(inputSize, rangeDim_);
		Eigen::VectorXf wkiPsiBlur0(inputSize);
		//Eigen::MatrixXf psiSplatAnd0(inputSize, spatialDim_+rangeDim_+1);
		//psiSplatAnd0.block(0, 0, inputSize, spatialDim_+rangeDim_) = psiSplat.block(0, 0, inputSize, spatialDim_+rangeDim_);
		//psiSplatAnd0.col(spatialDim_+rangeDim_) = psiSplat0;
		//psiSplatAnd0.block(0, 0, inputSize, spatialDim_+rangeDim_) = input;
		//psiSplatAnd0.col(spatialDim_+rangeDim_).fill(1.f);
		//pGaussianFilter_->setKernelFunc(ZKernelFuncs::GaussianKernelFuncNormal3);
		pGaussianFilter_->setKernelFunc(ZKernelFuncs::GaussianKernelFuncA);
		//pGaussianFilter_->setKernelFunc(NULL);
		//pGaussianFilter_->setKernelFunc(ZKernelFuncs::GaussianKernelSphericalFunc);
		pGaussianFilter_->setPara(ZMeshFilterParaNames::SpatialSigma, sigma_s);
		pGaussianFilter_->setPara(ZMeshFilterParaNames::RangeSigma, sigmaR_over_sqrt_2);
		//pGaussianFilter_->apply(psiSplatAnd0, clusterK);
		//CHECK_FALSE_AND_RETURN(pGaussianFilter_->apply(psiSplatAnd0, spatialDim_, rangeDim_+1, Eigen::VectorXf(), clusterK));
		CHECK_FALSE_AND_RETURN(pGaussianFilter_->apply(psiSplat, spatialDim_, rangeDim_+1, gaussianDistWeight, clusterK));
		Eigen::MatrixXf wkiPsiBlurAnd0 = pGaussianFilter_->getResult();
		wkiPsiBlur = wkiPsiBlurAnd0.block(0, spatialDim_, inputSize, rangeDim_);
		wkiPsiBlur0 = wkiPsiBlurAnd0.col(spatialDim_+rangeDim_);
		//ZFileHelper::saveEigenMatrixToFile("")
// 		std::cout << "wkiPsiBlurAnd0=\n" << wkiPsiBlurAnd0.block(0,0,10,rangeDim_+spatialDim_+1) << "\n";
// 		std::cout << "wkiPsiBlur0=\n" << wkiPsiBlur0.head(10) << "\n";

		//std::cout << "pixelDist2Manifold=\n" << pixelDist2Manifold.head(10) << "\n";
		//std::cout << "min_pixel_dist_to_manifold_squared=\n" << min_pixel_dist_to_manifold_squared_.head(10) << "\n";

		// for debug
// 		for (int i=0; i<inputSize; i++)
// 		{
// 			if (!g_isInfinite(wkiPsiBlur(i,0)))
// 				std::cout << "(" << i << "," << 0 << ")\n";
// 			if (!g_isInfinite(wkiPsiBlur(i,1)))
// 				std::cout << "(" << i << "," << 1 << ")\n";
// 			//if (!g_isInfinite(wkiPsiBlur(i,2)))
// 			//	std::cout << "(" << i << "," << 2 << ")\n";
// 		}
		//std::cout << wkiPsiBlur.norm() << "\n";

		Eigen::VectorXf rangeDiff(inputSize);
		for (int i=0; i<inputSize; i++)
		{
			Eigen::VectorXf n0 = wkiPsiBlur.row(i);
			Eigen::VectorXf n1 = input.row(i).tail(rangeDim_);
			n0.normalize();
			n1.normalize();
			rangeDiff(i) = 1.f-n0.dot(n1);
		}
		static bool bSaved = false;
		if (!bSaved)
		{
			ZFileHelper::saveEigenVectorToFile("rangeDiff.txt", rangeDiff);
			ZFileHelper::saveEigenVectorToFile("gaussian.txt", gaussianDistWeight);
			ZFileHelper::saveEigenMatrixToFile("splat.txt", psiSplat);
			ZFileHelper::saveEigenMatrixToFile("blur.txt", wkiPsiBlur);
			bSaved = true;
		}

		// Slicing
		Eigen::VectorXf wki = gaussianDistWeight;
		for (int i=0; i<inputSize; i++)
		{
			if (!clusterK[i]) continue;
			sum_w_ki_Psi_blur_.row(i) += wkiPsiBlur.row(i)*wki(i);
			sum_w_ki_Psi_blur_0_(i) += wkiPsiBlur0(i)*wki(i);
		}
		//////////////////////////////////////////////////////////////////////////
		// for debug
		wki_Psi_blurs_.push_back(Eigen::MatrixXf(inputSize, rangeDim_));
		wki_Psi_blur_0s_.push_back(Eigen::VectorXf(inputSize));
		Eigen::MatrixXf& lastM = wki_Psi_blurs_[wki_Psi_blurs_.size()-1];
		lastM.fill(0);
		Eigen::VectorXf& lastV = wki_Psi_blur_0s_[wki_Psi_blur_0s_.size()-1];
		lastV.fill(0);
		for (int i=0; i<inputSize; i++)
		{
			if (!clusterK[i]) continue;
			lastM.row(i) = wkiPsiBlur.row(i)*wki(i);
			lastV(i) = wkiPsiBlur0(i)*wki(i);
		}
		std::cout << sum_w_ki_Psi_blur_.norm() << "\n";
		//////////////////////////////////////////////////////////////////////////

		// compute two new manifolds eta_minus and eta_plus

		// test stopping criterion
		if (currentTreeLevel<filterPara_.tree_height)
		{
			// algorithm 1, Step 2: compute the eigenvector v1
			Eigen::VectorXf v1 = computeMaxEigenVector(diffX, clusterK);

			// algorithm 1, Step 3: Segment vertices into two clusters
			std::vector<bool> clusterMinus(inputSize, false);
			std::vector<bool> clusterPlus(inputSize, false);
			int countMinus=0;
			int countPlus =0;
			for (int i=0; i<inputSize; i++)
			{
				float dot = diffX.row(i).dot(v1);
				if (dot<0 && clusterK[i]) {countMinus++; verticeClusterIds[i] =etaI+0.5; clusterMinus[i] = true;}
				if (dot>=0 && clusterK[i]) {countPlus++; verticeClusterIds[i] =etaI-0.5; clusterPlus[i] = true;}
			}
			std::cout << "Minus manifold: " << countMinus << "\n";
			std::cout << "Plus manifold: " << countPlus << "\n"; 
// 			Eigen::MatrixXf diffXManifold(inputSize, spatialDim_+rangeDim_);
// 			diffXManifold.block(0, 0, inputSize, spatialDim_) = input.block(0, 0, inputSize, spatialDim_);
// 			diffXManifold.block(0, spatialDim_, inputSize, rangeDim_) = diffX;

			// algorithm 1, Step 4: Compute new manifolds by weighted low-pass filtering  -- Eq. (7)(8)
			Eigen::VectorXf theta(inputSize);
			theta.fill(1);
			theta = theta - wki.cwiseProduct(wki);
			pGaussianFilter_->setKernelFunc(NULL);
			CHECK_FALSE_AND_RETURN(pGaussianFilter_->apply(input, spatialDim_, rangeDim_, theta, clusterMinus));
			Eigen::MatrixXf etaMinus = pGaussianFilter_->getResult();
			CHECK_FALSE_AND_RETURN(pGaussianFilter_->apply(input, spatialDim_, rangeDim_, theta, clusterPlus));
			Eigen::MatrixXf etaPlus = pGaussianFilter_->getResult();

			// algorithm 1, Step 5: recursively build more manifolds
			CHECK_FALSE_AND_RETURN(buildManifoldAndPerformFiltering(input, etaMinus, clusterMinus, sigma_s, sigma_r, currentTreeLevel+1));
			CHECK_FALSE_AND_RETURN(buildManifoldAndPerformFiltering(input, etaPlus, clusterPlus, sigma_s, sigma_r, currentTreeLevel+1));
		}

		return true;
	}
Exemplo n.º 21
0
    Eigen::MatrixXf CoMIK::getJacobianOfCoM(RobotNodePtr node)
    {
        // Get number of degrees of freedom
        size_t nDoF = rns->getAllRobotNodes().size();

        // Create matrices for the position and the orientation part of the jacobian.
        Eigen::MatrixXf position = Eigen::MatrixXf::Zero(3, nDoF);

        const std::vector<RobotNodePtr> parentsN = bodyNodeParents[node];

        // Iterate over all degrees of freedom
        for (size_t i = 0; i < nDoF; i++)
        {
            RobotNodePtr dof = rns->getNode(i);// bodyNodes[i];

            // Check if the tcp is affected by this DOF
            if (find(parentsN.begin(), parentsN.end(), dof) != parentsN.end())
            {
                // Calculus for rotational joints is different as for prismatic joints.
                if (dof->isRotationalJoint())
                {
                    // get axis
                    boost::shared_ptr<RobotNodeRevolute> revolute
                        = boost::dynamic_pointer_cast<RobotNodeRevolute>(dof);
                    THROW_VR_EXCEPTION_IF(!revolute, "Internal error: expecting revolute joint");
                    // todo: find a better way of handling different joint types
                    Eigen::Vector3f axis = revolute->getJointRotationAxis(coordSystem);

                    // For CoM-Jacobians only the positional part is necessary
                    Eigen::Vector3f toTCP = node->getCoMLocal() + node->getGlobalPose().block(0, 3, 3, 1)
                                            - dof->getGlobalPose().block(0, 3, 3, 1);
                    position.block(0, i, 3, 1) = axis.cross(toTCP);
                }
                else if (dof->isTranslationalJoint())
                {
                    // -> prismatic joint
                    boost::shared_ptr<RobotNodePrismatic> prismatic
                        = boost::dynamic_pointer_cast<RobotNodePrismatic>(dof);
                    THROW_VR_EXCEPTION_IF(!prismatic, "Internal error: expecting prismatic joint");
                    // todo: find a better way of handling different joint types
                    Eigen::Vector3f axis = prismatic->getJointTranslationDirection(coordSystem);

                    position.block(0, i, 3, 1) = axis;
                }
            }
        }

        if (target.rows() == 2)
        {
            Eigen::MatrixXf result(2, nDoF);
            result.row(0) = position.row(0);
            result.row(1) = position.row(1);
            return result;
        }
        else if (target.rows() == 1)
        {
            VR_INFO << "One dimensional CoMs not supported." << endl;
        }

        return position;
    }
Exemplo n.º 22
0
void KinematicGraph::projectConfigurationSpace(int reducedDOFs,std::vector<double> stamps, KinematicParams &params,KinematicData &data) {
	if(reducedDOFs == 0) return;
	if(stamps.size() == 0) return;

	map< GenericModelPtr, int> DOFoffsets;
	int DOFs = 0;
	for(KinematicGraph::iterator it=begin(); it != end();it++) {
		DOFoffsets[it->second] = DOFs;
		DOFs += it->second->getDOFs();
	}
	if(DOFs==0) return;

	MatrixXf X_trans(stamps.size(), DOFs);

	for(KinematicGraph::iterator it=begin(); it != end();it++) {
		GenericModelPtr model = it->second;
		int DOFoffset = DOFoffsets[model];

		if(model->getDOFs()==0) continue;

		for(size_t timestamp_idx = 0;timestamp_idx < stamps.size();timestamp_idx++) {
			int idx = data.poseIndex[ it->first.first ][it->first.second][ stamps[timestamp_idx]];

			VectorXd c = model->getConfiguration(idx);
			for(size_t d=0;d< model->getDOFs();d++) {
				X_trans( timestamp_idx,DOFoffset + d ) = c(d);
			}
		}
	}

	VectorXf X_mean = VectorXf::Zero(DOFs,1);
	for( size_t i=0;i< stamps.size();i++) {
		for( int j=0;j< DOFs;j++) {
			X_mean(j) += X_trans(i,j);
		}
	}
	X_mean /= stamps.size();


	// check for nans or large numbers (crash eigen while svd'ing)
	for( int j=0;j< DOFs;j++) {
		if(isnan((double)X_mean(j))) {
			cout << X_trans << endl<<endl;
			cout << X_mean << endl;
			cout <<"reducing DOFs failed, NaN value"<<endl;
			return;
		}
		if(fabs((double)X_mean(j))>100) {
			cout << X_trans << endl<<endl;
			cout << X_mean << endl;
			cout <<"reducing DOFs failed, large value"<<endl;
			return;
		}
	}

	MatrixXf X_center(stamps.size(), DOFs);
	for( size_t i=0;i< stamps.size();i++) {
		for( int j=0;j< DOFs;j++) {
			X_center(i,j) = X_trans(i,j) - X_mean(j);
		}
	}

	if(stamps.size() < (size_t)DOFs) return;

	JacobiSVD<MatrixXf> svdOfA(X_center);

	const Eigen::MatrixXf U = svdOfA.matrixU();
	const Eigen::MatrixXf V = svdOfA.matrixV();
	const Eigen::VectorXf S = svdOfA.singularValues();

	//DiagonalMatrix<Eigen::VectorXf> S2(S);
	Eigen::MatrixXf V_projection = V.block(0,0,DOFs,reducedDOFs);
	MatrixXf X_reduced(stamps.size(),reducedDOFs);

	for( size_t i=0;i< stamps.size();i++) {
		for( int k=0;k< reducedDOFs;k++) {
			X_reduced(i,k) = 0.0;
			for( int j=0;j< DOFs;j++) {
				X_reduced(i,k) += V(j,k) * X_center(i,j);
			}
		}
	}

	MatrixXf X_projected(stamps.size(),DOFs);
	for( size_t i=0;i< stamps.size();i++) {
		for( int j=0;j< DOFs;j++) {
			X_projected(i,j) = X_mean(j);
			for( int k=0;k< reducedDOFs;k++) {
				X_projected(i,j) += V(j,k) * X_reduced(i,k);
			}
		}
	}

	for(KinematicGraph::iterator it=begin(); it != end();it++) {
		GenericModelPtr model = it->second;
		if(model->getDOFs()==0) continue;
		int DOFoffset = DOFoffsets[model];

		for(size_t timestamp_idx = 0;timestamp_idx < stamps.size();timestamp_idx++) {
			int idx = data.poseIndex[ it->first.first ][it->first.second][ stamps[timestamp_idx]];

			VectorXd c(model->getDOFs());
			for(size_t d=0;d< model->getDOFs();d++) {
				c(d) =X_projected( timestamp_idx,DOFoffset + d );
			}

			model->setConfiguration(idx,c);
			model->model.track.pose_projected[idx] = model->predictPose( c );
		}
	}
	DOF = reducedDOFs;
}