bool CLinearRidgeRegression::train_machine(CFeatures* data)
{
REQUIRE(m_labels,"No labels set\n");
if (!data)
data=features;
REQUIRE(data,"No features provided and no featured previously set\n");
REQUIRE(m_labels->get_num_labels() == data->get_num_vectors(),
"Number of training vectors (%d) does not match number of labels (%d)\n",
m_labels->get_num_labels(), data->get_num_vectors());
REQUIRE(data->get_feature_class() == C_DENSE,
"Expected Dense Features (%d) but got (%d)\n",
C_DENSE, data->get_feature_class());
REQUIRE(data->get_feature_type() == F_DREAL,
"Expected Real Features (%d) but got (%d)\n",
F_DREAL, data->get_feature_type());
CDenseFeatures<float64_t>* feats=(CDenseFeatures<float64_t>*) data;
int32_t num_feat=feats->get_num_features();
int32_t num_vec=feats->get_num_vectors();
SGMatrix<float64_t> kernel_matrix(num_feat,num_feat);
SGMatrix<float64_t> feats_matrix(feats->get_feature_matrix());
SGVector<float64_t> y(num_feat);
SGVector<float64_t> tau_vector(num_feat);
tau_vector.zero();
tau_vector.add(m_tau);
Map<MatrixXd> eigen_kernel_matrix(kernel_matrix.matrix, num_feat,num_feat);
Map<MatrixXd> eigen_feats_matrix(feats_matrix.matrix, num_feat,num_vec);
Map<VectorXd> eigen_y(y.vector, num_feat);
Map<VectorXd> eigen_labels(((CRegressionLabels*)m_labels)->get_labels(),num_vec);
Map<VectorXd> eigen_tau(tau_vector.vector, num_feat);
eigen_kernel_matrix = eigen_feats_matrix*eigen_feats_matrix.transpose();
eigen_kernel_matrix.diagonal() += eigen_tau;
eigen_y = eigen_feats_matrix*eigen_labels ;
LLT<MatrixXd> llt;
llt.compute(eigen_kernel_matrix);
if(llt.info() != Eigen::Success)
{
SG_WARNING("Features covariance matrix was not positive definite\n");
return false;
}
eigen_y = llt.solve(eigen_y);
set_w(y);
return true;
}
bool CKernelRidgeRegression::solve_krr_system()
{
SGMatrix<float64_t> kernel_matrix(kernel->get_kernel_matrix());
int32_t n = kernel_matrix.num_rows;
SGVector<float64_t> y = ((CRegressionLabels*)m_labels)->get_labels();
for(index_t i=0; i<n; i++)
kernel_matrix(i,i) += m_tau;
Map<MatrixXd> eigen_kernel_matrix(kernel_matrix.matrix, n, n);
Map<VectorXd> eigen_alphas(m_alpha.vector, n);
Map<VectorXd> eigen_y(y.vector, n);
LLT<MatrixXd> llt;
llt.compute(eigen_kernel_matrix);
if (llt.info() != Eigen::Success)
{
SG_WARNING("Features covariance matrix was not positive definite\n");
return false;
}
eigen_alphas = llt.solve(eigen_y);
return true;
}
void SlamSystem::BA()
{
R.resize(numOfState);
T.resize(numOfState);
vel.resize(numOfState);
for (int i = 0; i < numOfState ; i++)
{
int k = head + i;
if (k >= slidingWindowSize){
k -= slidingWindowSize;
}
R[k] = slidingWindow[k]->R_bk_2_b0;
T[k] = slidingWindow[k]->T_bk_2_b0;
vel[k] = slidingWindow[k]->v_bk;
}
int sizeofH = 9 * numOfState;
MatrixXd HTH(sizeofH, sizeofH);
VectorXd HTb(sizeofH);
MatrixXd tmpHTH;
VectorXd tmpHTb;
MatrixXd H_k1_2_k(9, 18);
MatrixXd H_k1_2_k_T;
VectorXd residualIMU(9);
MatrixXd H_i_2_j(9, 18);
MatrixXd H_i_2_j_T;
VectorXd residualCamera(9);
MatrixXd tmpP_inv(9, 9);
for (int iterNum = 0; iterNum < maxIterationBA; iterNum++)
{
HTH.setZero();
HTb.setZero();
//1. prior constraints
int m_sz = margin.size;
VectorXd dx = VectorXd::Zero(STATE_SZ(numOfState - 1));
if (m_sz != (numOfState - 1)){
assert("prior matrix goes wrong!!!");
}
for (int i = numOfState - 2; i >= 0; i--)
{
int k = head + i;
if (k >= slidingWindowSize){
k -= slidingWindowSize;
}
//dp, dv, dq
dx.segment(STATE_SZ(i), 3) = T[k] - slidingWindow[k]->T_bk_2_b0;
dx.segment(STATE_SZ(i) + 3, 3) = vel[k] - slidingWindow[k]->v_bk;
dx.segment(STATE_SZ(i) + 6, 3) = Quaterniond(slidingWindow[k]->R_bk_2_b0.transpose() * R[k]).vec() * 2.0;
}
HTH.block(0, 0, STATE_SZ(m_sz), STATE_SZ(m_sz)) += margin.Ap.block(0, 0, STATE_SZ(m_sz), STATE_SZ(m_sz));
HTb.segment(0, STATE_SZ(m_sz)) -= margin.Ap.block(0, 0, STATE_SZ(m_sz), STATE_SZ(m_sz))*dx;
HTb.segment(0, STATE_SZ(m_sz)) -= margin.bp.segment(0, STATE_SZ(m_sz));
//2. imu constraints
for (int i = numOfState-2; i >= 0; i-- )
{
int k = head + i;
if (k >= slidingWindowSize){
k -= slidingWindowSize;
}
if ( slidingWindow[k]->imuLinkFlag == false){
continue;
}
int k1 = k + 1;
if (k1 >= slidingWindowSize){
k1 -= slidingWindowSize;
}
residualIMU = math.ResidualImu(T[k], vel[k], R[k],
T[k1], vel[k1], R[k1],
gravity_b0, slidingWindow[k]->timeIntegral,
slidingWindow[k]->alpha_c_k, slidingWindow[k]->beta_c_k, slidingWindow[k]->R_k1_k);
H_k1_2_k = math.JacobianImu(T[k], vel[k], R[k],
T[k1], vel[k1], R[k1],
gravity_b0, slidingWindow[k]->timeIntegral);
H_k1_2_k_T = H_k1_2_k.transpose();
H_k1_2_k_T *= slidingWindow[k]->P_k.inverse();
HTH.block(i * 9, i * 9, 18, 18) += H_k1_2_k_T * H_k1_2_k;
HTb.segment(i * 9, 18) -= H_k1_2_k_T * residualIMU;
}
//3. camera constraints
for (int i = 0; i < numOfState; i++)
{
int currentStateID = head + i;
if (currentStateID >= slidingWindowSize){
currentStateID -= slidingWindowSize;
}
if (slidingWindow[currentStateID]->keyFrameFlag == false){
continue;
}
list<int>::iterator iter = slidingWindow[currentStateID]->cameraLinkList.begin();
for (; iter != slidingWindow[currentStateID]->cameraLinkList.end(); iter++ )
{
int linkID = *iter;
H_i_2_j = math.JacobianDenseTracking(T[currentStateID], R[currentStateID], T[linkID], R[linkID] ) ;
residualCamera = math.ResidualDenseTracking(T[currentStateID], R[currentStateID], T[linkID], R[linkID],
slidingWindow[currentStateID]->cameraLink[linkID].T_bi_2_bj,
slidingWindow[currentStateID]->cameraLink[linkID].R_bi_2_bj ) ;
// residualCamera.segment(0, 3) = R[linkID].transpose()*(T[currentStateID] - T[linkID]) - slidingWindow[currentStateID]->cameraLink[linkID].T_bi_2_bj;
// residualCamera.segment(3, 3).setZero();
// residualCamera.segment(6, 3) = 2.0 * (Quaterniond(slidingWindow[currentStateID]->cameraLink[linkID].R_bi_2_bj.transpose()) * Quaterniond(R[linkID].transpose()*R[currentStateID])).vec();
tmpP_inv.setZero();
tmpP_inv.block(0, 0, 3, 3) = slidingWindow[currentStateID]->cameraLink[linkID].P_inv.block(0, 0, 3, 3) ;
tmpP_inv.block(0, 6, 3, 3) = slidingWindow[currentStateID]->cameraLink[linkID].P_inv.block(0, 3, 3, 3) ;
tmpP_inv.block(6, 0, 3, 3) = slidingWindow[currentStateID]->cameraLink[linkID].P_inv.block(3, 0, 3, 3) ;
tmpP_inv.block(6, 6, 3, 3) = slidingWindow[currentStateID]->cameraLink[linkID].P_inv.block(3, 3, 3, 3) ;
double r_v = residualCamera.segment(0, 3).norm() ;
if ( r_v > huber_r_v ){
tmpP_inv *= huber_r_v/r_v ;
}
double r_w = residualCamera.segment(6, 3).norm() ;
if ( r_w > huber_r_w ){
tmpP_inv *= huber_r_w/r_w ;
}
H_i_2_j_T = H_i_2_j.transpose();
H_i_2_j_T *= tmpP_inv;
tmpHTH = H_i_2_j_T * H_i_2_j;
tmpHTb = H_i_2_j_T * residualCamera;
int currentStateIDIndex = currentStateID - head;
if ( currentStateIDIndex < 0){
currentStateIDIndex += slidingWindowSize;
}
int linkIDIndex = linkID - head ;
if (linkIDIndex < 0){
linkIDIndex += slidingWindowSize;
}
HTH.block(currentStateIDIndex * 9, currentStateIDIndex * 9, 9, 9) += tmpHTH.block(0, 0, 9, 9);
HTH.block(currentStateIDIndex * 9, linkIDIndex * 9, 9, 9) += tmpHTH.block(0, 9, 9, 9);
HTH.block(linkIDIndex * 9, currentStateIDIndex * 9, 9, 9) += tmpHTH.block(9, 0, 9, 9);
HTH.block(linkIDIndex * 9, linkIDIndex * 9, 9, 9) += tmpHTH.block(9, 9, 9, 9);
HTb.segment(currentStateIDIndex * 9, 9) -= tmpHTb.segment(0, 9);
HTb.segment(linkIDIndex * 9, 9) -= tmpHTb.segment(9, 9);
}
}
// printf("[numList in BA]=%d\n", numList ) ;
//solve the BA
//cout << "HTH\n" << HTH << endl;
LLT<MatrixXd> lltOfHTH = HTH.llt();
ComputationInfo info = lltOfHTH.info();
if (info == Success)
{
VectorXd dx = lltOfHTH.solve(HTb);
// printf("[BA] %d %f\n",iterNum, dx.norm() ) ;
// cout << iterNum << endl ;
// cout << dx.transpose() << endl ;
//cout << "iteration " << iterNum << "\n" << dx << endl;
#ifdef DEBUG_INFO
geometry_msgs::Vector3 to_pub ;
to_pub.x = dx.norm() ;
printf("%d %f\n",iterNum, to_pub.x ) ;
pub_BA.publish( to_pub ) ;
#endif
VectorXd errorUpdate(9);
for (int i = 0; i < numOfState; i++)
{
int k = head + i;
if (k >= slidingWindowSize){
k -= slidingWindowSize;
}
errorUpdate = dx.segment(i * 9, 9);
T[k] += errorUpdate.segment(0, 3);
vel[k] += errorUpdate.segment(3, 3);
Quaterniond q(R[k]);
Quaterniond dq;
dq.x() = errorUpdate(6) * 0.5;
dq.y() = errorUpdate(7) * 0.5;
dq.z() = errorUpdate(8) * 0.5;
dq.w() = sqrt(1 - SQ(dq.x()) * SQ(dq.y()) * SQ(dq.z()));
R[k] = (q * dq).normalized().toRotationMatrix();
}
//cout << T[head].transpose() << endl;
}
else
{
ROS_WARN("LLT error!!!");
iterNum = maxIterationBA;
//cout << HTH << endl;
//FullPivLU<MatrixXd> luHTH(HTH);
//printf("rank = %d\n", luHTH.rank() ) ;
//HTH.rank() ;
}
}
// Include correction for information vector
int m_sz = margin.size;
VectorXd r0 = VectorXd::Zero(STATE_SZ(numOfState - 1));
for (int i = numOfState - 2; i >= 0; i--)
{
int k = head + i;
if (k >= slidingWindowSize){
k -= slidingWindowSize;
}
//dp, dv, dq
r0.segment(STATE_SZ(i), 3) = T[k] - slidingWindow[k]->T_bk_2_b0;
r0.segment(STATE_SZ(i) + 3, 3) = vel[k] - slidingWindow[k]->v_bk;
r0.segment(STATE_SZ(i) + 6, 3) = Quaterniond(slidingWindow[k]->R_bk_2_b0.transpose() * R[k]).vec() * 2.0;
}
margin.bp.segment(0, STATE_SZ(m_sz)) += margin.Ap.block(0, 0, STATE_SZ(m_sz), STATE_SZ(m_sz))*r0;
for (int i = 0; i < numOfState ; i++)
{
int k = head + i;
if (k >= slidingWindowSize){
k -= slidingWindowSize;
}
slidingWindow[k]->R_bk_2_b0 = R[k];
slidingWindow[k]->T_bk_2_b0 = T[k];
slidingWindow[k]->v_bk = vel[k];
}
}