void MarginalizationInfo::marginalize() { int pos = 0; for (auto &it : parameter_block_idx) { it.second = pos; pos += localSize(parameter_block_size[it.first]); } m = pos; for (const auto &it : parameter_block_size) { if (parameter_block_idx.find(it.first) == parameter_block_idx.end()) { parameter_block_idx[it.first] = pos; pos += localSize(it.second); } } n = pos - m; //ROS_DEBUG("marginalization, pos: %d, m: %d, n: %d, size: %d", pos, m, n, (int)parameter_block_idx.size()); TicToc t_summing; Eigen::MatrixXd A(pos, pos); Eigen::VectorXd b(pos); A.setZero(); b.setZero(); /* for (auto it : factors) { for (int i = 0; i < static_cast<int>(it->parameter_blocks.size()); i++) { int idx_i = parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[i])]; int size_i = localSize(parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[i])]); Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i); for (int j = i; j < static_cast<int>(it->parameter_blocks.size()); j++) { int idx_j = parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[j])]; int size_j = localSize(parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[j])]); Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j); if (i == j) A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j; else { A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j; A.block(idx_j, idx_i, size_j, size_i) = A.block(idx_i, idx_j, size_i, size_j).transpose(); } } b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals; } } ROS_INFO("summing up costs %f ms", t_summing.toc()); */ //multi thread TicToc t_thread_summing; pthread_t tids[NUM_THREADS]; ThreadsStruct threadsstruct[NUM_THREADS]; int i = 0; for (auto it : factors) { threadsstruct[i].sub_factors.push_back(it); i++; i = i % NUM_THREADS; } for (int i = 0; i < NUM_THREADS; i++) { TicToc zero_matrix; threadsstruct[i].A = Eigen::MatrixXd::Zero(pos,pos); threadsstruct[i].b = Eigen::VectorXd::Zero(pos); threadsstruct[i].parameter_block_size = parameter_block_size; threadsstruct[i].parameter_block_idx = parameter_block_idx; int ret = pthread_create( &tids[i], NULL, ThreadsConstructA ,(void*)&(threadsstruct[i])); if (ret != 0) { ROS_WARN("pthread_create error"); ROS_BREAK(); } } for( int i = NUM_THREADS - 1; i >= 0; i--) { pthread_join( tids[i], NULL ); A += threadsstruct[i].A; b += threadsstruct[i].b; } //ROS_DEBUG("thread summing up costs %f ms", t_thread_summing.toc()); //ROS_INFO("A diff %f , b diff %f ", (A - tmp_A).sum(), (b - tmp_b).sum()); //TODO Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).transpose()); Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm); //ROS_ASSERT_MSG(saes.eigenvalues().minCoeff() >= -1e-4, "min eigenvalue %f", saes.eigenvalues().minCoeff()); Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() * saes.eigenvectors().transpose(); //printf("error1: %f\n", (Amm * Amm_inv - Eigen::MatrixXd::Identity(m, m)).sum()); Eigen::VectorXd bmm = b.segment(0, m); Eigen::MatrixXd Amr = A.block(0, m, m, n); Eigen::MatrixXd Arm = A.block(m, 0, n, m); Eigen::MatrixXd Arr = A.block(m, m, n, n); Eigen::VectorXd brr = b.segment(m, n); A = Arr - Arm * Amm_inv * Amr; b = brr - Arm * Amm_inv * bmm; Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(A); Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0)); Eigen::VectorXd S_inv = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0)); Eigen::VectorXd S_sqrt = S.cwiseSqrt(); Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt(); linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose(); linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b; //std::cout << A << std::endl // << std::endl; //std::cout << linearized_jacobians << std::endl; //printf("error2: %f %f\n", (linearized_jacobians.transpose() * linearized_jacobians - A).sum(), // (linearized_jacobians.transpose() * linearized_residuals - b).sum()); }