Skip to content

Commit fe999c0

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

File tree

3 files changed

+223
-17
lines changed

3 files changed

+223
-17
lines changed

‎include/backend_opt.h‎

Lines changed: 175 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,175 @@
1+
#include <cmath>
2+
#include <tuple>
3+
#include <mutex>
4+
#include <chrono>
5+
#include <thread>
6+
#include <unordered_map>
7+
#include <queue>
8+
#include <Eigen/Dense>
9+
10+
#include <scancontext/Scancontext.h>
11+
#include <open3d/geometry/PointCloud.h>
12+
#include <open3d/pipelines/registration/Registration.h>
13+
#include <open3d/pipelines/registration/PoseGraph.h>
14+
#include <open3d/pipelines/registration/GlobalOptimization.h>
15+
#include <open3d/pipelines/registration/GlobalOptimizationMethod.h>
16+
#include <open3d/utility/Logging.h>
17+
#include <open3d/io/PoseGraphIO.h>
18+
#include <open3d/pipelines/registration/Feature.h>
19+
20+
#include <open3d/visualization/visualizer/Visualizer.h>
21+
22+
#include <kitti_tools.h>
23+
#include <io_tools.h>
24+
25+
#include <gtsam/inference/Symbol.h>
26+
#include <gtsam/nonlinear/Values.h>
27+
#include <gtsam/nonlinear/Marginals.h>
28+
#include <gtsam/geometry/Rot3.h>
29+
#include <gtsam/geometry/Pose3.h>
30+
#include <gtsam/slam/PriorFactor.h>
31+
#include <gtsam/slam/BetweenFactor.h>
32+
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
33+
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
34+
#include <gtsam/nonlinear/ISAM2.h>
35+
36+
gtsam::Pose3 Eigen2GTSAM(const Eigen::Isometry3d &poseEigen);
37+
gtsam::Pose3 Eigen2GTSAM(const Eigen::Matrix4d &poseEigen);
38+
39+
std::tuple<double,double> PoseDif(Eigen::Isometry3d &srcPose, Eigen::Isometry3d &tgtPose);
40+
std::tuple<double,double> PoseDif(Eigen::Matrix4d &srcPose, Eigen::Matrix4d &tgtPose);
41+
42+
open3d::pipelines::registration::RegistrationResult Registration(const open3d::geometry::PointCloud &srcPointCloud, const open3d::geometry::PointCloud &tgtPointCloud, const Eigen::Matrix4d &initialPose, const double icp_corr_dist);
43+
open3d::pipelines::registration::RegistrationResult Registration(const open3d::geometry::PointCloud &srcPointCloud, const open3d::geometry::PointCloud &tgtPointCloud, const Eigen::Matrix4d &initialPose, const double icp_coarse_dist, const double icp_refine_dist);
44+
45+
open3d::geometry::PointCloud ReadPointCloudO3D(const std::string filename);
46+
47+
class BackEndOption{
48+
public:
49+
bool verborse = false;
50+
bool vis = false;
51+
double voxel = 0.4;
52+
double o3d_voxel = 0.2;
53+
double keyframeMeterGap = 2;
54+
double keyframeRadGap = 0.2;
55+
double LCkeyframeMeterGap = 20;
56+
double LCkeyframeRadGap = 2.0;
57+
int LCSubmapSize = 25;
58+
59+
double scDistThres = 0.2;
60+
double scMaximumRadius = 80;
61+
double loopOverlapThre = 0.8; // the overlapping area (# of inlier correspondences / # of points in target). Higher is better.
62+
double loopInlierRMSEThre = 0.3;
63+
double icp_corase_dist = 0.4 * 15;
64+
double icp_refine_dist = 0.4 * 1.5;
65+
int icp_max_iter = 30;
66+
67+
double relinearizeThreshold = 0.01;
68+
int relinearizeSkip = 1;
69+
gtsam::ISAM2Params::Factorization factorization = gtsam::ISAM2Params::QR;
70+
gtsam::Vector6 priorNoiseVector6;
71+
gtsam::Vector6 odomNoiseVector6;
72+
gtsam::Vector6 robustNoiseVector6;
73+
74+
double MRmaxCorrDist = 0.6;
75+
double MREdgePruneThre = 0.25;
76+
int MRmaxIter = 100;
77+
78+
public:
79+
BackEndOption(){
80+
priorNoiseVector6 << 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12;
81+
odomNoiseVector6 << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4;
82+
robustNoiseVector6 << 0.5, 0.5, 0.5, 0.5, 0.5, 0.5;
83+
}
84+
};
85+
86+
class BackEndOptimizer{
87+
public:
88+
BackEndOptimizer(const BackEndOption &option);
89+
void LoadPose(const std::string dirname);
90+
void LoadDataFilename(const std::string dirname);
91+
Eigen::Isometry3d getPose(const int i);
92+
Eigen::Isometry3d getrelPose(const int i, const int j);
93+
open3d::geometry::PointCloud LoadPCD(const int idx) const;
94+
open3d::geometry::PointCloud LoadPCD(const int idx, double voxel_) const;
95+
open3d::geometry::PointCloud MergeLoadPCD(const int ref_idx, const int start_idx, const int end_idx);
96+
void AddLoopClosure(const std::tuple<int,int,open3d::geometry::PointCloud> &item);
97+
bool IsLoopBufEmpty();
98+
void AddPriorFactor(const int i, const gtsam::Pose3 &pose);
99+
void AddOdomFactor(const int i, const int j, const gtsam::Pose3 &relPose, const gtsam::Pose3 &currPose);
100+
void AddBetweenFactor(const gtsam::BetweenFactor<gtsam::Pose3> &factor);
101+
std::tuple<int,int,open3d::geometry::PointCloud> PopLoopClosure();
102+
void SCManage(const int i = 0); // for initialization
103+
void SCManage(const int i, const int j); // for odometry
104+
void LoopClosureRegThread(void);
105+
bool CheckStopLoopClosure(void);
106+
void StopLoopClosure(void);
107+
void UpdateISAM();
108+
void UpdatePosesFromEstimates(const gtsam::Values &Estimates);
109+
void UpdatePosesFromInitialEstimates();
110+
void UpdatePosesFromPG();
111+
void LoopClosureRun(const std::string pose_dir, const std::string file_dir);
112+
void PerformLoopClosure(const int j, open3d::geometry::PointCloud &PointCloud);
113+
void MultiRegistration(bool OdomRefinement=false);
114+
void writePoseGraph(const std::string &filename) const;
115+
void writePoses(const std::string &filename);
116+
117+
private:
118+
bool verborse;
119+
bool vis;
120+
const double voxel;
121+
const double o3d_voxel;
122+
const double keyframeMeterGap;
123+
const double keyframeRadGap;
124+
const double scDistThres;
125+
const double scMaximumRadius;
126+
127+
const double loopOverlapThre;
128+
const double loopInlierRMSEThre;
129+
const int LCSubmapSize;
130+
const double LCkeyframeMeterGap;
131+
const double LCkeyframeRadGap;
132+
133+
const double icp_corase_dist;
134+
const double icp_refine_dist;
135+
const int icp_max_iter;
136+
137+
const double MRmaxCorrDist;
138+
const double MREdgePruneThre;
139+
const int MRmaxIter;
140+
141+
double translationAccumulated;
142+
double rotationAccumulated;
143+
double LCtranslationAccumulated;
144+
double LCrotationAccumulated;
145+
146+
bool IsLoopClosureStop;
147+
148+
std::vector<Eigen::Isometry3d> RawFramePoses;
149+
std::vector<std::string> PointCloudFiles;
150+
SC2::SCManager SCManager;
151+
open3d::pipelines::registration::PoseGraph PoseGraph;
152+
153+
// SCManager
154+
std::vector<Eigen::Isometry3d> FramePoses;
155+
std::vector<int> KeyFrameQuery;
156+
std::queue<std::tuple<int, int, open3d::geometry::PointCloud>> LoopClosureBuf;
157+
std::unordered_map<int, int> LoopQueryMap;
158+
159+
160+
// GTSAM
161+
Eigen::Isometry3d OdomPose;
162+
gtsam::ISAM2* isam;
163+
gtsam::Values initialEstimate;
164+
gtsam::NonlinearFactorGraph FactorGraph;
165+
gtsam::noiseModel::Diagonal::shared_ptr priorNoise;
166+
gtsam::noiseModel::Diagonal::shared_ptr odomNoise;
167+
gtsam::noiseModel::Base::shared_ptr robustLoopNoise;
168+
169+
std::mutex LoopClosureMutex;
170+
std::mutex LoopBufMutex;
171+
std::mutex FactorGraphMutex;
172+
std::mutex FramePoseMutex;
173+
174+
175+
};

‎include/io_tools.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
#include <vector>
66
#include "pcl/io/pcd_io.h"
77
#include "kitti_tools.h"
8-
#include <opencv2/opencv.hpp>
8+
#include <opencv2/highgui/highgui.hpp>
99

1010
template <typename PointType>
1111
bool readPointCloud(const std::string filename, pcl::PointCloud<PointType> &point_cloud)
@@ -147,7 +147,7 @@ bool readImage(const std::string &strImageFilename, cv::Mat &img, int flag=cv::I
147147
* @param fs // output file stream
148148
* @param mat // pose 3x4 or 4x4
149149
*/
150-
void writeKittiData(std::ofstream &fs, Eigen::Isometry3d &mat, bool end){
150+
void writeKittiData(std::ofstream &fs, const Eigen::Isometry3d &mat, bool end){
151151
short cnt = 0;
152152
for(short i=0; i<3; ++i)
153153
for(short j=0; j<4; ++j){

‎include/kitti_tools.h‎

Lines changed: 46 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
#pragma once
22
#include <fstream>
3+
#include <iostream>
34
#include <string>
45
#include <sys/stat.h> // F_OK
56
#include <unistd.h> // access
@@ -37,8 +38,7 @@ int remove_directory(const char *path) {
3738
try{
3839
char cmd[100];
3940
sprintf(cmd,"rm -rf %s",path);
40-
system(cmd);
41-
return 0;
41+
return system(cmd);
4242
}catch(const std::exception &e){
4343
std::cout << e.what() << std::endl;
4444
return -1;
@@ -70,17 +70,17 @@ void ReadPoseList(const std::string &fileName, std::vector<Eigen::Isometry3d> &v
7070
}
7171
std::ifstream ifs(fileName);
7272
while(ifs.peek()!=EOF){
73-
std::vector<double> data(12,0);
74-
for(auto &d:data){
75-
ifs >> d;
73+
double data[16];
74+
for(int i=0; i<12; ++i){
75+
ifs >> data[i];
7676
}
77-
Eigen::Isometry3d mat = Eigen::Isometry3d::Identity(); //
78-
Eigen::Matrix3d rotation;
79-
Eigen::Vector3d translation;
80-
rotation << data[0], data[1], data[2], data[4], data[5], data[6], data[8], data[9], data[10];
81-
translation << data[3], data[7], data[11];
82-
mat.rotate(rotation);
83-
mat.pretranslate(translation);
77+
data[12] = 0.0;
78+
data[13] = 0.0;
79+
data[14] = 0.0;
80+
data[15] = 1.0;
81+
Eigen::Map<Eigen::Matrix4d, Eigen::RowMajor> mat4d(data);
82+
Eigen::Isometry3d mat; //
83+
mat.matrix() = mat4d.matrix().transpose();
8484
vPose.push_back(mat);
8585
}
8686
ifs.close();
@@ -93,17 +93,48 @@ void ReadPoseList(const std::string &fileName, std::vector<Eigen::Isometry3d> &v
9393
* @param pose Rigid Transformation Matrix
9494
* @param scale Monocular Scale Factor
9595
*/
96-
void writePose(std::string &outfile, const Eigen::Isometry3d &pose, const double scale){
96+
void writeSim3(const std::string &outfile, const Eigen::Isometry3d &pose, const double scale){
9797
std::ofstream of(outfile, std::ios::ate);
9898
of.precision(std::numeric_limits< double >::max_digits10);
99-
for(Eigen::Index ri=0; ri<pose.rows(); ++ri)
100-
for(Eigen::Index ci=0; ci<pose.cols(); ++ci){
99+
for(Eigen::Index ri=0; ri<3; ++ri)
100+
for(Eigen::Index ci=0; ci<4; ++ci){
101101
of << pose(ri,ci) << " ";
102102
}
103103
of << scale;
104104
of.close();
105105
}
106106

107+
/**
108+
* @brief write 13 entries: 12 for 3x4 Rigid, 1 for scale
109+
*
110+
* @param outfile File to Write (ASCII)
111+
* @param pose Rigid Transformation Matrix
112+
* @param scale Monocular Scale Factor
113+
*/
114+
void writeSim3(const std::string &outfile, const Eigen::Matrix4d &pose, const double scale){
115+
std::ofstream of(outfile, std::ios::ate);
116+
of.precision(std::numeric_limits< double >::max_digits10);
117+
for(Eigen::Index ri=0; ri<3; ++ri)
118+
for(Eigen::Index ci=0; ci<4; ++ci){
119+
of << pose(ri,ci) << " ";
120+
}
121+
of << scale;
122+
of.close();
123+
}
124+
125+
std::tuple<Eigen::Matrix4d, double> readSim3(const std::string &file){
126+
std::ifstream ifs(file);
127+
double scale = 1.0;
128+
Eigen::Matrix4d mat(Eigen::Matrix4d::Identity());
129+
for(Eigen::Index ri=0; ri<3; ++ri)
130+
for(Eigen::Index ci=0; ci<4; ++ci){
131+
ifs >> mat(ri,ci);
132+
}
133+
ifs >> scale;
134+
ifs.close();
135+
return {mat, scale};
136+
}
137+
107138
void pose2Motion(std::vector<Eigen::Isometry3d> &vAbsPose, std::vector<Eigen::Isometry3d> &vRelPose){
108139
for(std::size_t i = 0; i < vAbsPose.size()-1; ++i){
109140
Eigen::Isometry3d mTwc(vAbsPose[i+1] * vAbsPose[i].inverse()); // T(i+1) * T(i).inverse()

0 commit comments

Comments
 (0)