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];
}
}
}
void run(int)
{
const Size sz(200, 500);
double diffPrjEps, diffBackPrjEps,
prjEps, backPrjEps,
evalEps, evecEps;
int maxComponents = 100;
double retainedVariance = 0.95;
Mat rPoints(sz, CV_32FC1), rTestPoints(sz, CV_32FC1);
RNG& rng = ts->get_rng();
rng.fill( rPoints, RNG::UNIFORM, Scalar::all(0.0), Scalar::all(1.0) );
rng.fill( rTestPoints, RNG::UNIFORM, Scalar::all(0.0), Scalar::all(1.0) );
PCA rPCA( rPoints, Mat(), CV_PCA_DATA_AS_ROW, maxComponents ), cPCA;
// 1. check C++ PCA & ROW
Mat rPrjTestPoints = rPCA.project( rTestPoints );
Mat rBackPrjTestPoints = rPCA.backProject( rPrjTestPoints );
Mat avg(1, sz.width, CV_32FC1 );
reduce( rPoints, avg, 0, CV_REDUCE_AVG );
Mat Q = rPoints - repeat( avg, rPoints.rows, 1 ), Qt = Q.t(), eval, evec;
Q = Qt * Q;
Q = Q /(float)rPoints.rows;
eigen( Q, eval, evec );
/*SVD svd(Q);
evec = svd.vt;
eval = svd.w;*/
Mat subEval( maxComponents, 1, eval.type(), eval.data ),
subEvec( maxComponents, evec.cols, evec.type(), evec.data );
#ifdef CHECK_C
Mat prjTestPoints, backPrjTestPoints, cPoints = rPoints.t(), cTestPoints = rTestPoints.t();
CvMat _points, _testPoints, _avg, _eval, _evec, _prjTestPoints, _backPrjTestPoints;
#endif
// check eigen()
double eigenEps = 1e-6;
double err;
for(int i = 0; i < Q.rows; i++ )
{
Mat v = evec.row(i).t();
Mat Qv = Q * v;
Mat lv = eval.at<float>(i,0) * v;
err = cvtest::norm( Qv, lv, NORM_L2 );
if( err > eigenEps )
{
ts->printf( cvtest::TS::LOG, "bad accuracy of eigen(); err = %f\n", err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
return;
}
}
// check pca eigenvalues
evalEps = 1e-6, evecEps = 1e-3;
err = cvtest::norm( rPCA.eigenvalues, subEval, NORM_L2 );
if( err > evalEps )
{
ts->printf( cvtest::TS::LOG, "pca.eigenvalues is incorrect (CV_PCA_DATA_AS_ROW); err = %f\n", err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
return;
}
// check pca eigenvectors
for(int i = 0; i < subEvec.rows; i++)
{
Mat r0 = rPCA.eigenvectors.row(i);
Mat r1 = subEvec.row(i);
err = cvtest::norm( r0, r1, CV_L2 );
if( err > evecEps )
{
r1 *= -1;
double err2 = cvtest::norm(r0, r1, CV_L2);
if( err2 > evecEps )
{
Mat tmp;
absdiff(rPCA.eigenvectors, subEvec, tmp);
double mval = 0; Point mloc;
minMaxLoc(tmp, 0, &mval, 0, &mloc);
ts->printf( cvtest::TS::LOG, "pca.eigenvectors is incorrect (CV_PCA_DATA_AS_ROW); err = %f\n", err );
ts->printf( cvtest::TS::LOG, "max diff is %g at (i=%d, j=%d) (%g vs %g)\n",
mval, mloc.y, mloc.x, rPCA.eigenvectors.at<float>(mloc.y, mloc.x),
subEvec.at<float>(mloc.y, mloc.x));
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
return;
}
}
}
prjEps = 1.265, backPrjEps = 1.265;
for( int i = 0; i < rTestPoints.rows; i++ )
{
// check pca project
Mat subEvec_t = subEvec.t();
Mat prj = rTestPoints.row(i) - avg; prj *= subEvec_t;
err = cvtest::norm(rPrjTestPoints.row(i), prj, CV_RELATIVE_L2);
if( err > prjEps )
{
ts->printf( cvtest::TS::LOG, "bad accuracy of project() (CV_PCA_DATA_AS_ROW); err = %f\n", err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
return;
}
// check pca backProject
Mat backPrj = rPrjTestPoints.row(i) * subEvec + avg;
err = cvtest::norm( rBackPrjTestPoints.row(i), backPrj, CV_RELATIVE_L2 );
if( err > backPrjEps )
{
ts->printf( cvtest::TS::LOG, "bad accuracy of backProject() (CV_PCA_DATA_AS_ROW); err = %f\n", err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
return;
}
}
// 2. check C++ PCA & COL
cPCA( rPoints.t(), Mat(), CV_PCA_DATA_AS_COL, maxComponents );
diffPrjEps = 1, diffBackPrjEps = 1;
Mat ocvPrjTestPoints = cPCA.project(rTestPoints.t());
err = cvtest::norm(cv::abs(ocvPrjTestPoints), cv::abs(rPrjTestPoints.t()), CV_RELATIVE_L2 );
if( err > diffPrjEps )
{
ts->printf( cvtest::TS::LOG, "bad accuracy of project() (CV_PCA_DATA_AS_COL); err = %f\n", err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
return;
}
err = cvtest::norm(cPCA.backProject(ocvPrjTestPoints), rBackPrjTestPoints.t(), CV_RELATIVE_L2 );
if( err > diffBackPrjEps )
{
ts->printf( cvtest::TS::LOG, "bad accuracy of backProject() (CV_PCA_DATA_AS_COL); err = %f\n", err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
return;
}
// 3. check C++ PCA w/retainedVariance
cPCA( rPoints.t(), Mat(), CV_PCA_DATA_AS_COL, retainedVariance );
diffPrjEps = 1, diffBackPrjEps = 1;
Mat rvPrjTestPoints = cPCA.project(rTestPoints.t());
if( cPCA.eigenvectors.rows > maxComponents)
err = cvtest::norm(cv::abs(rvPrjTestPoints.rowRange(0,maxComponents)), cv::abs(rPrjTestPoints.t()), CV_RELATIVE_L2 );
else
err = cvtest::norm(cv::abs(rvPrjTestPoints), cv::abs(rPrjTestPoints.colRange(0,cPCA.eigenvectors.rows).t()), CV_RELATIVE_L2 );
if( err > diffPrjEps )
{
ts->printf( cvtest::TS::LOG, "bad accuracy of project() (CV_PCA_DATA_AS_COL); retainedVariance=0.95; err = %f\n", err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
return;
}
err = cvtest::norm(cPCA.backProject(rvPrjTestPoints), rBackPrjTestPoints.t(), CV_RELATIVE_L2 );
if( err > diffBackPrjEps )
{
ts->printf( cvtest::TS::LOG, "bad accuracy of backProject() (CV_PCA_DATA_AS_COL); retainedVariance=0.95; err = %f\n", err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
return;
}
#ifdef CHECK_C
// 4. check C PCA & ROW
_points = rPoints;
_testPoints = rTestPoints;
_avg = avg;
_eval = eval;
_evec = evec;
prjTestPoints.create(rTestPoints.rows, maxComponents, rTestPoints.type() );
backPrjTestPoints.create(rPoints.size(), rPoints.type() );
_prjTestPoints = prjTestPoints;
_backPrjTestPoints = backPrjTestPoints;
cvCalcPCA( &_points, &_avg, &_eval, &_evec, CV_PCA_DATA_AS_ROW );
cvProjectPCA( &_testPoints, &_avg, &_evec, &_prjTestPoints );
cvBackProjectPCA( &_prjTestPoints, &_avg, &_evec, &_backPrjTestPoints );
err = cvtest::norm(prjTestPoints, rPrjTestPoints, CV_RELATIVE_L2);
if( err > diffPrjEps )
{
ts->printf( cvtest::TS::LOG, "bad accuracy of cvProjectPCA() (CV_PCA_DATA_AS_ROW); err = %f\n", err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
return;
}
err = cvtest::norm(backPrjTestPoints, rBackPrjTestPoints, CV_RELATIVE_L2);
if( err > diffBackPrjEps )
{
ts->printf( cvtest::TS::LOG, "bad accuracy of cvBackProjectPCA() (CV_PCA_DATA_AS_ROW); err = %f\n", err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
return;
}
// 5. check C PCA & COL
_points = cPoints;
_testPoints = cTestPoints;
avg = avg.t(); _avg = avg;
eval = eval.t(); _eval = eval;
evec = evec.t(); _evec = evec;
prjTestPoints = prjTestPoints.t(); _prjTestPoints = prjTestPoints;
backPrjTestPoints = backPrjTestPoints.t(); _backPrjTestPoints = backPrjTestPoints;
cvCalcPCA( &_points, &_avg, &_eval, &_evec, CV_PCA_DATA_AS_COL );
cvProjectPCA( &_testPoints, &_avg, &_evec, &_prjTestPoints );
cvBackProjectPCA( &_prjTestPoints, &_avg, &_evec, &_backPrjTestPoints );
err = cvtest::norm(cv::abs(prjTestPoints), cv::abs(rPrjTestPoints.t()), CV_RELATIVE_L2 );
if( err > diffPrjEps )
{
ts->printf( cvtest::TS::LOG, "bad accuracy of cvProjectPCA() (CV_PCA_DATA_AS_COL); err = %f\n", err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
return;
}
err = cvtest::norm(backPrjTestPoints, rBackPrjTestPoints.t(), CV_RELATIVE_L2);
if( err > diffBackPrjEps )
{
ts->printf( cvtest::TS::LOG, "bad accuracy of cvBackProjectPCA() (CV_PCA_DATA_AS_COL); err = %f\n", err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
return;
}
#endif
// Test read and write
FileStorage fs( "PCA_store.yml", FileStorage::WRITE );
rPCA.write( fs );
fs.release();
PCA lPCA;
fs.open( "PCA_store.yml", FileStorage::READ );
lPCA.read( fs.root() );
err = cvtest::norm( rPCA.eigenvectors, lPCA.eigenvectors, CV_RELATIVE_L2 );
if( err > 0 )
{
ts->printf( cvtest::TS::LOG, "bad accuracy of write/load functions (YML); err = %f\n", err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
}
err = cvtest::norm( rPCA.eigenvalues, lPCA.eigenvalues, CV_RELATIVE_L2 );
if( err > 0 )
{
ts->printf( cvtest::TS::LOG, "bad accuracy of write/load functions (YML); err = %f\n", err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
}
err = cvtest::norm( rPCA.mean, lPCA.mean, CV_RELATIVE_L2 );
if( err > 0 )
{
ts->printf( cvtest::TS::LOG, "bad accuracy of write/load functions (YML); err = %f\n", err );
ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
}
}
//********************************
//* 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;
}
