Skip to content

Commit 1ba8561

Browse files
gitounigitouni
authored
Add files via upload
1 parent a6f7948 commit 1ba8561

File tree

4 files changed

+90
-47
lines changed

4 files changed

+90
-47
lines changed

‎src/examples/ba_calib.cpp‎

Lines changed: 48 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -6,18 +6,37 @@
66
#include "kitti_tools.h"
77
#include <limits>
88
#include "orb_slam/include/System.h"
9+
#include <yaml-cpp/yaml.h>
10+
#include <vector>
11+
#include <map>
912

1013
int main(int argc, char** argv){
11-
if(argc < 8){
12-
std::cout << "Got " << argc - 1 << " Parameters, but expected 7";
13-
throw std::invalid_argument("Expected parameters: Twc_file Twl_file res_file orb_setting_file orb_vocabulary_file keyFrame_dir map_file");
14+
if(argc < 4){
15+
std::cerr << "Got " << argc - 1 << " Parameters, but Expected parameters: yaml_file orb_setting_file orb_vocabulary_file" << std::endl;
1416
exit(0);
1517
}
16-
std::vector<Eigen::Isometry3d> vTwc, vTwl;
17-
std::string TwcFilename(argv[1]), TwlFilename(argv[2]), resFileName(argv[3]);
18-
std::string ORBSetFilename(argv[4]), VocFilename(argv[5]), KeyFrameDir(argv[6]), MapFileNmae(argv[7]);
18+
std::string config_file(argv[1]), ORBSetFilename(argv[2]), VocFilename(argv[3]);
19+
YAML::Node config = YAML::LoadFile(config_file);
20+
std::string base_dir = config["BaseDir"].as<std::string>();
21+
checkpath(base_dir);
22+
std::vector<Eigen::Isometry3d> vTwc, vTwl, vTwlraw;
23+
std::string TwcFilename = base_dir + config["VOFile"].as<std::string>();
24+
std::string TwlFilename = base_dir + config["LOFile"].as<std::string>();
25+
std::string resFileName = base_dir + config["ResFile"].as<std::string>();
26+
std::string KeyFrameDir = base_dir + config["KeyFrameDir"].as<std::string>();
27+
std::string KyeFrameIdFile = base_dir + config["VOIdFile"].as<std::string>();
28+
std::string MapFileNmae = base_dir + config["MapFile"].as<std::string>();
29+
std::map<std::string, int> method_options = config["method_options"].as<std::map<std::string, int> >();
30+
std::string method = config["method"].as<std::string>();
31+
assert(method_options.count(method)>0);
32+
std::cout << "Use " << method << " Bundle Adjustment for calibration." << std::endl;
1933
ReadPoseList(TwcFilename, vTwc);
20-
ReadPoseList(TwlFilename, vTwl);
34+
ReadPoseList(TwlFilename, vTwlraw);
35+
YAML::Node FrameIdCfg = YAML::LoadFile(KyeFrameIdFile);
36+
std::vector<int> vKFFrameId = FrameIdCfg["mnFrameId"].as<std::vector<int>>();
37+
for(auto &KFId:vKFFrameId)
38+
vTwl.push_back(vTwlraw[KFId]);
39+
vTwlraw.clear();
2140
std::cout << "Load " << vTwc.size() << " Twc Pose files." << std::endl;
2241
std::cout << "Load " << vTwl.size() << " Twl Pose files." << std::endl;
2342
assert(vTwc.size()==vTwl.size());
@@ -31,8 +50,7 @@ int main(int argc, char** argv){
3150
double s;
3251

3352

34-
// writePose(resFileName, rigidTCL, s);
35-
// std::cout << "Result of Hand-eye Calibration saved to " << resFileName << std::endl;
53+
3654
ORB_SLAM2::System VSLAM(VocFilename, ORBSetFilename, ORB_SLAM2::System::MONOCULAR, false);
3755
VSLAM.Shutdown(); // Do not need any thread
3856
VSLAM.RestoreSystemFromFile(KeyFrameDir, MapFileNmae);
@@ -44,28 +62,36 @@ int main(int argc, char** argv){
4462
std::cout << "s :" << s << std::endl;
4563

4664
std::tie(RCL,tCL,s) = HECalibRobustKernelg2o(vmTwc, vmTwl, RCL, tCL, s, true, 0.003);
47-
std::cout << "Robust Kernel Hand-eye Calibration:\n";
65+
std::cout << "Robust Kernel Hand-eye Calibration with Regulation:\n";
4866
std::cout << "Rotation: \n" << RCL << std::endl;
4967
std::cout << "Translation: \n" << tCL << std::endl;
5068
std::cout << "s :" << s << std::endl;
5169
Eigen::Isometry3d rigidTCL = Eigen::Isometry3d::Identity();
5270
rigidTCL.rotate(RCL);
53-
// rigidTCL.pretranslate(tCL);
71+
rigidTCL.pretranslate(tCL);
5472

5573
Eigen::Isometry3d TCL0(rigidTCL);
5674
int ngood;
57-
// ngood = VSLAM.OptimizeExtrinsicGlobal(vTwl, TCL0, s);
58-
// std::cout << "Bundle Adjustment Optimization (Global):\n";
59-
// std::cout << "Rotation: \n" << TCL0.rotation() << std::endl;
60-
// std::cout << "Translation: \n" << TCL0.translation() << std::endl;
61-
// std::cout<< "scale= "<< s <<" "<<"ngood= "<<ngood<<std::endl;
75+
if(method == "global"){
76+
ngood = VSLAM.OptimizeExtrinsicGlobal(vTwl, TCL0, s);
77+
std::cout << "Bundle Adjustment Optimization (Global):\n";
78+
std::cout << "Rotation: \n" << TCL0.rotation() << std::endl;
79+
std::cout << "Translation: \n" << TCL0.translation() << std::endl;
80+
std::cout<< "scale= "<< s <<" "<<"ngood= "<<ngood<<std::endl;
81+
}
82+
else
83+
{
84+
TCL0 = rigidTCL;
85+
ngood = VSLAM.OptimizeExtrinsicLocal(vTwl, TCL0, s);
86+
std::cout << "Bundle Adjustment Optimization (Local):\n";
87+
std::cout << "Rotation: \n" << TCL0.rotation() << std::endl;
88+
std::cout << "Translation: \n" << TCL0.translation() << std::endl;
89+
std::cout<< "scale= "<< s <<" "<<"ngood= "<< ngood <<std::endl;
90+
}
91+
writeSim3(resFileName, TCL0, s);
92+
std::cout << "Result of BA Calibration saved to " << resFileName << std::endl;
93+
6294

63-
TCL0 = rigidTCL;
64-
ngood = VSLAM.OptimizeExtrinsicLocal(vTwl, TCL0, s);
65-
std::cout << "Bundle Adjustment Optimization (Local):\n";
66-
std::cout << "Rotation: \n" << TCL0.rotation() << std::endl;
67-
std::cout << "Translation: \n" << TCL0.translation() << std::endl;
68-
std::cout<< "scale= "<< s <<" "<<"ngood= "<< ngood <<std::endl;
6995

7096
// std::tie(RCL,tCL,s) = HECalibRobustKernelg2o(vmTwc, vmTwl, RCL, tCL, s);
7197
// std::cout << "Robust Hand-eye Calibration:\n";

‎src/examples/epipolar_calib.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@ int main(int argc, char** argv){
3535
double s;
3636

3737

38-
// writePose(resFileName, rigidTCL, s);
38+
// writeSim3(resFileName, rigidTCL, s);
3939
// std::cout << "Result of Hand-eye Calibration saved to " << resFileName << std::endl;
4040
ORB_SLAM2::System VSLAM(VocFilename, ORBSetFilename, ORB_SLAM2::System::MONOCULAR, false);
4141
VSLAM.Shutdown(); // Do not need any thread

‎src/examples/floam_backend.cpp‎

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,8 @@ int main(int argc, char** argv){
3939
option.MRmaxIter = config["multiway"]["MRmaxIter"].as<int>();
4040
option.MRmaxCorrDist = config["multiway"]["MRmaxCorrDist"].as<double>();
4141
option.MREdgePruneThre = config["multiway"]["MREdgePruneThre"].as<double>();
42+
bool odom_refine = config["multiway"]["RefineOdom"].as<bool>();
43+
bool use_multiway = config["multiway"]["use"].as<bool>();
4244
std::cout << "Parameters have been loaded from " << yaml_file << "." << std::endl;
4345
BackEndOptimizer optimizer(option);
4446
std::thread p(&BackEndOptimizer::LoopClosureRegThread, &optimizer);
@@ -48,8 +50,10 @@ int main(int argc, char** argv){
4850
optimizer.UpdateISAM();
4951
optimizer.writePoses(output_isam_poses);
5052
// *** Results of ISAM and Multway are too similar! ***
51-
// optimizer.MultiRegistration();
52-
// optimizer.writePoseGraph(output_pose_graph);
53-
// optimizer.UpdatePosesFromPG();
54-
// optimizer.writePoses(output_mr_poses);
53+
if(use_multiway){
54+
optimizer.MultiRegistration(odom_refine);
55+
optimizer.writePoseGraph(output_pose_graph);
56+
optimizer.UpdatePosesFromPG();
57+
optimizer.writePoses(output_mr_poses);
58+
}
5559
}

‎src/examples/he_demo_kitti.cpp‎

Lines changed: 33 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -5,18 +5,38 @@
55
#include <string>
66
#include "kitti_tools.h"
77
#include <limits>
8-
8+
#include <yaml-cpp/yaml.h>
9+
#include <vector>
10+
#include <map>
911

1012
int main(int argc, char** argv){
11-
if(argc < 4){
12-
std::cout << "Got " << argc - 1 << " Parameters, but expected 3";
13-
throw std::invalid_argument("Expected parameters: Twc_file Twl_file res_file");
13+
if(argc < 2){
14+
std::cerr << "Got " << argc - 1 << " Parameters, but Expected parameters: yaml_file" << std::endl;
1415
exit(0);
1516
}
16-
std::vector<Eigen::Isometry3d> vTwc, vTwl;
17-
std::string TwcFilename(argv[1]), TwlFilename(argv[2]), resFileName(argv[3]);
17+
std::string config_file(argv[1]);
18+
YAML::Node config = YAML::LoadFile(config_file);
19+
YAML::Node io_config = config["io"];
20+
YAML::Node runtime_config = config["runtime"];
21+
std::string base_dir = io_config["BaseDir"].as<std::string>();
22+
checkpath(base_dir);
23+
std::vector<Eigen::Isometry3d> vTwc, vTwl, vTwlraw;
24+
std::string TwcFilename = base_dir + io_config["VOFile"].as<std::string>();
25+
std::string TwlFilename = base_dir + io_config["LOFile"].as<std::string>();
26+
std::string resFileName = base_dir + io_config["ResFile"].as<std::string>();
27+
std::string KyeFrameIdFile = base_dir + io_config["VOIdFile"].as<std::string>();
28+
29+
bool regulation = runtime_config["regulation"].as<bool>();
30+
double regulation_weight = runtime_config["regulation_weight"].as<double>();
31+
bool verborse = runtime_config["verborse"].as<bool>();
32+
1833
ReadPoseList(TwcFilename, vTwc);
19-
ReadPoseList(TwlFilename, vTwl);
34+
ReadPoseList(TwlFilename, vTwlraw);
35+
YAML::Node FrameIdCfg = YAML::LoadFile(KyeFrameIdFile);
36+
std::vector<int> vKFFrameId = FrameIdCfg["mnFrameId"].as<std::vector<int>>();
37+
for(auto &KFId:vKFFrameId)
38+
vTwl.push_back(vTwlraw[KFId]);
39+
vTwlraw.clear();
2040
std::cout << "Load " << vTwc.size() << " Twc Pose files." << std::endl;
2141
std::cout << "Load " << vTwl.size() << " Twl Pose files." << std::endl;
2242
assert(vTwc.size()==vTwl.size());
@@ -35,21 +55,14 @@ int main(int argc, char** argv){
3555
std::cout << "Translation: \n" << tCL << std::endl;
3656
std::cout << "s :" << s << std::endl;
3757

58+
std::tie(RCL,tCL,s) = HECalibRobustKernelg2o(vmTwc, vmTwl, RCL, tCL, s, regulation, regulation_weight, verborse);
59+
std::cout << "Robust Kernel Hand-eye Calibration with Regulation:\n";
60+
std::cout << "Rotation: \n" << RCL << std::endl;
61+
std::cout << "Translation: \n" << tCL << std::endl;
62+
std::cout << "scale :" << s << std::endl;
3863
Eigen::Isometry3d rigidTCL = Eigen::Isometry3d::Identity();
3964
rigidTCL.rotate(RCL);
4065
rigidTCL.pretranslate(tCL);
41-
42-
writePose(resFileName, rigidTCL, s);
66+
writeSim3(resFileName, rigidTCL, s);
4367
std::cout << "Result of Hand-eye Calibration saved to " << resFileName << std::endl;
44-
std::tie(RCL,tCL,s) = HECalibRobustKernelg2o(vmTwc, vmTwl, RCL, tCL, s);
45-
std::cout << "Robust Hand-eye Calibration:\n";
46-
std::cout << "Rotation: \n" << RCL << std::endl;
47-
std::cout << "Translation: \n" << tCL << std::endl;
48-
std::cout << "s :" << s << std::endl;
49-
50-
std::tie(RCL,tCL,s) = HECalibLPg2o(vmTwc, vmTwl, RCL, tCL, s);
51-
std::cout << "Line Process Hand-eye Calibration:\n";
52-
std::cout << "Rotation: \n" << RCL << std::endl;
53-
std::cout << "Translation: \n" << tCL << std::endl;
54-
std::cout << "s :" << s << std::endl;
5568
}

0 commit comments

Comments
 (0)