Skip to content

Commit ff7264e

Browse files
gitounigitouni
authored
Add files via upload
1 parent 7eef6d1 commit ff7264e

16 files changed

+1277
-208
lines changed

‎src/examples/iba_calib.cpp‎

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@ class IBAParams{
3232
IBAParams(){};
3333
public:
3434
double max_pixel_dist = 2.; // maximum distance to build correspondence between projected LiDAR points and Keypoints
35-
int min_covis_weight = 200; // minmum convisibility weight between two KeyFrames for optimization
35+
int min_covis_weight = 200; // minmum covisibility weight between two KeyFrames for optimization
3636
int kdtree2d_max_leaf_size = 10; // maximum leaf size of the KDTree for projectedPoints-Keypoints correspondence Search
3737
int kdtree3d_max_leaf_size = 30; // maximum leaf size of the KDTree for neighborhood LiDAR points Search
3838
double neigh_radius = 0.6; // build local manifold within this radius
@@ -190,13 +190,13 @@ void BuildOptimizer(const std::vector<std::string> &PointCloudFiles, std::vector
190190
for(const CorrType &corr:corrset)
191191
indices.push_back(corr.second);
192192
std::tie(normals, flags) = ExtractLocalManifold(points, indices, params.neigh_radius, params.kdtree3d_max_leaf_size, params.neigh_max_pts);
193-
std::vector<ORB_SLAM2::KeyFrame*> pConvisKFs = pKF->GetCovisiblesByWeightSafe(params.min_covis_weight); // for debug
194-
std::vector<std::map<int, int>> KptMapList; // KeyPoints Correspondence between Reference KF and Convisible KeyFrame
195-
std::vector<Eigen::Matrix4d> relPoseList; // RelPose From Reference to Convisible KeyFrame
196-
KptMapList.reserve(pConvisKFs.size());
197-
relPoseList.reserve(pConvisKFs.size());
193+
std::vector<ORB_SLAM2::KeyFrame*> pCovisKFs = pKF->GetCovisiblesByWeightSafe(params.min_covis_weight); // for debug
194+
std::vector<std::map<int, int>> KptMapList; // KeyPoints Correspondence between Reference KF and Covisible KeyFrame
195+
std::vector<Eigen::Matrix4d> relPoseList; // RelPose From Reference to Covisible KeyFrame
196+
KptMapList.reserve(pCovisKFs.size());
197+
relPoseList.reserve(pCovisKFs.size());
198198
const cv::Mat invRefPose = pKF->GetPoseInverseSafe();
199-
for(auto pKFConv:pConvisKFs)
199+
for(auto pKFConv:pCovisKFs)
200200
{
201201
auto KptMap = pKF->GetMatchedKptIds(pKFConv);
202202
cv::Mat relPose = pKFConv->GetPose() * invRefPose; // Transfer from c1 coordinate to c2 coordinate
@@ -214,14 +214,14 @@ void BuildOptimizer(const std::vector<std::string> &PointCloudFiles, std::vector
214214
// transform 3d point back to LiDAR coordinate
215215
Eigen::Vector3d p0 = initSE3.inverse() * points[point3d_idx]; // cooresponding point (LiDAR coord)
216216
Eigen::Vector3d n0 = initSE3_4x4.topLeftCorner(3, 3).transpose() * normals[sub_idx]; // cooresponding point normal (LiDAR coord)
217-
for(std::size_t pKFConvi = 0; pKFConvi < pConvisKFs.size(); ++pKFConvi){
218-
auto pKFConv = pConvisKFs[pKFConvi];
217+
for(std::size_t pKFConvi = 0; pKFConvi < pCovisKFs.size(); ++pKFConvi){
218+
auto pKFConv = pCovisKFs[pKFConvi];
219219
// Skip if Cannot Find this 2d-3d matching map in Keypoint-to-Keypoint matching map
220220
if(KptMapList[pKFConvi].count(point2d_idx) == 0)
221221
continue;
222-
const int convis_idx = KptMapList[pKFConvi][point2d_idx]; // corresponding KeyPoints idx in a convisible KeyFrame
223-
double u1 = pKFConv->mvKeysUn[convis_idx].pt.x;
224-
double v1 = pKFConv->mvKeysUn[convis_idx].pt.y;
222+
const int covis_idx = KptMapList[pKFConvi][point2d_idx]; // corresponding KeyPoints idx in a covisible KeyFrame
223+
double u1 = pKFConv->mvKeysUn[covis_idx].pt.x;
224+
double v1 = pKFConv->mvKeysUn[covis_idx].pt.y;
225225
Eigen::Matrix4d relPose = relPoseList[pKFConvi]; // Twc2 * inv(Twc1)
226226
// IBATestEdge* e = new IBATestEdge(pKF->fx, pKF->fy, pKF->cx, pKF->cy, u0, v0, u1, v1, p0,
227227
// relPose.topLeftCorner(3, 3), relPose.topRightCorner(3, 1));

‎src/examples/iba_calib3d_gpr.cpp‎

Lines changed: 14 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -92,19 +92,19 @@ void BuildProblem(const std::vector<VecVector3d> &PointClouds, const std::vector
9292
mapKpt2Mpt.reserve(pKF->mmapMpt2Kpt.size());
9393
for(auto const& [key, val] : pKF->mmapMpt2Kpt)
9494
mapKpt2Mpt[val] = key;
95-
std::vector<ORB_SLAM2::KeyFrame*> pConvisKFs;
96-
if(iba_params.num_best_convis > 0)
97-
pConvisKFs = pKF->GetBestCovisibilityKeyFramesSafe(iba_params.num_best_convis); // for debug
95+
std::vector<ORB_SLAM2::KeyFrame*> pCovisKFs;
96+
if(iba_params.num_best_covis > 0)
97+
pCovisKFs = pKF->GetBestCovisibilityKeyFramesSafe(iba_params.num_best_covis); // for debug
9898
else
9999
{
100-
pConvisKFs = pKF->GetCovisiblesByWeightSafe(iba_params.min_covis_weight);
100+
pCovisKFs = pKF->GetCovisiblesByWeightSafe(iba_params.min_covis_weight);
101101
}
102102
std::vector<std::map<int, int>> KptMapList; // Keypoint-Keypoint Corr
103-
std::vector<Eigen::Matrix4d> relPoseList; // RelPose From Reference to Convisible KeyFrames
104-
KptMapList.reserve(pConvisKFs.size());
105-
relPoseList.reserve(pConvisKFs.size());
103+
std::vector<Eigen::Matrix4d> relPoseList; // RelPose From Reference to Covisible KeyFrames
104+
KptMapList.reserve(pCovisKFs.size());
105+
relPoseList.reserve(pCovisKFs.size());
106106
const cv::Mat invRefPose = pKF->GetPoseInverseSafe();
107-
for(auto pKFConv:pConvisKFs)
107+
for(auto pKFConv:pCovisKFs)
108108
{
109109
auto KptMap = pKF->GetMatchedKptIds(pKFConv);
110110
cv::Mat relPose = pKFConv->GetPose() * invRefPose; // Transfer from c1 coordinate to c2 coordinate
@@ -146,14 +146,14 @@ void BuildProblem(const std::vector<VecVector3d> &PointClouds, const std::vector
146146
std::vector<double> u1_list, v1_list;
147147
std::vector<Eigen::Matrix3d> R_list;
148148
std::vector<Eigen::Vector3d> t_list;
149-
for(std::size_t pKFConvi = 0; pKFConvi < pConvisKFs.size(); ++pKFConvi){
150-
auto pKFConv = pConvisKFs[pKFConvi];
149+
for(std::size_t pKFConvi = 0; pKFConvi < pCovisKFs.size(); ++pKFConvi){
150+
auto pKFConv = pCovisKFs[pKFConvi];
151151
// Skip if Cannot Find this 2d-3d matching map in Keypoint-to-Keypoint matching map
152152
if(KptMapList[pKFConvi].count(point2d_idx) == 0)
153153
continue;
154-
const int convis_idx = KptMapList[pKFConvi][point2d_idx]; // corresponding KeyPoints idx in a convisible KeyFrame
155-
double u1 = pKFConv->mvKeysUn[convis_idx].pt.x;
156-
double v1 = pKFConv->mvKeysUn[convis_idx].pt.y;
154+
const int covis_idx = KptMapList[pKFConvi][point2d_idx]; // corresponding KeyPoints idx in a covisible KeyFrame
155+
double u1 = pKFConv->mvKeysUn[covis_idx].pt.x;
156+
double v1 = pKFConv->mvKeysUn[covis_idx].pt.y;
157157
Eigen::Matrix4d relPose = relPoseList[pKFConvi]; // Twc2 * inv(Twc1)
158158
u1_list.push_back(u1);
159159
v1_list.push_back(v1);
@@ -249,7 +249,7 @@ int main(int argc, char** argv){
249249
// runtime config
250250
IBAGPR3dParams iba_params;
251251
iba_params.max_pixel_dist = runtime_config["max_pixel_dist"].as<double>();
252-
iba_params.num_best_convis = runtime_config["num_best_convis"].as<int>();
252+
iba_params.num_best_covis = runtime_config["num_best_covis"].as<int>();
253253
iba_params.min_covis_weight = runtime_config["min_covis_weight"].as<int>();
254254
iba_params.kdtree2d_max_leaf_size = runtime_config["kdtree2d_max_leaf_size"].as<int>();
255255
iba_params.kdtree3d_max_leaf_size = runtime_config["kdtree3d_max_leaf_size"].as<int>();

‎src/examples/iba_calib_gpr.cpp‎

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -170,13 +170,13 @@ void BuildOptimizer(const std::vector<std::string> &PointCloudFiles, std::vector
170170
for(const CorrType &corr:corrset)
171171
indices.push_back(corr.second);
172172
std::tie(normals, subindices) = ComputeLocalNormal(points, indices, params.norm_radius, params.kdtree3d_max_leaf_size, params.norm_max_pts);
173-
std::vector<ORB_SLAM2::KeyFrame*> pConvisKFs = pKF->GetBestCovisibilityKeyFramesSafe(10); // for debug
174-
std::vector<std::map<int, int>> KptMapList; // KeyPoints Correspondence between Reference KF and Convisible KeyFrame
175-
std::vector<Eigen::Matrix4d> relPoseList; // RelPose From Reference to Convisible KeyFrame
176-
KptMapList.reserve(pConvisKFs.size());
177-
relPoseList.reserve(pConvisKFs.size());
173+
std::vector<ORB_SLAM2::KeyFrame*> pCovisKFs = pKF->GetBestCovisibilityKeyFramesSafe(10); // for debug
174+
std::vector<std::map<int, int>> KptMapList; // KeyPoints Correspondence between Reference KF and Covisible KeyFrame
175+
std::vector<Eigen::Matrix4d> relPoseList; // RelPose From Reference to Covisible KeyFrame
176+
KptMapList.reserve(pCovisKFs.size());
177+
relPoseList.reserve(pCovisKFs.size());
178178
const cv::Mat invRefPose = pKF->GetPoseInverseSafe();
179-
for(auto pKFConv:pConvisKFs)
179+
for(auto pKFConv:pCovisKFs)
180180
{
181181
auto KptMap = pKF->GetMatchedKptIds(pKFConv);
182182
cv::Mat relPose = pKFConv->GetPose() * invRefPose; // Transfer from c1 coordinate to c2 coordinate
@@ -198,14 +198,14 @@ void BuildOptimizer(const std::vector<std::string> &PointCloudFiles, std::vector
198198
std::vector<double> u1_list, v1_list;
199199
std::vector<Eigen::Matrix3d> R_list;
200200
std::vector<Eigen::Vector3d> t_list;
201-
for(std::size_t pKFConvi = 0; pKFConvi < pConvisKFs.size(); ++pKFConvi){
202-
auto pKFConv = pConvisKFs[pKFConvi];
201+
for(std::size_t pKFConvi = 0; pKFConvi < pCovisKFs.size(); ++pKFConvi){
202+
auto pKFConv = pCovisKFs[pKFConvi];
203203
// Skip if Cannot Find this 2d-3d matching map in Keypoint-to-Keypoint matching map
204204
if(KptMapList[pKFConvi].count(point2d_idx) == 0)
205205
continue;
206-
const int convis_idx = KptMapList[pKFConvi][point2d_idx]; // corresponding KeyPoints idx in a convisible KeyFrame
207-
double u1 = pKFConv->mvKeysUn[convis_idx].pt.x;
208-
double v1 = pKFConv->mvKeysUn[convis_idx].pt.y;
206+
const int covis_idx = KptMapList[pKFConvi][point2d_idx]; // corresponding KeyPoints idx in a covisible KeyFrame
207+
double u1 = pKFConv->mvKeysUn[covis_idx].pt.x;
208+
double v1 = pKFConv->mvKeysUn[covis_idx].pt.y;
209209
Eigen::Matrix4d relPose = relPoseList[pKFConvi]; // Twc2 * inv(Twc1)
210210
u1_list.push_back(u1);
211211
v1_list.push_back(v1);

‎src/examples/iba_calib_gpr2.cpp‎

Lines changed: 14 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -85,19 +85,19 @@ void BuildProblem(const std::vector<VecVector3d> &PointClouds, const std::vector
8585
FindProjectCorrespondences(points, pKF, iba_params.kdtree2d_max_leaf_size, iba_params.max_pixel_dist, corrset);
8686
if(corrset.size() < iba_params.num_min_corr)
8787
continue;
88-
std::vector<ORB_SLAM2::KeyFrame*> pConvisKFs;
89-
if(iba_params.num_best_convis > 0)
90-
pConvisKFs = pKF->GetBestCovisibilityKeyFramesSafe(iba_params.num_best_convis); // for debug
88+
std::vector<ORB_SLAM2::KeyFrame*> pCovisKFs;
89+
if(iba_params.num_best_covis > 0)
90+
pCovisKFs = pKF->GetBestCovisibilityKeyFramesSafe(iba_params.num_best_covis); // for debug
9191
else
9292
{
93-
pConvisKFs = pKF->GetCovisiblesByWeightSafe(iba_params.min_covis_weight);
93+
pCovisKFs = pKF->GetCovisiblesByWeightSafe(iba_params.min_covis_weight);
9494
}
9595
std::vector<std::map<int, int>> KptMapList; // Keypoint-Keypoint Corr
96-
std::vector<Eigen::Matrix4d> relPoseList; // RelPose From Reference to Convisible KeyFrames
97-
KptMapList.reserve(pConvisKFs.size());
98-
relPoseList.reserve(pConvisKFs.size());
96+
std::vector<Eigen::Matrix4d> relPoseList; // RelPose From Reference to Covisible KeyFrames
97+
KptMapList.reserve(pCovisKFs.size());
98+
relPoseList.reserve(pCovisKFs.size());
9999
const cv::Mat invRefPose = pKF->GetPoseInverseSafe();
100-
for(auto pKFConv:pConvisKFs)
100+
for(auto pKFConv:pCovisKFs)
101101
{
102102
auto KptMap = pKF->GetMatchedKptIds(pKFConv);
103103
cv::Mat relPose = pKFConv->GetPose() * invRefPose; // Transfer from c1 coordinate to c2 coordinate
@@ -129,14 +129,14 @@ void BuildProblem(const std::vector<VecVector3d> &PointClouds, const std::vector
129129
std::vector<double> u1_list, v1_list;
130130
std::vector<Eigen::Matrix3d> R_list;
131131
std::vector<Eigen::Vector3d> t_list;
132-
for(std::size_t pKFConvi = 0; pKFConvi < pConvisKFs.size(); ++pKFConvi){
133-
auto pKFConv = pConvisKFs[pKFConvi];
132+
for(std::size_t pKFConvi = 0; pKFConvi < pCovisKFs.size(); ++pKFConvi){
133+
auto pKFConv = pCovisKFs[pKFConvi];
134134
// Skip if Cannot Find this 2d-3d matching map in Keypoint-to-Keypoint matching map
135135
if(KptMapList[pKFConvi].count(point2d_idx) == 0)
136136
continue;
137-
const int convis_idx = KptMapList[pKFConvi][point2d_idx]; // corresponding KeyPoints idx in a convisible KeyFrame
138-
double u1 = pKFConv->mvKeysUn[convis_idx].pt.x;
139-
double v1 = pKFConv->mvKeysUn[convis_idx].pt.y;
137+
const int covis_idx = KptMapList[pKFConvi][point2d_idx]; // corresponding KeyPoints idx in a covisible KeyFrame
138+
double u1 = pKFConv->mvKeysUn[covis_idx].pt.x;
139+
double v1 = pKFConv->mvKeysUn[covis_idx].pt.y;
140140
Eigen::Matrix4d relPose = relPoseList[pKFConvi]; // Twc2 * inv(Twc1)
141141
u1_list.push_back(u1);
142142
v1_list.push_back(v1);
@@ -202,7 +202,7 @@ int main(int argc, char** argv){
202202
// runtime config
203203
IBAGPRParams iba_params;
204204
iba_params.max_pixel_dist = runtime_config["max_pixel_dist"].as<double>();
205-
iba_params.num_best_convis = runtime_config["num_best_convis"].as<int>();
205+
iba_params.num_best_covis = runtime_config["num_best_covis"].as<int>();
206206
iba_params.min_covis_weight = runtime_config["min_covis_weight"].as<int>();
207207
iba_params.kdtree2d_max_leaf_size = runtime_config["kdtree2d_max_leaf_size"].as<int>();
208208
iba_params.kdtree3d_max_leaf_size = runtime_config["kdtree3d_max_leaf_size"].as<int>();

‎src/examples/iba_calib_plane.cpp‎

Lines changed: 17 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@ class IBAPlaneParams{
2727
IBAPlaneParams(){};
2828
public:
2929
double max_pixel_dist = 1.5;
30-
int num_best_convis = 3;
30+
int num_best_covis = 3;
3131
int min_covis_weight = 100;
3232
int kdtree2d_max_leaf_size = 10;
3333
int kdtree3d_max_leaf_size = 30;
@@ -166,22 +166,22 @@ void BuildOptimizer(const std::vector<std::string> &PointCloudFiles, std::vector
166166
FindProjectCorrespondences(points, pKF, iba_params.kdtree2d_max_leaf_size, iba_params.max_pixel_dist, corrset);
167167
if(corrset.size() < 50)
168168
continue;
169-
std::vector<ORB_SLAM2::KeyFrame*> pConvisKFs;
170-
if(iba_params.num_best_convis > 0)
171-
pConvisKFs = pKF->GetBestCovisibilityKeyFramesSafe(iba_params.num_best_convis); // for debug
169+
std::vector<ORB_SLAM2::KeyFrame*> pCovisKFs;
170+
if(iba_params.num_best_covis > 0)
171+
pCovisKFs = pKF->GetBestCovisibilityKeyFramesSafe(iba_params.num_best_covis); // for debug
172172
else
173173
{
174-
pConvisKFs = pKF->GetCovisiblesByWeightSafe(iba_params.min_covis_weight);
175-
if(pConvisKFs.size() > 10)
176-
pConvisKFs.resize(10); // maximum memory cache of g2o edges
174+
pCovisKFs = pKF->GetCovisiblesByWeightSafe(iba_params.min_covis_weight);
175+
if(pCovisKFs.size() > 10)
176+
pCovisKFs.resize(10); // maximum memory cache of g2o edges
177177
}
178178

179179
std::vector<std::map<int, int>> KptMapList; // Keypoint-Keypoint Corr
180-
std::vector<Eigen::Matrix4d> relPoseList; // RelPose From Reference to Convisible KeyFrames
181-
KptMapList.reserve(pConvisKFs.size());
182-
relPoseList.reserve(pConvisKFs.size());
180+
std::vector<Eigen::Matrix4d> relPoseList; // RelPose From Reference to Covisible KeyFrames
181+
KptMapList.reserve(pCovisKFs.size());
182+
relPoseList.reserve(pCovisKFs.size());
183183
const cv::Mat invRefPose = pKF->GetPoseInverseSafe();
184-
for(auto pKFConv:pConvisKFs)
184+
for(auto pKFConv:pCovisKFs)
185185
{
186186
auto KptMap = pKF->GetMatchedKptIds(pKFConv);
187187
cv::Mat relPose = pKFConv->GetPose() * invRefPose; // Transfer from c1 coordinate to c2 coordinate
@@ -209,14 +209,14 @@ void BuildOptimizer(const std::vector<std::string> &PointCloudFiles, std::vector
209209
std::vector<double> u1_list, v1_list;
210210
std::vector<Eigen::Matrix3d> R_list;
211211
std::vector<Eigen::Vector3d> t_list;
212-
for(std::size_t pKFConvi = 0; pKFConvi < pConvisKFs.size(); ++pKFConvi){
213-
auto pKFConv = pConvisKFs[pKFConvi];
212+
for(std::size_t pKFConvi = 0; pKFConvi < pCovisKFs.size(); ++pKFConvi){
213+
auto pKFConv = pCovisKFs[pKFConvi];
214214
// Skip if Cannot Find this 2d-3d matching map in Keypoint-to-Keypoint matching map
215215
if(KptMapList[pKFConvi].count(point2d_idx) == 0)
216216
continue;
217-
const int convis_idx = KptMapList[pKFConvi][point2d_idx]; // corresponding KeyPoints idx in a convisible KeyFrame
218-
double u1 = pKFConv->mvKeysUn[convis_idx].pt.x;
219-
double v1 = pKFConv->mvKeysUn[convis_idx].pt.y;
217+
const int covis_idx = KptMapList[pKFConvi][point2d_idx]; // corresponding KeyPoints idx in a covisible KeyFrame
218+
double u1 = pKFConv->mvKeysUn[covis_idx].pt.x;
219+
double v1 = pKFConv->mvKeysUn[covis_idx].pt.y;
220220
Eigen::Matrix4d relPose = relPoseList[pKFConvi]; // Twc2 * inv(Twc1)
221221
u1_list.push_back(u1);
222222
v1_list.push_back(v1);
@@ -289,7 +289,7 @@ int main(int argc, char** argv){
289289
// runtime config
290290
IBAPlaneParams iba_params;
291291
iba_params.max_pixel_dist = runtime_config["max_pixel_dist"].as<double>();
292-
iba_params.num_best_convis = runtime_config["num_best_convis"].as<double>();
292+
iba_params.num_best_covis = runtime_config["num_best_covis"].as<double>();
293293
iba_params.min_covis_weight = runtime_config["min_covis_weight"].as<int>();
294294
iba_params.kdtree2d_max_leaf_size = runtime_config["kdtree2d_max_leaf_size"].as<int>();
295295
iba_params.kdtree3d_max_leaf_size = runtime_config["kdtree3d_max_leaf_size"].as<int>();

0 commit comments

Comments
 (0)