double ClusteringCoefficient::avgLocal(Graph& G) { WARN("DEPRECATED: use centrality.LocalClusteringCoefficient and take average"); LocalClusteringCoefficient lcc(G); lcc.run(); auto coefficients = lcc.scores(); // $c(u) := \frac{2 \cdot |E(N(u))| }{\deg(u) \cdot ( \deg(u) - 1)}$ double sum = 0.0; count size = 0; G.forNodes([&](node u) { if (G.degree(u) >= 2) { sum += coefficients[u]; size++; } }); return sum / (double) size; }
GraphAndValues Masseuse::LoadPoseGraphAndLCC( const string& pose_graph_file, bool read_lcc) { FILE *fp = (FILE *)fopen(pose_graph_file.c_str(), "rb"); if (fp == NULL) { fprintf(stderr, "Could not open file %s\n", pose_graph_file.c_str()); } if (fread(&origin, sizeof(Eigen::Vector6d), 1, fp) != 1) { throw invalid_argument("LoadPoseGraphAndLCC: Cannot load the origin!"); } // std::cerr << "read origin: " << origin.transpose() << std::endl; unsigned numRelPoses = 0; unsigned numLCC = 0; if (fread(&numRelPoses, sizeof(unsigned), 1, fp) != 1) { printf("error! Cannot load num of relative poses.\n"); throw invalid_argument("LoadPoseGraphAndLCC: error loading file"); } if(read_lcc){ if (fread(&numLCC, sizeof(unsigned), 1, fp) != 1) { printf("error! Cannot load num of loop closure constraints.\n"); throw invalid_argument("LoadPoseGraphAndLCC: error loading file"); } } std::cerr << "Will load " << numRelPoses << " rel poses, " << numLCC << " loop closure constranits." << std::endl; relative_poses.clear(); // save all rel poses for (unsigned i = 0; i != numRelPoses; i++) { RelPose rPos; if (fread(&rPos.ref_id, sizeof(unsigned), 1, fp) != 1) { throw invalid_argument("LoadPoseGraphAndLCC: error loading file"); } if (fread(&rPos.live_id, sizeof(unsigned), 1, fp) != 1) { throw invalid_argument("LoadPoseGraphAndLCC: error loading file"); } if (fread(&rPos.rel_pose, sizeof(Eigen::Vector6d), 1, fp) != 1) { throw invalid_argument("LoadPoseGraphAndLCC: error loading file"); } if (fread(&rPos.cov, sizeof(Eigen::Matrix6d), 1, fp) != 1) { throw invalid_argument("LoadPoseGraphAndLCC: error loading file"); } relative_poses.push_back(rPos); } if(read_lcc){ loop_closure_constraints.clear(); // save all lcc here for (unsigned i = 0; i != numLCC; i++) { RelPose rPos; if (fread(&rPos.ref_id, sizeof(unsigned), 1, fp) != 1) { throw invalid_argument("LoadPoseGraphAndLCC: error loading file"); } if (fread(&rPos.live_id, sizeof(unsigned), 1, fp) != 1) { throw invalid_argument("LoadPoseGraphAndLCC: error loading file"); } if (fread(&rPos.rel_pose, sizeof(Eigen::Vector6d), 1, fp) != 1) { throw invalid_argument("LoadPoseGraphAndLCC: error loading file"); } if (fread(&rPos.cov, sizeof(Eigen::Matrix6d), 1, fp) != 1) { throw invalid_argument("LoadPoseGraphAndLCC: error loading file"); } loop_closure_constraints.push_back(rPos); } } fclose(fp); std::cerr << "Finished loading Pose Graph from " << pose_graph_file << std::endl; std::shared_ptr<Values> initial(new Values); std::shared_ptr<Graph> graph(new Graph); Pose3 prev_pose; unsigned numICPfailed = 0; for (size_t ii = 0; ii < relative_poses.size(); ++ii){ RelPose curr_pose = relative_poses[ii]; if(ii == 0){ // first relative pose, initialize graph at origin unsigned id = curr_pose.ref_id; Rot3 R(origin[3], origin[4], origin[5]); Point3 t = origin.head<3>(); Pose3 orig(R, t); (*initial)[id] = orig; prev_pose = orig; // std::cerr << "inserting origin: Rot: " << orig.rotationMatrix().eulerAngles // (0,1,2).transpose() << " Trans: " << orig.translation().transpose() << // " at index: " << id << // std::endl; } // Build the next vertex using the relative contstraint Rot3 R(curr_pose.rel_pose[3], curr_pose.rel_pose[4], curr_pose.rel_pose[5]); Point3 t = curr_pose.rel_pose.head<3>(); Pose3 rel(R, t); Pose3 new_pose = prev_pose*rel; (*initial)[curr_pose.live_id] = new_pose; // std::cerr << "inserting pose: Rot: " << new_pose.rotationMatrix().eulerAngles // (0,1,2).transpose() << " Trans: " << new_pose.translation().transpose() << // " at index: " << curr_pose.live_id << // std::endl; prev_pose = new_pose; // Also insert a factor for the binary pose constraint unsigned id1 = curr_pose.ref_id; unsigned id2 = curr_pose.live_id; Matrix m = curr_pose.cov; if(m.sum() == 0 || m.determinant() <= 0 || std::isnan(m.determinant()) /*|| m.determinant() > options.cov_det_thresh*/){ // std::cerr << "ICP failed for rel pose between " << id1 << " and " << id2 << // "Setting fixed covaraince..." << // std::endl; Eigen::Vector6d cov_vec; // TODO: Improve handling of cases where the frame-to-frame ICP failed cov_vec << 7e-8, 7e-8, 7e-8, 8e-11, 8e-11, 8e-11; m = cov_vec.asDiagonal(); numICPfailed++; } // std::cerr << "Adding binary constraint between id: " << id1 << " and " << // id2 << std::endl << "with cov: \n" << m << std::endl; // Create a new factor between poses if(options.use_identity_covariance){ m.setIdentity(); } m = m * options.rel_covariance_mult; Factor factor(id1, id2, rel, m); graph->push_back(factor); } std::cerr << std::setprecision(3) << std::fixed <<"ICP failed " << numICPfailed << " times " << "out of " << relative_poses.size() << " ( " << (double)numICPfailed/(double)relative_poses.size() * 100 << "% )" << std::endl; if( read_lcc ){ std::cerr << "Reading LLC." << std::endl; int discarded_lcc = 0; for(size_t ii = 0; ii < loop_closure_constraints.size(); ++ii){ RelPose curr_lcc = loop_closure_constraints[ii]; unsigned id1 = curr_lcc.ref_id; unsigned id2 = curr_lcc.live_id; Rot3 R(curr_lcc.rel_pose[3], curr_lcc.rel_pose[4], curr_lcc.rel_pose[5]); Point3 t = curr_lcc.rel_pose.head<3>(); Pose3 lcc(R, t); Matrix m = curr_lcc.cov; if(m.sum() == 0 || m.determinant() > options.cov_det_thresh || m.determinant() <= 0 || std::isnan(m.determinant())) { // ICP failed or something went wrong for this loop closure, ignoring // The determinant of the cov. matrix may also be larger than the // specified threshold. discarded_lcc++; continue; } // check if the lcc is between far away poses. If so, downweight it's // covariance. // const Pose3* lcc_0 = dynamic_cast<const Pose3*>(&initial->at(id1)); // const Pose3* lcc_1 = dynamic_cast<const Pose3*>(&initial->at(id2)); // Pose3 lcc_diff = lcc_0->compose(lcc).inverse().compose(*lcc_1); // std::cerr << "distance between poses for lcc: " << ii << // " between poses " << id1 << " and " << id2 << ": " // << lcc_diff.translation().norm() << std::endl; // Pose3 diff = (*lcc_0).inverse().compose(*lcc_1); // std::cerr << "distance between poses for lcc: " << ii << // " between poses " << id1 << " and " << id2 << ": " // << lcc.translation().norm() << std::endl; // Create a new factor between poses if(options.use_identity_covariance){ m.setIdentity(); } Factor lcc_factor(id1, id2, lcc, m); lcc_factor.isLCC = true; graph->push_back(lcc_factor); curr_lcc.ext_id = graph->size()-1; // std::cerr << std::scientific << // "Adding lcc between id: " << id1 << " and " << // id2 << std::endl << "with cov: \n" << lcc_factor.cov << std::endl; //m_addedLCC[keyFromId(id2)] = curr_lcc; /* // If we already have a constraint between poses i and j, only use the one // with the highest degree of certainty (lowest cov. determinant) if(m_addedLCC.count(keyFromId(id2)) == 0){ // LCC has not been added yet, just add graph->push_back(factor); curr_lcc.m_nExtId = graph->size()-1; m_addedLCC[keyFromId(id2)] = curr_lcc; }else{ // Check if the covariance of the new lcc for poses i and j // is smaller than what we are currently using EstPose old_lcc = m_addedLCC[keyFromId(id2)]; if(old_lcc.m_Cov.determinant() > curr_lcc.m_Cov.determinant()){ // new det is smaller, replace curr_lcc.m_nExtId = old_lcc.m_nExtId; graph->replace(old_lcc.m_nExtId, factor); m_addedLCC[keyFromId(id2)] = curr_lcc; // std::cerr << "determinant for new lcc between " << id1 << " and " << // id2 << " is: " << curr_lcc.m_Cov.determinant() << " < " // << old_lcc.m_Cov.determinant() << std::endl; }else{ // Determinant for new constraint is larger, skip it continue; } } */ } // //ZZZZZZZZZZZZZ Temp, add wrong LCC to test switchable constraints // unsigned id1 = 480; // unsigned id2 = 870; // Pose3& T2 = initial->at(id2); // Pose3& T1 = initial->at(id1); // Pose3 T12 = T1.inverse() * T2; // // Add random values to the translation // T12.translation()[0] += 5; // T12.translation()[1] -= 2; // T12.translation()[3] += 7; // // Rotate the pose by an arbitrary amount // Sophus::SO3d rot(0.5, 0.7, 0.1); // T12.rotationMatrix() *= rot.matrix(); // Factor wrong_lcc(id1, id2, T12, Eigen::Matrix6d::Identity()); // wrong_lcc.isLCC = true; // // now add the wrong LCC to the graph // graph->push_back(wrong_lcc); std::cerr << std::setprecision(3) << std::fixed <<"Did not use " << discarded_lcc << " LCC " << "out of " << numLCC << " ( " << (double)discarded_lcc/(double)numLCC * 100 << "% )" << std::endl; } return make_pair(graph, initial); }