Skip to content

Commit

Permalink
added stereo multi scene retrieve
Browse files Browse the repository at this point in the history
  • Loading branch information
@cyanine-gi
cyanine-gi committed Apr 16, 2019
1 parent 1386c5f commit 376fdb8
Show file tree
Hide file tree
Showing 3 changed files with 260 additions and 22 deletions.
226 changes: 225 additions & 1 deletion algorithms/GlobalOptimizationGraph/GlobalOptimizationGraph.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,231 @@ bool GlobalOptimizationGraph::init_gps()//init longitude,latitude,altitude.
this->gps_init_alt_variance = vari_alt;

return true;
}
}/*
bool GlobalOptimizationGraph::SpeedInitialization() {
// 速度 初始化 需要由后端提供关键帧（不需要地图）
std::deque<shared_ptr<Frame>> vpKFs = mpBackEnd->GetAllKF();
int N = vpKFs.size();
if (N < setting::minInitKFs) // 初始化需要若干个关键帧
return false;
// Note 1.
// Input : N (N>=4) KeyFrame/Frame poses of stereo vslam
// Assume IMU bias are identical for N KeyFrame/Frame
// Compute :
// bg: gyroscope bias
// ba: accelerometer bias
// gv: gravity in vslam frame
// Vwbi: velocity of N KeyFrame/Frame
// (Absolute scale is available for stereo vslam)
// Note 2.
// Convention by wangjing:
// W: world coordinate frame (z-axis aligned with gravity, gW=[0;0;~9.8])
// B: body coordinate frame (IMU)
// V: camera frame of first camera (vslam's coordinate frame)
// TWB/T : 6dof pose of frame, TWB = [RWB, PWB; 0, 1], XW = RWB*XW + PWB
// RWB/R : 3dof rotation of frame's body(IMU)
// PWB/P : 3dof translation of frame's body(IMU) in world
// XW/XB : 3dof point coordinate in world/body
// Step0. get all keyframes in map
// reset v/bg/ba to 0
// re-compute pre-integration
Vector3d v3zero = Vector3d::Zero();
for (auto pKF: vpKFs) {
pKF->SetBiasG(v3zero);
pKF->SetBiasA(v3zero);
}
for (int i = 1; i < N; i++) {
vpKFs[i]->ComputeIMUPreInt();
}
// Step1. gyroscope bias estimation
// update bg and re-compute pre-integration
// 第一步，估计陀螺偏置
Vector3d bgest = IMUInitEstBg(vpKFs);
// 重新计算预积分器
for (auto pKF: vpKFs) {
pKF->SetBiasG(bgest);
}
for (int i = 1; i < N; i++) {
vpKFs[i]->ComputeIMUPreInt();
}
// Step2. accelerometer bias and gravity estimation (gv = Rvw*gw)
// let's first assume ba is given by prior and solve the gw
// Step 2.1 gravity estimation
// Solve C*x=D for x=[gw] (3+3)x1 vector
// \see section IV in "Visual Inertial Monocular SLAM with Map Reuse"
Vector3d baPrior = setting::biasAccePrior;
MatrixXd C(3 * (N - 2), 3);
C.setZero();
VectorXd D(3 * (N - 2));
D.setZero();
Matrix3d I3 = Matrix3d::Identity();
for (int i = 0; i < N - 2; i++) {
// 三个帧才能建立加速度约束
shared_ptr<Frame> pKF1 = vpKFs[i];
shared_ptr<Frame> pKF2 = vpKFs[i + 1];
shared_ptr<Frame> pKF3 = vpKFs[i + 2];
// Poses
Matrix3d R1 = pKF1->mRwb.matrix();
Matrix3d R2 = pKF2->mRwb.matrix();
Vector3d p1 = pKF1->mTwb;
Vector3d p2 = pKF2->mTwb;
Vector3d p3 = pKF3->mTwb;
// Delta time between frames
double dt12 = pKF2->mIMUPreInt.getDeltaTime();
double dt23 = pKF3->mIMUPreInt.getDeltaTime();
// Pre-integrated measurements
Vector3d dp12 = pKF2->mIMUPreInt.getDeltaP();
Vector3d dv12 = pKF2->mIMUPreInt.getDeltaV();
Vector3d dp23 = pKF3->mIMUPreInt.getDeltaP();
Matrix3d Jpba12 = pKF2->mIMUPreInt.getJPBiasa();
Matrix3d Jvba12 = pKF2->mIMUPreInt.getJVBiasa();
Matrix3d Jpba23 = pKF3->mIMUPreInt.getJPBiasa();
// 谜之计算
Matrix3d lambda = 0.5 * (dt12 * dt12 * dt23 + dt12 * dt23 * dt23) * I3;
Vector3d phi = R2 * Jpba23 * baPrior * dt12 -
R1 * Jpba12 * baPrior * dt23 +
R1 * Jvba12 * baPrior * dt12 * dt23;
Vector3d gamma = p3 * dt12 + p1 * dt23 + R1 * dp12 * dt23 - p2 * (dt12 + dt23)
- R2 * dp23 * dt12 - R1 * dv12 * dt12 * dt23;
C.block<3, 3>(3 * i, 0) = lambda;
D.segment<3>(3 * i) = gamma - phi;
}
// Use svd to compute C*x=D, x=[ba] 6x1 vector
// Solve Ax = b where x is ba
JacobiSVD<MatrixXd> svd2(C, ComputeThinU | ComputeThinV);
VectorXd y = svd2.solve(D);
Vector3d gpre = y.head(3);
// normalize g
Vector3d g0 = gpre / gpre.norm() * setting::gravity;
// Step2.2
// estimate the bias from g
MatrixXd A(3 * (N - 2), 3);
A.setZero();
VectorXd B(3 * (N - 2));
B.setZero();
for (int i = 0; i < N - 2; i++) {
// 三个帧才能建立加速度约束
shared_ptr<Frame> pKF1 = vpKFs[i];
shared_ptr<Frame> pKF2 = vpKFs[i + 1];
shared_ptr<Frame> pKF3 = vpKFs[i + 2];
// Poses
Matrix3d R1 = pKF1->mRwb.matrix();
Matrix3d R2 = pKF2->mRwb.matrix();
Vector3d p1 = pKF1->mTwb;
Vector3d p2 = pKF2->mTwb;
Vector3d p3 = pKF3->mTwb;
// Delta time between frames
double dt12 = pKF2->mIMUPreInt.getDeltaTime();
double dt23 = pKF3->mIMUPreInt.getDeltaTime();
// Pre-integrated measurements
Vector3d dp12 = pKF2->mIMUPreInt.getDeltaP();
Vector3d dv12 = pKF2->mIMUPreInt.getDeltaV();
Vector3d dp23 = pKF3->mIMUPreInt.getDeltaP();
Matrix3d Jpba12 = pKF2->mIMUPreInt.getJPBiasa();
Matrix3d Jvba12 = pKF2->mIMUPreInt.getJVBiasa();
Matrix3d Jpba23 = pKF3->mIMUPreInt.getJPBiasa();
// 谜之计算
Vector3d lambda = 0.5 * (dt12 * dt12 * dt23 + dt12 * dt23 * dt23) * I3 * g0;
Matrix3d phi = R2 * Jpba23 * dt12 -
R1 * Jpba12 * dt23 +
R1 * Jvba12 * dt12 * dt23;
Vector3d gamma = p3 * dt12 + p1 * dt23 + R1 * dp12 * dt23 - p2 * (dt12 + dt23)
- R2 * dp23 * dt12 - R1 * dv12 * dt12 * dt23;
A.block<3, 3>(3 * i, 0) = phi;
B.segment<3>(3 * i) = gamma - lambda;
}
JacobiSVD<MatrixXd> svd(A, ComputeThinU | ComputeThinV);
VectorXd y2 = svd.solve(B);
Vector3d baest = y2;
// update ba and re-compute pre-integration
for (auto pkf : vpKFs) {
pkf->SetBiasA(baest);
}
for (int i = 1; i < N; i++) {
vpKFs[i]->ComputeIMUPreInt();
}
// Step3. velocity estimation
for (int i = 0; i < N; i++) {
auto pKF = vpKFs[i];
if (i != N - 1) {
// not last KeyFrame, R1*dp12 = p2 - p1 -v1*dt12 - 0.5*gw*dt12*dt12
// ==>> v1 = 1/dt12 * (p2 - p1 - 0.5*gw*dt12*dt12 - R1*dp12)
auto pKF2 = vpKFs[i + 1];
const Vector3d p2 = pKF2->mTwb;
const Vector3d p1 = pKF->mTwb;
const Matrix3d R1 = pKF->mRwb.matrix();
const double dt12 = pKF2->mIMUPreInt.getDeltaTime();
const Vector3d dp12 = pKF2->mIMUPreInt.getDeltaP();
Vector3d v1 = (p2 - p1 - 0.5 * g0 * dt12 * dt12 - R1 * dp12) / dt12;
pKF->SetSpeed(v1);
} else {
// last KeyFrame, R0*dv01 = v1 - v0 - gw*dt01 ==>> v1 = v0 + gw*dt01 + R0*dv01
auto pKF0 = vpKFs[i - 1];
const Matrix3d R0 = pKF0->mRwb.matrix();
const Vector3d v0 = pKF0->mSpeedAndBias.segment<3>(0);
const double dt01 = pKF->mIMUPreInt.getDeltaTime();
const Vector3d dv01 = pKF->mIMUPreInt.getDeltaV();
Vector3d v1 = v0 + g0 * dt01 + R0 * dv01;
pKF->SetSpeed(v1);
}
}
double gprenorm = gpre.norm();
// double baestdif = (baest0 - baest).norm();
LOG(INFO) << "Estimated gravity before: " << gpre.transpose() << ", |gw| = " << gprenorm << endl;
LOG(INFO) << "Estimated acc bias after: " << baest.transpose() << endl;
LOG(INFO) << "Estimated gyr bias: " << bgest.transpose() << endl;
bool initflag = false;
if (gprenorm > 9.7 && gprenorm < 9.9 &&
baest.norm() < 1) {
LOG(INFO) << "IMU init ok!" << endl;
initflag = true;
} else {
// goodcnt = 0;
}
// align 'world frame' to gravity vector, making mgWorld = [0,0,9.8]
if (initflag) {
mgWorld = g0;
}
return initflag;
}
*/

/*
bool GlobalOptimizationGraph::inputGPS(const sensor_msgs::NavSatFix& gps)
{
Expand Down
2 changes: 2 additions & 0 deletions algorithms/GlobalOptimizationGraph/GlobalOptimizationGraph.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,10 +69,12 @@ class GlobalOptimizationGraph
void addBlockFCAttitude();

void doOptimization();

void resetOptimizationGraph()
{
this->optimizer.clear();
}
bool SpeedInitialization();
bool estimateCurrentSpeed();

private:
Expand Down
54 changes: 33 additions & 21 deletions algorithms/scene_retrieving/src/multi_scene_retriever.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ int MultiSceneRetriever::retrieveSceneWithScaleFromMonoImage(const cv::Mat image
//step<1> select scene nearby.
if(img_lon_lat_valid == false)
{
cout<<"Error:Image Longitude,Latitude must be valid!"<<endl;
cout<<"Fatal Error:Image Longitude,Latitude must be valid!"<<endl;
return -1;
}
vector<int> scene_index_list;
Expand Down Expand Up @@ -66,8 +66,8 @@ int MultiSceneRetriever::retrieveSceneWithScaleFromMonoImage(const cv::Mat image
}

}
/*
virtual int MultiSceneRetriever::retrieveSceneFromStereoImage(const cv::Mat image_left_rect,

int MultiSceneRetriever::retrieveSceneFromStereoImage(const cv::Mat image_left_rect,
const cv::Mat image_right_rect,
const cv::Mat& Q_mat,
cv::Mat& RT_mat_of_stereo_cam_output,
Expand All @@ -81,39 +81,51 @@ virtual int MultiSceneRetriever::retrieveSceneFromStereoImage(const cv::Mat imag
cout<<"Fatal Error:no gps info in retrieveSceneFromStereoImage().Failed."<<endl;
return -1;
}

vector<int> scene_index_list;
scene_list = this->findNRelativeSceneByGPS(lon,lat,scene_index_list);
this->findNRelativeSceneByGPS(img_lon,img_lat,scene_index_list);

//step<2> do match.
vector<match_result> match_result_list;

for (const int& index:scene_index_list)
{
bool match_success;
//do matching.get multiple results.
this->idToNodeMap[index].pScene->retrieveSceneFromStereoImage(cv::imread(left_image_path[i]), cv::imread(right_image_path[i]), Q_mat, RT_mat, match_success);
int matched_points_count = this->idToNodeMap[index].pScene->retrieveSceneFromStereoImage(cv::imread(left_image_path[i]), cv::imread(right_image_path[i]), Q_mat, RT_mat, match_success);
if(match_success)
{
match_result_list.push_back(...);
match_result_list.push_back(std::make_pair(index,matched_points_count));
}
}
//step<3> select and reserve only the best match.
//step<3> select and reserve only the best match.
int best_match_id = -1;
int best_match_points_count = -1;
for(auto match:match_result_list)
{
if match....
....
int curr_index = std::get<0>(match);
int curr_count = std::get<1>(match);
if(curr_count>best_match_points_count)
{
best_match_points_count = curr_count;
best_match_id = curr_index;
}
}
if(best_match_id>=0 && best_match_points_count>5)
{
//redo mapping;for the last one is not always the best one.
this->idToNodeMap[best_match_id]->pSceneRetriever->retrieveSceneWithScaleFromMonoImage(image_in_rect, cameraMatrix, RT_mat_of_mono_cam_output, match_success);
}
if(match_success)
{
return best_match_id;
}
else
{
return -1;
}
}

}*/

/*
void MultiSceneRetriever::insertSceneIntoKDTree(double longi,double lati,shared_ptr<Scene> pScene)
{
shared_ptr<MultiSceneNode> pNew(new MultiSceneNode());
pNew->longitude = longi;
pNew->lati = lati;
pNew->pScene = pScene;
this->insertSceneIntoKDTree(pNew);
}*/
void MultiSceneRetriever::insertSceneIntoKDTree(shared_ptr<MultiSceneNode> nodeptr)
{
this->idToNodeMap[this->scene_index] = nodeptr;
Expand Down Expand Up @@ -163,4 +175,4 @@ void MultiSceneRetriever::findNRelativeSceneByGPS(double gps_longitude,double gp
output_scene_index.push_back(pointIdxRadiusSearch[i]);
}
}
}
}

0 comments on commit 376fdb8

Please sign in to comment.