00001 /* 00002 * graph_learner.cpp 00003 * 00004 * Created on: Jul 25, 2010 00005 * Author: sturm 00006 */ 00007 00008 #include "graph_learner.h" 00009 #include "hogman_wrapper.h" 00010 00011 PoseMap GraphLearner::getPrediction(double timestamp, KinematicGraphType &tree, 00012 PoseMap &observedMarkers,PoseMap &predictedEmpty) { 00013 return hogman_solver(tree, poseIndex, observedMarkers, sigma_position, 00014 sigma_orientation, timestamp); 00015 } 00016 00017 void GraphLearner::sendTreeTransforms(KinematicGraphType graph) { 00018 if (graph.size() == 0) 00019 return; 00020 00021 double latestTimestamp = stampedMarker.rbegin()->first; 00022 00023 PoseMap observation = getObservation(latestTimestamp); 00024 PoseMap prediction = getPrediction(intersectionOfStamps().back(), graph, 00025 observation); 00026 00027 // send transform for root node 00028 static tf::TransformBroadcaster br; 00029 const PoseStamped &latestPose = 00030 stampedMarker[latestTimestamp].begin()->second->pose; 00031 for (auto it = prediction.begin(); it != prediction.end(); it++) { 00032 br.sendTransform(tf::StampedTransform(poseToTransform(it->second), 00033 latestPose.header.stamp, "/camera", boost::str(boost::format( 00034 "/pred/%1%") % it->first))); 00035 } 00036 } 00037 00038 //void GraphLearner::selectSceneModel() { 00039 // std::vector<double> stamps = intersectionOfStamps(); 00040 // 00041 // if (stamps.size() == 0) { 00042 // cout << "no intersecting timestamps.." << endl; 00043 // return; 00044 // } 00045 // KinematicGraphType bestGraph; 00046 // map<double, pair<string, KinematicGraphType> > graphMap; 00047 // double bestBIC = DBL_MAX; 00048 // int bestDOF = -1; 00049 // double best_avg_pos_err = -1; 00050 // double best_avg_orient_err = -1; 00051 // double best_loglh = DBL_MAX; 00052 // 00053 // map<int, int> marker_to_vertex; 00054 // map<int, int> vertex_to_marker; 00055 // int n = 0; 00056 // for (map<int, double>::iterator it = latestTimestamp.begin(); it 00057 // != latestTimestamp.end(); it++) { 00058 // marker_to_vertex[it->first] = n; 00059 // vertex_to_marker[n] = it->first; 00060 // n++; 00061 // } 00062 // 00063 // int vertices = models.size(); 00064 // int edges = vertices * vertices; 00065 // 00066 // for (long graph = 1; graph < (1 << (edges - 1)); graph++) { 00067 // // iterate over all possible graphs 00068 // std::vector<bool> connected(vertex_to_marker.size(), false); 00069 // connected[0] = true; 00070 // KinematicGraphType tree; 00071 // for (int e = 0; e < edges; e++) { 00072 // if ((graph & (1 << e)) > 0) { 00073 // // edge is present in this particular graph 00074 // int e_from = vertex_to_marker[e / vertices]; 00075 // int e_to = vertex_to_marker[e % vertices]; 00076 // if (e_from >= e_to) { 00077 // tree.clear(); 00078 // break; 00079 // } 00080 // connected[e % vertices] = connected[e / vertices]; 00081 // if (!models[e_from][e_to]) { 00082 // tree.clear(); 00083 // break; 00084 // } 00085 // if (!models[e_to][e_from]) { 00086 // tree.clear(); 00087 // break; 00088 // } 00089 // tree.push_back(pair<pair<int, int> , GenericModelPtr> (pair< 00090 // int, int> (e_from, e_to), models[e_from][e_to])); 00091 // } 00092 // } 00093 // if ((int) tree.size() < vertices - 1) 00094 // continue; 00095 // if (restrict_graphs) { 00096 // if ((restricted_graphs.find(" " + getTreeName(tree, false, false) 00097 // + " ") == restricted_graphs.npos) 00098 // && (restricted_graphs.find(" " + getTreeName(tree, true, 00099 // false) + " ") == restricted_graphs.npos) 00100 // && (restricted_graphs.find(" " + getTreeName(tree, true, 00101 // true) + " ") == restricted_graphs.npos)) 00102 // continue; 00103 // } 00104 // 00105 // bool allConnected = true; 00106 // for (size_t i = 0; i < connected.size(); i++) { 00107 // if (!connected[i]) 00108 // allConnected = false; 00109 // } 00110 // if (allConnected) { 00111 // // generate tree with all possible link models 00112 // // find the number of possible combinations 00113 // KinematicGraphType basetree = tree; 00114 // tree.clear(); 00115 // int combination_total = 1; 00116 // for (KinematicGraphType::iterator it = basetree.begin(); it 00117 // != basetree.end(); it++) { 00118 // combination_total 00119 // *= models_all[it->first.first][it->first.second].size(); 00120 // } 00121 // if (search_all_models) 00122 // cout << "need to evaluate n=" << combination_total 00123 // << " different versions of tree " << getTreeName( 00124 // basetree, true, true) << endl; 00125 // for (int combination = 0; combination < combination_total; combination++) { 00126 // if (search_all_models) { 00127 // tree.clear(); 00128 // int c_tmp = combination; 00129 // for (KinematicGraphType::iterator it = basetree.begin(); it 00130 // != basetree.end(); it++) { 00131 // int 00132 // idx = 00133 // c_tmp 00134 // % models_all[it->first.first][it->first.second].size(); 00135 // tree.push_back( 00136 // pair<pair<int, int> , GenericModelPtr> ( 00137 // pair<int, int> (it->first.first, 00138 // it->first.second), 00139 // models_all[it->first.first][it->first.second][idx])); 00140 // c_tmp 00141 // /= models_all[it->first.first][it->first.second].size(); 00142 // } 00143 // } else { 00144 // tree = basetree; 00145 // } 00146 // 00147 // int DOFs = getDOFs(tree);// compute sum 00148 // for (int reducedDOFs = (reduce_dofs ? 1 : DOFs); reducedDOFs 00149 // <= DOFs; reducedDOFs++) { 00150 // KinematicGraphType copyOfTree = copyTree(tree); 00151 // if (reducedDOFs != DOFs) { 00152 // // cout <<"reducing dofs"<<endl; 00153 // projectConfigurationSpace(copyOfTree, reducedDOFs, 00154 // stamps); 00155 // } 00156 // 00157 // string gname_short = boost::str(boost::format("%1%-DOF ") 00158 // % reducedDOFs) + getTreeName(copyOfTree, false, 00159 // false); 00160 // string gname_models = boost::str(boost::format("%1%-DOF ") 00161 // % reducedDOFs) + getTreeName(copyOfTree, true, 00162 // false); 00163 // string gname_models_samples = boost::str(boost::format( 00164 // "%1%-DOF ") % reducedDOFs) + getTreeName( 00165 // copyOfTree, true, false); 00166 // double avg_pos_err, avg_orient_err, loglh; 00167 // double BIC = evalTree(copyOfTree, gname_models_samples, 00168 // stamps, avg_pos_err, avg_orient_err, loglh); 00169 // if (reducedDOFs < DOFs) { 00170 // BIC -= (((DOFs - reducedDOFs) * stamps.size() + (DOFs 00171 // * reducedDOFs)) * log(stamps.size())); 00172 // } 00173 // evalGraphBIC[gname_models][stamps.size()] = BIC; 00174 // graphMap[BIC] = pair<string, KinematicGraphType> ( 00175 // gname_models, copyOfTree); 00176 // if (BIC < bestBIC) { 00177 // bestGraph = tree; 00178 // bestBIC = BIC; 00179 // bestDOF = reducedDOFs; 00180 // best_avg_pos_err = avg_pos_err; 00181 // best_avg_orient_err = avg_orient_err; 00182 // best_loglh = loglh; 00183 // } 00184 // // cout <<"BIC="<<BIC <<": "<<boost::str(boost::format( "%1%-DOF ") % reducedDOFs ) + graphname.str() <<endl; 00185 // 00186 // // should we finish? 00187 // static ros::NodeHandle n; 00188 // if (!n.ok()) 00189 // return; 00190 // } 00191 // if (!search_all_models) 00192 // break; 00193 // } 00194 // } 00195 // } 00196 // saveGraphEval(); 00197 // sendModels(bestGraph); 00198 // sendStructureVisualization(bestGraph, bestBIC, bestDOF, best_avg_pos_err, 00199 // best_avg_orient_err, best_loglh); 00200 // cout << "Evaluation @ t=" << stamps.size() << endl; 00201 // for (map<double, pair<string, KinematicGraphType> >::iterator it = 00202 // graphMap.begin(); it != graphMap.end(); it++) { 00203 // cout << it->first << " " << it->second.first << endl; 00204 // } 00205 // cout << "---" << endl; 00206 //} 00207 00208 int main(int argc, char** argv) { 00209 ros::init(argc, argv, "graph_learner"); 00210 00211 GraphLearner learner; 00212 00213 ros::NodeHandle nh; 00214 ros::Publisher lifebeat_pub = 00215 nh.advertise<std_msgs::Empty> ("/lifebeat", 1); 00216 while (nh.ok()) { 00217 cout << "lifebeat.." << learner.intersectionOfStamps().size() << endl; 00218 ros::Rate loop_rate(3); 00219 loop_rate.sleep(); 00220 ros::spinOnce(); 00221 00222 lifebeat_pub.publish(std_msgs::Empty()); 00223 } 00224 }
