00001 #include "pr2_overhead_grasping/mahalanobis_dist.h"
00002
00003 #include <omp.h>
00004 #include <stdio.h>
00005 #include <signal.h>
00006
00007
00008 using namespace std;
00009 using namespace pr2_overhead_grasping;
00010 using namespace std_msgs;
00011
00012
00013 namespace collision_detection {
00014
00015 void MahalanobisDist::loadDataset() {
00016 dataset = new vector< SensorPoint::ConstPtr >;
00017 string bag_path;
00018 XmlRpc::XmlRpcValue bag_names, bag_labels;
00019 nh_priv->getParam("bag_path", bag_path);
00020 nh_priv->getParam("bag_names", bag_names);
00021 nh_priv->getParam("bag_labels", bag_labels);
00022 map<int, int> labels_dict;
00023 for(int i=0;i<bag_names.size();i++) {
00024 string file_loc = bag_path + (string) bag_names[i];
00025 loadDataBag(file_loc, bag_labels[i]);
00026 labels_dict[bag_labels[i]] = 0;
00027 }
00028 num_classes = labels_dict.size();
00029 printf("%d\n", num_classes);
00030 }
00031
00032 void MahalanobisDist::loadDataBag(string& data_bag, int label) {
00033
00034 rosbag::Bag bag(data_bag);
00035 rosbag::View view(bag, rosbag::TopicQuery("/collision_data"));
00036 BOOST_FOREACH(rosbag::MessageInstance const m, view) {
00037 SensorPoint::Ptr sp = m.instantiate<SensorPoint>();
00038 if(sp != NULL) {
00039 sp->label = label;
00040
00041
00042 int ind = 340;
00043 for(int i =420;i<920;i++)
00044 sp->features[ind++] = (sp->features[i]);
00045 for(int i =1000;i<(int) sp->features.size();i++)
00046 sp->features[ind++] = (sp->features[i]);
00047 sp->features.resize(sp->features.size() - 160);
00048
00049 dataset->push_back(sp);
00050 }
00051 }
00052 assert(dataset->size() != 0);
00053 }
00054
00055 MahalanobisDist::~MahalanobisDist() {
00056 }
00057
00058 void MahalanobisDist::loadCovMat() {
00059 string mahal_bag_name;
00060 string bag_path;
00061 double eigen_thresh;
00062 nh_priv->getParam("bag_path", bag_path);
00063 nh_priv->getParam("mahal_bag_name", mahal_bag_name);
00064 nh_priv->getParam("eigen_thresh", eigen_thresh);
00065 string file_loc = bag_path + mahal_bag_name;
00066 rosbag::Bag bag(file_loc);
00067 rosbag::View view(bag, rosbag::TopicQuery("matrix"));
00068 BOOST_FOREACH(rosbag::MessageInstance const m, view) {
00069 CovarianceMatrix::Ptr cov_mat_msg = m.instantiate<CovarianceMatrix>();
00070 if(cov_mat_msg != NULL) {
00071 ROS_INFO("size %d, %d, %d", cov_mat_msg->size, cov_mat_msg->cov_mat.size(), cov_mat_msg->means.size());
00072 MatrixXd cov_mat(cov_mat_msg->size, cov_mat_msg->size);
00073 VectorXd means(cov_mat_msg->size);
00074 int m_ind = 0;
00075 for(uint32_t i=0;i<cov_mat_msg->size;i++) {
00076 for(uint32_t j=0;j<cov_mat_msg->size;j++)
00077 cov_mat(i,j) = cov_mat_msg->cov_mat[m_ind++];
00078 means(i) = cov_mat_msg->means[i];
00079 }
00080 ROS_INFO("size %d", cov_mat_msg->size);
00081 cov_inv.makeInv(cov_mat, means, eigen_thresh);
00082 ROS_INFO("size %d", cov_mat_msg->size);
00083 }
00084 }
00085 std_msgs::Bool loaded;
00086 loaded.data = true;
00087 loaded_pub.publish(loaded);
00088 classifier_loaded = true;
00089 ROS_INFO("[mahalanobis_dist] Classifier loaded.");
00090 }
00091
00092 void MahalanobisDist::createCovMat() {
00093 loadDataset();
00094 dataset->resize(10000);
00095 int num_feats = dataset->at(0)->features.size();
00096
00097 VectorXd means = VectorXd::Zero(num_feats);
00098 for(uint32_t i=0;i<dataset->size();i++) {
00099 for(int j=0;j<num_feats;j++) {
00100 means(j) += dataset->at(i)->features[j];
00101 }
00102 }
00103 means /= dataset->size();
00104 int j=0, l=0, i=0, data_size = dataset->size();
00105 MatrixXd var_mat = MatrixXd::Zero(num_feats, num_feats);
00106 #pragma omp parallel default(shared) private(j, l, i) num_threads(10)
00107 {
00108 for(i=0;i<data_size;i++) {
00109 ROS_INFO("sample pt %d", i);
00110 #pragma omp for schedule(dynamic, 10)
00111 for(j=0;j<num_feats;j++) {
00112 for(l=0;l<num_feats;l++) {
00113 var_mat(j,l) += (dataset->at(i)->features[j] - means(j)) * (dataset->at(i)->features[l] - means(l));
00114 }
00115 }
00116 }
00117 }
00118 var_mat /= dataset->size();
00119
00120 saveCovMat(var_mat, means);
00121 }
00122
00123 void MahalanobisDist::saveCovMat(MatrixXd& var_mat, VectorXd& means) {
00124 CovarianceMatrix cov_mat_msg;
00125 int num_feats = var_mat.cols();
00126 cov_mat_msg.size = num_feats;
00127 for(int j=0;j<num_feats;j++) {
00128 for(int l=0;l<num_feats;l++)
00129 cov_mat_msg.cov_mat.push_back(var_mat(j,l));
00130 cov_mat_msg.means.push_back(means(j));
00131 }
00132
00133 string mahal_bag_name;
00134 string bag_path;
00135 nh_priv->getParam("bag_path", bag_path);
00136 nh_priv->getParam("mahal_bag_name", mahal_bag_name);
00137 string file_loc = bag_path + mahal_bag_name;
00138 rosbag::Bag bag;
00139 string forest_bag_name;
00140 int bagmode = rosbag::bagmode::Write;
00141 bag.open(file_loc, bagmode);
00142 ROS_INFO("Writing covariance matrix to bag");
00143 bag.write("matrix", ros::Time::now(), cov_mat_msg);
00144 bag.close();
00145 ROS_INFO("Finished writing");
00146 }
00147
00148 void MahalanobisDist::classifyCallback(const boost::shared_ptr<SensorPoint>& inst) {
00149 if(!classifier_loaded) {
00150 ROS_INFO("[mahalanobis_dist] Classifcation requested but classifier not loaded.");
00151 return;
00152 }
00153 VectorXd pt(inst->features.size() - 160);
00154
00155 int ind = 0;
00156 for(int i =0;i<340;i++)
00157 pt(ind++) = inst->features[i];
00158 for(int i =420;i<920;i++)
00159 pt(ind++) = inst->features[i];
00160 for(int i =1000;i<(int) inst->features.size();i++)
00161 pt(ind++) = inst->features[i];
00162
00163 Float32 dist;
00164 dist.data = (float) cov_inv.dist(pt);
00165 results_pub.publish(dist);
00166 }
00167
00168 double MahalanobisDist::mahalanobisDist(MatrixXd& cov_inv, VectorXd& means, VectorXd& pt) {
00169 return (pt - means).dot(cov_inv * (pt - means));
00170 }
00171
00172 void MahalanobisDist::makeInv(MatrixXd& A, MatrixXd& A_inv, double min_eig_val) {
00173 SelfAdjointEigenSolver<MatrixXd> solver(A);
00174 VectorXd eigenvals = solver.eigenvalues();
00175 ROS_INFO("MINIMUM EIGEN %f\n", eigenvals.minCoeff());
00176 MatrixXd eigenvectors = solver.eigenvectors();
00177 double thresh = eigenvals.maxCoeff() * min_eig_val;
00178 for(uint32_t i=0;i<eigenvals.size();i++) {
00179 if(eigenvals(i) < thresh)
00180 eigenvals(i) = 0;
00181 else
00182 eigenvals(i) = 1.0 / eigenvals(i);
00183 }
00184 A_inv = eigenvectors * eigenvals.asDiagonal() * eigenvectors.transpose();
00185
00186 SelfAdjointEigenSolver<MatrixXd> solver2(A_inv);
00187 ROS_INFO("MINIMUM EIGEN2 %f\n", solver2.eigenvalues().minCoeff());
00188 }
00189
00190 void MahalanobisDist::doMahalanobis() {
00191 loadDataset();
00192 vector<vector< SensorPoint::ConstPtr > > datasets(1000);
00193 for(uint32_t i=0;i<dataset->size();i++)
00194 datasets[dataset->at(i)->label].push_back(dataset->at(i));
00195 for(uint32_t i=0;i<datasets.size();i++)
00196 if(datasets[i].size() == 0) {
00197 datasets.resize(i);
00198 num_classes = i;
00199 break;
00200 }
00201 datasets[0].resize(10000);
00202 datasets[1].resize(10000);
00203 int num_feats = datasets[0][0]->features.size();
00204
00205 vector<DistFinder> cov_inv(num_classes);
00206
00207 vector<VectorXd> means(num_classes);
00208 for(int k=0;k<num_classes;k++) {
00209 means[k] = VectorXd::Zero(num_feats);
00210 for(uint32_t i=0;i<datasets[k].size();i++) {
00211 for(int j=0;j<num_feats;j++) {
00212 means[k](j) += datasets[k][i]->features[j];
00213 }
00214 }
00215 means[k] /= datasets[k].size();
00216 }
00217 int k=0, j=0, l=0, i=0, data_size = datasets[k].size();
00218
00219 for(k=0;k<1;k++) {
00220
00221 MatrixXd var_mat = MatrixXd::Zero(num_feats, num_feats);
00222 #pragma omp parallel default(shared) private(j, l, i) num_threads(10)
00223 {
00224 for(i=0;i<data_size;i++) {
00225 ROS_INFO("sample pt %d", i);
00226 #pragma omp for schedule(dynamic, 10)
00227 for(j=0;j<num_feats;j++) {
00228 for(l=0;l<num_feats;l++) {
00229
00230 var_mat(j,l) += (datasets[k][i]->features[j] - means[k](j)) * (datasets[k][i]->features[l] - means[k](l));
00231 }
00232 }
00233 }
00234 }
00235 var_mat /= datasets[k].size();
00236
00237
00238
00239
00240
00241
00242 cov_inv[k].makeInv(var_mat, means[k], 0.000001);
00243 }
00244
00245 for(int k=0;k<1;k++) {
00246 for(int l=0;l<num_classes;l++) {
00247 ArrayXd dists(datasets[l].size());
00248 int ind = 0, data_size = datasets[l].size();
00249 vector<VectorXd> feat_vecs(data_size);
00250 for(ind=0;ind<data_size;ind++) {
00251 feat_vecs[ind] = VectorXd::Zero(num_feats);
00252 for(j=0;j<num_feats;j++)
00253 feat_vecs[ind](j) = datasets[l][ind]->features[j];
00254 }
00255 #pragma omp parallel default(shared) private(ind) num_threads(10)
00256 {
00257 #pragma omp for schedule(dynamic, 10)
00258 for(ind=0;ind<data_size;ind++) {
00259
00260 dists[ind] = cov_inv[k].dist(feat_vecs[ind]);
00261 }
00262 }
00263
00264 int nans = 0, negs = 0;
00265 for(int i=0;i<data_size;i++) {
00266 if(dists[i] < 0)
00267 negs++;
00268 if(dists[i] != dists[i]) {
00269 dists[i] = 0;
00270 nans++;
00271 }
00272 }
00273
00274 double mean_dist = dists.sum() / (datasets[l].size() - nans);
00275 VectorXd diff = dists - mean_dist;
00276 double var_dist = diff.dot(diff) / (datasets[l].size() - nans);
00277 double std_dist = sqrt(var_dist);
00278 double min_dist = dists.minCoeff();
00279 double max_dist = dists.maxCoeff();
00280 printf("cov %d, data %d, mean_dist %e, std_dist %e, min %e, max %e, nans %d, negs %d\n", k, l, mean_dist, std_dist, min_dist, max_dist, nans, negs);
00281 }
00282 printf("cov %d, num_samps %d, rank 0\n", k, (int) datasets[k].size());
00283 }
00284 printf("num_classes %d, num_feats %d\n", num_classes, num_feats);
00285 }
00286
00287 void MahalanobisDist::summarizeData() {
00288 loadDataset();
00289 int num_feats = dataset->at(0)->features.size();
00290
00291 MatrixXd means = MatrixXd::Zero(num_classes, num_feats);
00292 MatrixXd vars = MatrixXd::Zero(num_classes, num_feats);
00293 MatrixXd mins = MatrixXd::Zero(num_classes, num_feats);
00294 MatrixXd maxs = MatrixXd::Zero(num_classes, num_feats);
00295 VectorXi nums = VectorXi::Zero(num_classes);
00296 for(int l=0;l<num_classes;l++)
00297 for(uint32_t i=0;i<dataset->size();i++)
00298 nums(dataset->at(i)->label)++;
00299
00300 for(int l=0;l<num_classes;l++) {
00301 MatrixXd data = MatrixXd::Zero(nums(l), num_feats);
00302 int ind = 0;
00303 for(uint32_t i=0;i<dataset->size();i++) {
00304 if(dataset->at(i)->label != l)
00305 continue;
00306 for(int j=0;j<num_feats;j++)
00307 data(ind,j) = abs(dataset->at(i)->features[j]);
00308 ind++;
00309 }
00310 nums(l) = ind;
00311 for(int j=0;j<num_feats;j++) {
00312 double mean = data.col(j).sum() / ind;
00313 VectorXd err = data.col(j).array() - mean;
00314 double var = err.dot(err) / ind;
00315 double min = data.col(j).minCoeff();
00316 double max = data.col(j).maxCoeff();
00317
00318 means(l,j) = mean; vars(l, j) = var; mins(l, j) = min; maxs(l, j) = max;
00319 }
00320 }
00321
00322 for(int j=0;j<num_feats;j++) {
00323 printf("\n%d\n", j);
00324 for(int l=0;l<num_classes;l++)
00325 printf("%+1.3e %+1.3e %+1.3e %+1.3e\n", means(l,j), vars(l,j), mins(l,j), maxs(l,j));
00326 }
00327 cout << nums;
00328 }
00329
00330 void MahalanobisDist::onInit() {
00331 nh = new ros::NodeHandle;
00332 nh_priv = new ros::NodeHandle("~");
00333
00334 std::string results_topic, classify_topic, data_bag, forest_bag, loaded_topic;
00335 bool training_mode, is_validation, is_data_summary;
00336
00337 nh_priv->getParam("is_validation", is_validation);
00338 if(is_validation) {
00339 doMahalanobis();
00340 return;
00341 }
00342
00343 nh_priv->getParam("training_mode", training_mode);
00344 if(training_mode) {
00345 createCovMat();
00346 return;
00347 }
00348
00349 nh_priv->getParam("is_data_summary", is_data_summary);
00350 if(is_data_summary) {
00351 summarizeData();
00352 return;
00353 }
00354
00355 nh_priv->getParam("classify_topic", classify_topic);
00356 nh_priv->getParam("results_topic", results_topic);
00357 nh_priv->getParam("loaded_topic", loaded_topic);
00358 nh_priv->getParam("classifier_id", classifier_id);
00359 nh_priv->getParam("classifier_name", classifier_name);
00360
00361 classify_sub = nh->subscribe(classify_topic.c_str(), 2,
00362 &MahalanobisDist::classifyCallback, this);
00363 ROS_INFO("[mahalanobis_dist] Subscribed to %s", classify_topic.c_str());
00364 results_pub = nh->advertise<Float32>(results_topic, 1);
00365 ROS_INFO("[mahalanobis_dist] Publishing on %s", results_topic.c_str());
00366 loaded_pub = nh->advertise<Bool>(loaded_topic, 1);
00367 ROS_INFO("[mahalanobis_dist] Publishing on %s", loaded_topic.c_str());
00368
00369 classifier_loaded = false;
00370 loadCovMat();
00371 }
00372
00373 }
00374
00375 using namespace collision_detection;
00376
00377 void INTHandler(int sig);
00378
00379 void INTHandler(int sig) {
00380 char c;
00381 signal(sig, SIG_IGN);
00382 printf("Want to exit?\n");
00383 c = getchar();
00384 if(c == 'y' || c == 'Y')
00385 exit(0);
00386 else
00387 signal(SIGINT, INTHandler);
00388 }
00389
00390 int main(int argc, char** argv) {
00391 ros::init(argc, argv, "mahalanobis_dist", ros::init_options::AnonymousName);
00392
00393
00394 MahalanobisDist md;
00395 md.onInit();
00396 ros::spin();
00397 printf("Exiting\n");
00398 return 0;
00399 }