Vector3d Optimizer::OptimizeInitialGyroBias(const vector<cv::Mat>& vTwc, const vector<IMUPreintegrator>& vImuPreInt)
{int N = vTwc.size(); if(vTwc.size()!=vImuPreInt.size()) cerr<<"vTwc.size()!=vImuPreInt.size()"<<endl;Matrix4d Tbc = ConfigParam::GetEigTbc();Matrix3d Rcb = Tbc.topLeftCorner(3,3).transpose();// Setup optimizer
    g2o::SparseOptimizer optimizer;g2o::BlockSolverX::LinearSolverType * linearSolver;linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>();g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);//g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);// Add vertex of gyro bias, to optimizer graphg2o::VertexGyrBias * vBiasg = new g2o::VertexGyrBias();vBiasg->setEstimate(Eigen::Vector3d::Zero());vBiasg->setId(0);optimizer.addVertex(vBiasg);// Add unary edges for gyro bias vertex//for(std::vector<KeyFrame*>::const_iterator lit=vpKFs.begin(), lend=vpKFs.end(); lit!=lend; lit++)for(int i=0; i<N; i++){// Ignore the first KFif(i==0)continue;const cv::Mat& Twi = vTwc[i-1];    // pose of previous KFMatrix3d Rwci = Converter::toMatrix3d(Twi.rowRange(0,3).colRange(0,3));//Matrix3d Rwci = Twi.rotation_matrix();const cv::Mat& Twj = vTwc[i];        // pose of this KFMatrix3d Rwcj = Converter::toMatrix3d(Twj.rowRange(0,3).colRange(0,3));//Matrix3d Rwcj =Twj.rotation_matrix();const IMUPreintegrator& imupreint = vImuPreInt[i];g2o::EdgeGyrBias * eBiasg = new g2o::EdgeGyrBias();eBiasg->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));// measurement is not used in EdgeGyrBiaseBiasg->dRbij = imupreint.getDeltaR();eBiasg->J_dR_bg = imupreint.getJRBiasg();eBiasg->Rwbi = Rwci*Rcb;eBiasg->Rwbj = Rwcj*Rcb;//eBiasg->setInformation(Eigen::Matrix3d::Identity());eBiasg->setInformation(imupreint.getCovPVPhi().bottomRightCorner(3,3).inverse());optimizer.addEdge(eBiasg);}// It's actualy a linear estimator, so 1 iteration is enough.//optimizer.setVerbose(true);
    optimizer.initializeOptimization();optimizer.optimize(1);g2o::VertexGyrBias * vBgEst = static_cast<g2o::VertexGyrBias*>(optimizer.vertex(0));return vBgEst->estimate();
}

void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{Matrix3d A;Vector3d b;Vector3d delta_bg;A.setZero();b.setZero();map<double, ImageFrame>::iterator frame_i;map<double, ImageFrame>::iterator frame_j;for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++){frame_j = next(frame_i);MatrixXd tmp_A(3, 3);tmp_A.setZero();VectorXd tmp_b(3);tmp_b.setZero();Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();A += tmp_A.transpose() * tmp_A;b += tmp_A.transpose() * tmp_b;}delta_bg = A.ldlt().solve(b);ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());for (int i = 0; i <= WINDOW_SIZE; i++)Bgs[i] += delta_bg;for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++){frame_j = next(frame_i);frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);}
}

// Solve A*x=B for x=[s,gw] 4x1 vectorcv::Mat A = cv::Mat::zeros(3*(N-2),4,CV_32F);cv::Mat B = cv::Mat::zeros(3*(N-2),1,CV_32F);cv::Mat I3 = cv::Mat::eye(3,3,CV_32F);// Step 2.// Approx Scale and Gravity vector in 'world' frame (first KF's camera frame)for(int i=0; i<N-2; i++){//KeyFrameInit* pKF1 = vKFInit[i];//vScaleGravityKF[i];KeyFrameInit* pKF2 = vKFInit[i+1];KeyFrameInit* pKF3 = vKFInit[i+2];// Delta time between framesdouble dt12 = pKF2->mIMUPreInt.getDeltaTime();double dt23 = pKF3->mIMUPreInt.getDeltaTime();// Pre-integrated measurementscv::Mat dp12 = Converter::toCvMat(pKF2->mIMUPreInt.getDeltaP());cv::Mat dv12 = Converter::toCvMat(pKF2->mIMUPreInt.getDeltaV());cv::Mat dp23 = Converter::toCvMat(pKF3->mIMUPreInt.getDeltaP());// Pose of camera in world framecv::Mat Twc1 = vTwc[i].clone();//pKF1->GetPoseInverse();cv::Mat Twc2 = vTwc[i+1].clone();//pKF2->GetPoseInverse();cv::Mat Twc3 = vTwc[i+2].clone();//pKF3->GetPoseInverse();// Position of camera centercv::Mat pc1 = Twc1.rowRange(0,3).col(3);cv::Mat pc2 = Twc2.rowRange(0,3).col(3);cv::Mat pc3 = Twc3.rowRange(0,3).col(3);// Rotation of camera, Rwccv::Mat Rc1 = Twc1.rowRange(0,3).colRange(0,3);cv::Mat Rc2 = Twc2.rowRange(0,3).colRange(0,3);cv::Mat Rc3 = Twc3.rowRange(0,3).colRange(0,3);// Stack to A/B matrix// lambda*s + beta*g = gammacv::Mat lambda = (pc2-pc1)*dt23 + (pc2-pc3)*dt12;cv::Mat beta = 0.5*I3*(dt12*dt12*dt23 + dt12*dt23*dt23);cv::Mat gamma = (Rc3-Rc2)*pcb*dt12 + (Rc1-Rc2)*pcb*dt23 + Rc1*Rcb*dp12*dt23 - Rc2*Rcb*dp23*dt12 - Rc1*Rcb*dv12*dt12*dt23;lambda.copyTo(A.rowRange(3*i+0,3*i+3).col(0));beta.copyTo(A.rowRange(3*i+0,3*i+3).colRange(1,4));gamma.copyTo(B.rowRange(3*i+0,3*i+3));// Tested the formulation in paper, -gamma. Then the scale and gravity vector is -xx// Debug log//cout<<"iter "<<i<<endl;
    }// Use svd to compute A*x=B, x=[s,gw] 4x1 vector// A = u*w*vt, u*w*vt*x=B// Then x = vt'*winv*u'*B
    cv::Mat w,u,vt;// Note w is 4x1 vector by SVDecomp()// A is changed in SVDecomp() with cv::SVD::MODIFY_A for speed
    cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A);// Debug log//cout<<"u:"<<endl<<u<<endl;//cout<<"vt:"<<endl<<vt<<endl;//cout<<"w:"<<endl<<w<<endl;// Compute winvcv::Mat winv=cv::Mat::eye(4,4,CV_32F);for(int i=0;i<4;i++){if(fabs(w.at<float>(i))<1e-10){w.at<float>(i) += 1e-10;// Test logcerr<<"w(i) < 1e-10, w="<<endl<<w<<endl;}winv.at<float>(i,i) = 1./w.at<float>(i);}// Then x = vt'*winv*u'*Bcv::Mat x = vt.t()*winv*u.t()*B;// x=[s,gw] 4x1 vectordouble sstar = x.at<float>(0);    // scale should be positivecv::Mat gwstar = x.rowRange(1,4);   // gravity should be about ~9.8

bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{int all_frame_count = all_image_frame.size();int n_state = all_frame_count * 3 + 3 + 1;MatrixXd A{n_state, n_state};A.setZero();VectorXd b{n_state};b.setZero();map<double, ImageFrame>::iterator frame_i;map<double, ImageFrame>::iterator frame_j;int i = 0;for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++){frame_j = next(frame_i);MatrixXd tmp_A(6, 10);tmp_A.setZero();VectorXd tmp_b(6);tmp_b.setZero();double dt = frame_j->second.pre_integration->sum_dt;tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;     tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0];//cout << "delta_p   " << frame_j->second.pre_integration->delta_p.transpose() << endl;tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v;//cout << "delta_v   " << frame_j->second.pre_integration->delta_v.transpose() << endl;
Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();//cov.block<6, 6>(0, 0) = IMU_cov[i + 1];//MatrixXd cov_inv = cov.inverse();
        cov_inv.setIdentity();MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();b.segment<6>(i * 3) += r_b.head<6>();A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();b.tail<4>() += r_b.tail<4>();A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();}A = A * 1000.0;b = b * 1000.0;x = A.ldlt().solve(b);double s = x(n_state - 1) / 100.0;ROS_DEBUG("estimated scale: %f", s);g = x.segment<3>(n_state - 4);ROS_DEBUG_STREAM(" result g     " << g.norm() << " " << g.transpose());if(fabs(g.norm() - G.norm()) > 1.0 || s < 0){return false;}RefineGravity(all_image_frame, g, x);s = (x.tail<1>())(0) / 100.0;(x.tail<1>())(0) = s;ROS_DEBUG_STREAM(" refine     " << g.norm() << " " << g.transpose());if(s < 0.0 )return false;   elsereturn true;
}

void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{Vector3d g0 = g.normalized() * G.norm();Vector3d lx, ly;//VectorXd x;int all_frame_count = all_image_frame.size();int n_state = all_frame_count * 3 + 2 + 1;MatrixXd A{n_state, n_state};A.setZero();VectorXd b{n_state};b.setZero();map<double, ImageFrame>::iterator frame_i;map<double, ImageFrame>::iterator frame_j;for(int k = 0; k < 4; k++){MatrixXd lxly(3, 2);lxly = TangentBasis(g0);int i = 0;for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++){frame_j = next(frame_i);MatrixXd tmp_A(6, 9);tmp_A.setZero();VectorXd tmp_b(6);tmp_b.setZero();double dt = frame_j->second.pre_integration->sum_dt;tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();tmp_A.block<3, 2>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly;tmp_A.block<3, 1>(0, 8) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;     tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0] - frame_i->second.R.transpose() * dt * dt / 2 * g0;tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;tmp_A.block<3, 2>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly;tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0;Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();//cov.block<6, 6>(0, 0) = IMU_cov[i + 1];//MatrixXd cov_inv = cov.inverse();
            cov_inv.setIdentity();MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();b.segment<6>(i * 3) += r_b.head<6>();A.bottomRightCorner<3, 3>() += r_A.bottomRightCorner<3, 3>();b.tail<3>() += r_b.tail<3>();A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>();A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>();}A = A * 1000.0;b = b * 1000.0;x = A.ldlt().solve(b);VectorXd dg = x.segment<2>(n_state - 3);g0 = (g0 + lxly * dg).normalized() * G.norm();//double s = x(n_state - 1);
    }   g = g0;
}

// Step 3.// Use gravity magnitude 9.8 as constraint// gI = [0;0;1], the normalized gravity vector in an inertial frame, NED type with no orientation.cv::Mat gI = cv::Mat::zeros(3,1,CV_32F);gI.at<float>(2) = 1;// Normalized approx. gravity vecotr in world framecv::Mat gwn = gwstar/cv::norm(gwstar);// Debug log//cout<<"gw normalized: "<<gwn<<endl;// vhat = (gI x gw) / |gI x gw|cv::Mat gIxgwn = gI.cross(gwn);double normgIxgwn = cv::norm(gIxgwn);cv::Mat vhat = gIxgwn/normgIxgwn;double theta = std::atan2(normgIxgwn,gI.dot(gwn));// Debug log//cout<<"vhat: "<<vhat<<", theta: "<<theta*180.0/M_PI<<endl;
Eigen::Vector3d vhateig = Converter::toVector3d(vhat);Eigen::Matrix3d RWIeig = Sophus::SO3::exp(vhateig*theta).matrix();cv::Mat Rwi = Converter::toCvMat(RWIeig);cv::Mat GI = gI*ConfigParam::GetG();//9.8012;// Solve C*x=D for x=[s,dthetaxy,ba] (1+2+3)x1 vectorcv::Mat C = cv::Mat::zeros(3*(N-2),6,CV_32F);cv::Mat D = cv::Mat::zeros(3*(N-2),1,CV_32F);for(int i=0; i<N-2; i++){//KeyFrameInit* pKF1 = vKFInit[i];//vScaleGravityKF[i];KeyFrameInit* pKF2 = vKFInit[i+1];KeyFrameInit* pKF3 = vKFInit[i+2];// Delta time between framesdouble dt12 = pKF2->mIMUPreInt.getDeltaTime();double dt23 = pKF3->mIMUPreInt.getDeltaTime();// Pre-integrated measurementscv::Mat dp12 = Converter::toCvMat(pKF2->mIMUPreInt.getDeltaP());cv::Mat dv12 = Converter::toCvMat(pKF2->mIMUPreInt.getDeltaV());cv::Mat dp23 = Converter::toCvMat(pKF3->mIMUPreInt.getDeltaP());cv::Mat Jpba12 = Converter::toCvMat(pKF2->mIMUPreInt.getJPBiasa());cv::Mat Jvba12 = Converter::toCvMat(pKF2->mIMUPreInt.getJVBiasa());cv::Mat Jpba23 = Converter::toCvMat(pKF3->mIMUPreInt.getJPBiasa());// Pose of camera in world framecv::Mat Twc1 = vTwc[i].clone();//pKF1->GetPoseInverse();cv::Mat Twc2 = vTwc[i+1].clone();//pKF2->GetPoseInverse();cv::Mat Twc3 = vTwc[i+2].clone();//pKF3->GetPoseInverse();// Position of camera centercv::Mat pc1 = Twc1.rowRange(0,3).col(3);cv::Mat pc2 = Twc2.rowRange(0,3).col(3);cv::Mat pc3 = Twc3.rowRange(0,3).col(3);// Rotation of camera, Rwccv::Mat Rc1 = Twc1.rowRange(0,3).colRange(0,3);cv::Mat Rc2 = Twc2.rowRange(0,3).colRange(0,3);cv::Mat Rc3 = Twc3.rowRange(0,3).colRange(0,3);// Stack to C/D matrix// lambda*s + phi*dthetaxy + zeta*ba = psicv::Mat lambda = (pc2-pc1)*dt23 + (pc2-pc3)*dt12;cv::Mat phi = - 0.5*(dt12*dt12*dt23 + dt12*dt23*dt23)*Rwi*SkewSymmetricMatrix(GI);  // note: this has a '-', different to papercv::Mat zeta = Rc2*Rcb*Jpba23*dt12 + Rc1*Rcb*Jvba12*dt12*dt23 - Rc1*Rcb*Jpba12*dt23;cv::Mat psi = (Rc1-Rc2)*pcb*dt23 + Rc1*Rcb*dp12*dt23 - (Rc2-Rc3)*pcb*dt12- Rc2*Rcb*dp23*dt12 - Rc1*Rcb*dv12*dt23*dt12 - 0.5*Rwi*GI*(dt12*dt12*dt23 + dt12*dt23*dt23); // note:  - paperlambda.copyTo(C.rowRange(3*i+0,3*i+3).col(0));phi.colRange(0,2).copyTo(C.rowRange(3*i+0,3*i+3).colRange(1,3)); //only the first 2 columns, third term in dtheta is zero, here compute dthetaxy 2x1.zeta.copyTo(C.rowRange(3*i+0,3*i+3).colRange(3,6));psi.copyTo(D.rowRange(3*i+0,3*i+3));// Debug log//cout<<"iter "<<i<<endl;
    }// Use svd to compute C*x=D, x=[s,dthetaxy,ba] 6x1 vector// C = u*w*vt, u*w*vt*x=D// Then x = vt'*winv*u'*D
    cv::Mat w2,u2,vt2;// Note w2 is 6x1 vector by SVDecomp()// C is changed in SVDecomp() with cv::SVD::MODIFY_A for speed
    cv::SVDecomp(C,w2,u2,vt2,cv::SVD::MODIFY_A);// Debug log//cout<<"u2:"<<endl<<u2<<endl;//cout<<"vt2:"<<endl<<vt2<<endl;//cout<<"w2:"<<endl<<w2<<endl;// Compute winvcv::Mat w2inv=cv::Mat::eye(6,6,CV_32F);for(int i=0;i<6;i++){if(fabs(w2.at<float>(i))<1e-10){w2.at<float>(i) += 1e-10;// Test logcerr<<"w2(i) < 1e-10, w="<<endl<<w2<<endl;}w2inv.at<float>(i,i) = 1./w2.at<float>(i);}// Then y = vt'*winv*u'*Dcv::Mat y = vt2.t()*w2inv*u2.t()*D;double s_ = y.at<float>(0);cv::Mat dthetaxy = y.rowRange(1,3);cv::Mat dbiasa_ = y.rowRange(3,6);Vector3d dbiasa_eig = Converter::toVector3d(dbiasa_);// dtheta = [dx;dy;0]cv::Mat dtheta = cv::Mat::zeros(3,1,CV_32F);dthetaxy.copyTo(dtheta.rowRange(0,2));Eigen::Vector3d dthetaeig = Converter::toVector3d(dtheta);// Rwi_ = Rwi*exp(dtheta)Eigen::Matrix3d Rwieig_ = RWIeig*Sophus::SO3::exp(dthetaeig).matrix();cv::Mat Rwi_ = Converter::toCvMat(Rwieig_);