Add files via upload

Author: gitouni

src/examples/ba_calib.cpp

@@ -11,11 +11,11 @@
 #include <map>
 
 int main(int argc, char** argv){
-    if(argc < 4){
-        std::cerr << "Got " << argc - 1 << " Parameters, but Expected parameters: yaml_file orb_setting_file orb_vocabulary_file" << std::endl;
+    if(argc < 2){
+        std::cerr << "Got " << argc - 1 << " Parameters, but Expected parameters: yaml_file" << std::endl;
         exit(0);
     }
-    std::string config_file(argv[1]), ORBSetFilename(argv[2]), VocFilename(argv[3]);
+    std::string config_file(argv[1]);
     YAML::Node config = YAML::LoadFile(config_file);
     std::string base_dir = config["BaseDir"].as<std::string>();
     checkpath(base_dir);
@@ -26,6 +26,12 @@ int main(int argc, char** argv){
     std::string KeyFrameDir = base_dir + config["KeyFrameDir"].as<std::string>();
     std::string KyeFrameIdFile = base_dir + config["VOIdFile"].as<std::string>();
     std::string MapFileNmae = base_dir + config["MapFile"].as<std::string>();
+    std::string initSim3File = base_dir + config["init_sim3"].as<std::string>();
+    std::string VocFilename = config["ORBVocabulary"].as<std::string>();
+    std::string ORBSetFilename = config["ORBConfig"].as<std::string>();
+    assert(file_exist(TwcFilename));
+    assert(file_exist(TwlFilename));
+    assert(file_exist(initSim3File));
     std::map<std::string, int> method_options = config["method_options"].as<std::map<std::string, int> >();
     std::string method = config["method"].as<std::string>();
     assert(method_options.count(method)>0);
@@ -45,32 +51,21 @@ int main(int argc, char** argv){
     vmTwl.reserve(vTwl.size()-1);
     pose2Motion(vTwc, vmTwc);
     pose2Motion(vTwl, vmTwl);
-    Eigen::Matrix3d RCL;
-    Eigen::Vector3d tCL;
+    Eigen::Matrix4d rigid;
     double s;
+    std::tie(rigid, s) = readSim3(initSim3File);
 
-    
-
     ORB_SLAM2::System VSLAM(VocFilename, ORBSetFilename, ORB_SLAM2::System::MONOCULAR, false);
     VSLAM.Shutdown(); // Do not need any thread
     VSLAM.RestoreSystemFromFile(KeyFrameDir, MapFileNmae);
     std::cout << "System Restored Successfully." << std::endl;
-    std::tie(RCL,tCL,s) = HECalib(vmTwc, vmTwl);
-    std::cout << "Ordinary Hand-eye Calibration:\n";
-    std::cout << "Rotation: \n" << RCL << std::endl;
-    std::cout << "Translation: \n" << tCL << std::endl;
-    std::cout << "s :" << s << std::endl;
-
-    std::tie(RCL,tCL,s) = HECalibRobustKernelg2o(vmTwc, vmTwl, RCL, tCL, s, true, 0.003);
-    std::cout << "Robust Kernel Hand-eye Calibration with Regulation:\n";
-    std::cout << "Rotation: \n" << RCL << std::endl;
-    std::cout << "Translation: \n" << tCL << std::endl;
+    std::cout << "Initial Extrinsic:\n";
+    std::cout << "Rotation: \n" << rigid.topLeftCorner(3,3) << std::endl;
+    std::cout << "Translation: \n" << rigid.topRightCorner(3,1) << std::endl;
     std::cout << "s :" << s << std::endl;
-    Eigen::Isometry3d rigidTCL = Eigen::Isometry3d::Identity();
-    rigidTCL.rotate(RCL);
-    rigidTCL.pretranslate(tCL);
 
-    Eigen::Isometry3d TCL0(rigidTCL);
+    Eigen::Isometry3d TCL0;
+    TCL0.matrix() = rigid;
     int ngood;
     if(method == "global"){
         ngood = VSLAM.OptimizeExtrinsicGlobal(vTwl, TCL0, s);
@@ -81,27 +76,12 @@ int main(int argc, char** argv){
     }
     else
     {
-        TCL0 = rigidTCL;
         ngood = VSLAM.OptimizeExtrinsicLocal(vTwl, TCL0, s);
         std::cout << "Bundle Adjustment Optimization (Local):\n";
         std::cout << "Rotation: \n" << TCL0.rotation() << std::endl;
         std::cout << "Translation: \n" << TCL0.translation() << std::endl;
-        std::cout<< "scale=  "<< s <<"  "<<"ngood=  "<< ngood <<std::endl;
+        std::cout<< "scale =  "<< s <<"  "<<"ngood=  "<< ngood <<std::endl;
     }
     writeSim3(resFileName, TCL0, s);
     std::cout << "Result of BA Calibration saved to " << resFileName << std::endl;
-
-
-
-    // std::tie(RCL,tCL,s) = HECalibRobustKernelg2o(vmTwc, vmTwl, RCL, tCL, s);
-    // std::cout << "Robust Hand-eye Calibration:\n";
-    // std::cout << "Rotation: \n" << RCL << std::endl;
-    // std::cout << "Translation: \n" << tCL << std::endl;
-    // std::cout << "s :" << s << std::endl;
-
-    // std::tie(RCL,tCL,s) = HECalibLPg2o(vmTwc, vmTwl, RCL, tCL, s);
-    // std::cout << "Line Process Hand-eye Calibration:\n";
-    // std::cout << "Rotation: \n" << RCL << std::endl;
-    // std::cout << "Translation: \n" << tCL << std::endl;
-    // std::cout << "s :" << s << std::endl;
 }

src/examples/he_calib.cpp

@@ -23,10 +23,12 @@ int main(int argc, char** argv){
     const std::string TwcFilename = base_dir + io_config["VOFile"].as<std::string>();
     const std::string TwlFilename = base_dir + io_config["LOFile"].as<std::string>();
     const std::string resFileName = base_dir + io_config["ResFile"].as<std::string>();
+    const std::string resRBFileName = base_dir + io_config["ResRBFile"].as<std::string>();
     const std::string resLPFileName = base_dir + io_config["ResLPFile"].as<std::string>();
     const std::string KyeFrameIdFile = base_dir + io_config["VOIdFile"].as<std::string>();
 
     const bool zero_translation = runtime_config["zero_translation"].as<bool>();
+    const double robust_kernel_size = runtime_config["robust_kernel_size"].as<double>();
     const bool regulation = runtime_config["regulation"].as<bool>();
     const double regulation_weight = runtime_config["regulation_weight"].as<double>();
     const int inner_iter = runtime_config["inner_iter"].as<int>();
@@ -40,17 +42,27 @@ int main(int argc, char** argv){
     ReadPoseList(TwlFilename, vTwlraw);
     YAML::Node FrameIdCfg = YAML::LoadFile(KyeFrameIdFile);
     std::vector<int> vKFFrameId = FrameIdCfg["mnFrameId"].as<std::vector<int>>();
-    for(auto &KFId:vKFFrameId)
-        vTwl.push_back(vTwlraw[KFId]);
+    vTwl.reserve(vKFFrameId.size());
+    if(vKFFrameId[0]==0)
+        for(auto &KFId:vKFFrameId)
+            vTwl.push_back(vTwlraw[KFId]);
+    else
+    {
+        Eigen::Isometry3d refPose = vTwlraw[vKFFrameId[0]].inverse();
+        for(auto &KFId:vKFFrameId)
+            vTwl.push_back(refPose * vTwlraw[KFId]);
+    }
     vTwlraw.clear();
     std::cout << "Load " << vTwc.size() << " Twc Pose files." << std::endl;
     std::cout << "Load " << vTwl.size() << " Twl Pose files." << std::endl;
     assert(vTwc.size()==vTwl.size());
+
     std::vector<Eigen::Isometry3d> vmTwc, vmTwl;
     vmTwc.reserve(vTwc.size()-1);
     vmTwl.reserve(vTwl.size()-1);
     pose2Motion(vTwc, vmTwc);
     pose2Motion(vTwl, vmTwl);
+
     Eigen::Matrix3d RCL;
     Eigen::Vector3d tCL;
     double s;
@@ -62,14 +74,16 @@ int main(int argc, char** argv){
     std::cout << "s :" << s << std::endl;
     if(zero_translation)
         tCL.setZero(); // Translation is too bad
-    std::tie(RCL,tCL,s) = HECalibRobustKernelg2o(vmTwc, vmTwl, RCL, tCL, s, regulation, regulation_weight, verborse);
+    writeSim3(resFileName, RCL, tCL, s);
+    std::cout << "Result of Hand-eye Calibration saved to " << resFileName << std::endl;
+    std::tie(RCL,tCL,s) = HECalibRobustKernelg2o(vmTwc, vmTwl, RCL, tCL, s, robust_kernel_size, regulation, regulation_weight, verborse);
     std::cout << "Robust Kernel Hand-eye Calibration with Regulation:\n";
     std::cout << "Rotation: \n" << RCL << std::endl;
     std::cout << "Translation: \n" << tCL << std::endl;
     std::cout << "scale :" << s << std::endl;
-    writeSim3(resFileName, RCL, tCL, s);
+    writeSim3(resRBFileName, RCL, tCL, s);
 
-    std::cout << "Result of Hand-eye Calibration saved to " << resFileName << std::endl;
+    std::cout << "Result of Hand-eye Calibration saved to " << resRBFileName << std::endl;
     std::tie(RCL,tCL,s) = HECalibLineProcessg2o(vmTwc, vmTwl, RCL, tCL, s, inner_iter, init_mu, divide_factor, min_mu, ex_iter, regulation, regulation_weight, verborse);
     std::cout << "Line Process Hand-eye Calibration:\n";
     std::cout << "Rotation: \n" << RCL << std::endl;

src/examples/iba_calib_plane2.cpp

@@ -168,7 +168,7 @@ void BuildProblem(const std::vector<VecVector3d> &PointClouds, const std::vector
             if(u1_list.size() == 0)
                 continue; // Should Not Run into
             ceres::LossFunction *loss_function = new ceres::HuberLoss(iba_params.robust_kernel_delta);
-            ceres::CostFunction *cost_function = IBA_PlaneFactor::Create(pKF->fx, pKF->fy, pKF->cx, pKF->cy, u0, v0,
+            ceres::CostFunction *cost_function = IBA_PlaneFactor::Create(H, W, pKF->fx, pKF->fy, pKF->cx, pKF->cy, u0, v0,
                 u1_list, v1_list, R_list, t_list, p0, n0);
             #pragma omp critical
             {

src/examples/iba_global.cpp

@@ -48,6 +48,7 @@ class IBAGlobalParams{
     int PointCloudskip = 1;
     bool PointCloudOnlyPositiveX = false;
     int max_bbeval = 200;
+    double valid_rate = 0.5;
     bool verborse = true;
     bool use_plane = true;
 };
@@ -90,7 +91,7 @@ void FindProjectCorrespondences(const VecVector3d &points, const ORB_SLAM2::KeyF
         std::vector<double> sq_dist(num_closest, std::numeric_limits<double>::max());
         nanoflann::KNNResultSet<double, IndexType> resultSet(num_closest);
         resultSet.init(indices.data(), sq_dist.data());
-        kdtree->index->findNeighbors(resultSet, vKeyUnEigen[i].data(), nanoflann::SearchParameters(0.0F, true));
+        kdtree->index->findNeighbors(resultSet, vKeyUnEigen[i].data(), nanoflann::SearchParameters());
         if(resultSet.size() > 0 && sq_dist[0] <= max_corr_dist * max_corr_dist)
             corrset.push_back(std::make_pair(i, ProjectIndex[indices[0]]));
     }
@@ -328,14 +329,19 @@ std::tuple<double, double, double, int, int> BAError(
             }
         }
     }
-    assert(valid_cnt_3d_2d > 0 && valid_cnt_3d_3d > 0);
-    corr_3d_2d_err /= valid_cnt_3d_2d;
-    corr_3d_3d_err /= valid_cnt_3d_3d;
+    if(valid_cnt_3d_2d==0 && iba_params.err_weight[0] > 1e-10)
+        corr_3d_2d_err = std::numeric_limits<double>::max();
+    else
+        corr_3d_2d_err /= valid_cnt_3d_2d;
+    if(valid_cnt_3d_3d==0 && iba_params.err_weight[1] > 1e-10)
+        corr_3d_3d_err = std::numeric_limits<double>::max();
+    else
+        corr_3d_3d_err /= valid_cnt_3d_3d;
     Cval /= Ccnt;
     // auto logger = LogEdges(corr_3d_3d_errlist);
     // print_map("corr3d_3d:",logger);
     if(verborse)
-        std::printf("plane: %d, point: %d\n", valid_pl_3d_3d, valid_pt_3d_3d);
+        std::printf("plane: %d, point: %d 3d-2d: %d\n", valid_pl_3d_3d, valid_pt_3d_3d, valid_cnt_3d_2d);
     return {corr_3d_2d_err, corr_3d_3d_err, Cval, valid_cnt_3d_2d, cnt_3d_2d};
 }
 
@@ -381,9 +387,11 @@ class BALoss: public NOMAD::Evaluator
         std::tie(f1val, f2val, Cval, valid_edge_cnt, edge_cnt) = BAError(xvec, *PointClouds, kdtree_list, *PointCloudPoses, *KFIdMap, *KeyFrames, *iba_params);
         NOMAD::Double f = f1val * iba_params->err_weight[0] + f2val * iba_params->err_weight[1];
         NOMAD::Double C1 = Cval - iba_params->he_threshold, C2 = -Cval - iba_params->he_threshold;  // |Cval| <= he_threshold
+        NOMAD::Double C3 = iba_params->valid_rate - static_cast<double>(valid_edge_cnt) / (edge_cnt+1);
         std::string bbo = f.tostring();
         bbo += " " + C1.tostring();
         bbo += " " + C2.tostring();
+        bbo += " " + C3.tostring();
         x.setBBO(bbo);
         countEval = true;
         return true;
@@ -456,9 +464,11 @@ int main(int argc, char** argv){
     iba_params.PointCloudskip = io_config["PointCloudskip"].as<int>();
     iba_params.PointCloudOnlyPositiveX = io_config["PointCloudOnlyPositiveX"].as<bool>();
     iba_params.max_bbeval = runtime_config["max_bbeval"].as<int>();
+    iba_params.valid_rate = runtime_config["valid_rate"].as<double>();
     iba_params.verborse = runtime_config["verborse"].as<bool>();
     iba_params.use_plane = runtime_config["use_plane"].as<bool>();
     bool use_cache = runtime_config["use_cache"].as<bool>();
+    bool use_vns = runtime_config["use_vns"].as<bool>();
     int seed = runtime_config["seed"].as<int>();
     const std::string NomadCacheFile = runtime_config["cacheFile"].as<std::string>();
     const double min_mesh = runtime_config["min_mesh"].as<double>();
@@ -553,6 +563,7 @@ int main(int argc, char** argv){
     bbOutputTypes.push_back(NOMAD::BBOutputType::OBJ);
     bbOutputTypes.push_back(NOMAD::BBOutputType::PB); // Relaxible Constraint |he_err| < delta
     bbOutputTypes.push_back(NOMAD::BBOutputType::PB); // Relaxible Constraint
+    bbOutputTypes.push_back(NOMAD::BBOutputType::PB); // Corr Valid Rate > threshold
     nomad_params->set_BB_OUTPUT_TYPE(bbOutputTypes);
     NOMAD::ArrayOfDouble nomad_lb(lb), nomad_ub(ub);
     nomad_params->set_LOWER_BOUND(nomad_lb);
@@ -566,7 +577,7 @@ int main(int argc, char** argv){
     NOMAD::ArrayOfDouble nomad_mesh_minsize(min_mesh_size), nomad_frame_init(init_frame_size);
     nomad_params->set_INITIAL_POLL_SIZE(nomad_frame_init);
     nomad_params->set_MIN_MESH_SIZE(nomad_mesh_minsize);
-    nomad_params->setAttributeValue("VNS_MADS_SEARCH",true);
+    nomad_params->setAttributeValue("VNS_MADS_SEARCH",use_vns);
     nomad_params->setAttributeValue("DIRECTION_TYPE",NOMAD::DirectionType::ORTHO_2N);
     nomad_params->checkAndComply();
     auto ev = std::make_unique<BALoss>(nomad_params->getEvalParams(), nomad_type,