38 #include "opencv/cxcore.h" 39 #include "opencv/cv.h" 40 #include "opencv/ml.h" 42 #include "people_msgs/PositionMeasurement.h" 43 #include "sensor_msgs/LaserScan.h" 78 printf(
"Loading positive training data from file: %s\n", file);
81 printf(
"Loading negative training data from file: %s\n", file);
84 printf(
"Loading test data from file: %s\n", file);
90 ros::record::Player p;
120 vector< vector<float> >* data = (vector< vector<float> >*)(n);
122 if (mask_count_++ < 20)
132 for (list<SampleSet*>::iterator i = processor.
getClusters().begin();
141 int sample_size = pos_data_.size() + neg_data_.size();
142 feat_count_ = pos_data_[0].size();
144 CvMat* cv_data = cvCreateMat(sample_size, feat_count_, CV_32FC1);
145 CvMat* cv_resp = cvCreateMat(sample_size, 1, CV_32S);
149 for (vector< vector<float> >::iterator i = pos_data_.begin();
150 i != pos_data_.end();
153 float* data_row = (
float*)(cv_data->data.ptr + cv_data->step * j);
154 for (
int k = 0; k < feat_count_; k++)
155 data_row[k] = (*i)[k];
157 cv_resp->data.i[j] = 1;
162 for (vector< vector<float> >::iterator i = neg_data_.begin();
163 i != neg_data_.end();
166 float* data_row = (
float*)(cv_data->data.ptr + cv_data->step * j);
167 for (
int k = 0; k < feat_count_; k++)
168 data_row[k] = (*i)[k];
170 cv_resp->data.i[j] = -1;
174 CvMat* var_type = cvCreateMat(1, feat_count_ + 1, CV_8U);
175 cvSet(var_type, cvScalarAll(CV_VAR_ORDERED));
176 cvSetReal1D(var_type, feat_count_, CV_VAR_CATEGORICAL);
178 float priors[] = {1.0, 1.0};
180 CvRTParams fparam(8, 20, 0,
false, 10, priors,
false, 5, 50, 0.001
f, CV_TERMCRIT_ITER);
181 fparam.term_crit = cvTermCriteria(CV_TERMCRIT_ITER, 100, 0.1);
183 forest.train(cv_data, CV_ROW_SAMPLE, cv_resp, 0, 0, var_type, 0,
187 cvReleaseMat(&cv_data);
188 cvReleaseMat(&cv_resp);
189 cvReleaseMat(&var_type);
194 CvMat* tmp_mat = cvCreateMat(1, feat_count_, CV_32FC1);
198 for (vector< vector<float> >::iterator i = pos_data_.begin();
199 i != pos_data_.end();
202 for (
int k = 0; k < feat_count_; k++)
203 tmp_mat->data.fl[k] = (
float)((*i)[k]);
204 if (forest.predict(tmp_mat) > 0)
211 for (vector< vector<float> >::iterator i = neg_data_.begin();
212 i != neg_data_.end();
215 for (
int k = 0; k < feat_count_; k++)
216 tmp_mat->data.fl[k] = (
float)((*i)[k]);
217 if (forest.predict(tmp_mat) < 0)
224 for (vector< vector<float> >::iterator i = test_data_.begin();
225 i != test_data_.end();
228 for (
int k = 0; k < feat_count_; k++)
229 tmp_mat->data.fl[k] = (
float)((*i)[k]);
230 if (forest.predict(tmp_mat) > 0)
235 printf(
" Pos train set: %d/%d %g\n", pos_right, pos_total, (
float)(pos_right) / pos_total);
236 printf(
" Neg train set: %d/%d %g\n", neg_right, neg_total, (
float)(neg_right) / neg_total);
237 printf(
" Test set: %d/%d %g\n", test_right, test_total, (
float)(test_right) / test_total);
239 cvReleaseMat(&tmp_mat);
249 int main(
int argc,
char **argv)
258 printf(
"Loading data...\n");
259 for (
int i = 1; i < argc; i++)
261 if (!strcmp(argv[i],
"--train"))
263 else if (!strcmp(argv[i],
"--neg"))
265 else if (!strcmp(argv[i],
"--test"))
267 else if (!strcmp(argv[i],
"--save"))
270 strncpy(save_file, argv[i], 100);
277 printf(
"Training classifier...\n");
280 printf(
"Evlauating classifier...\n");
283 if (strlen(save_file) > 0)
285 printf(
"Saving classifier as: %s\n", save_file);
void loadCb(string name, sensor_msgs::LaserScan *scan, ros::Time t, ros::Time t_no_use, void *n)
vector< vector< float > > pos_data_
void splitConnected(float thresh)
std::vector< float > calcLegFeatures(laser_processor::SampleSet *cluster, const sensor_msgs::LaserScan &scan)
A namespace containing the laser processor helper classes.
std::list< SampleSet * > & getClusters()
int main(int argc, char **argv)
void loadData(LoadType load, char *file)
void removeLessThan(uint32_t num)
void addScan(sensor_msgs::LaserScan &scan)
A mask for filtering out Samples based on range.
vector< vector< float > > test_data_
vector< vector< float > > neg_data_