void compressFeature( string filename, std::vector< std::vector<float> > &models, const int dim, bool ascii ){ PCA pca; pca.read( filename.c_str(), ascii ); VectorXf variance = pca.getVariance(); MatrixXf tmpMat = pca.getAxis(); MatrixXf tmpMat2 = tmpMat.block(0,0,tmpMat.rows(),dim); const int num = (int)models.size(); for( int i=0; i<num; i++ ){ Map<VectorXf> vec( &(models[i][0]), models[i].size() ); //vec = tmpMat2.transpose() * vec; 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]; } } }
//******************************** //* 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; }
//******************************** //* 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; }