Eigen::MatrixXf
powerspectrum::from_pcm(const Eigen::VectorXf& pcm_samples)
{
MINILOG(logTRACE) << "Powerspectrum computation. input samples="
<< pcm_samples.size();
// check if inputs are sane
if ((pcm_samples.size() < win_size) || (hop_size > win_size)) {
return Eigen::MatrixXf(0, 0);
}
size_t frames = (pcm_samples.size() - (win_size-hop_size)) / hop_size;
size_t freq_bins = win_size/2 + 1;
// initialize power spectrum
Eigen::MatrixXf ps(freq_bins, frames);
// peak normalization value
float pcm_scale = std::max(fabs(pcm_samples.minCoeff()),
fabs(pcm_samples.maxCoeff()));
// scale signal to 96db (16bit)
pcm_scale = std::pow(10.0f, 96.0f/20.0f) / pcm_scale;
// compute the power spectrum
for (size_t i = 0; i < frames; i++) {
// fill pcm
for (int j = 0; j < win_size; j++) {
kiss_pcm[j] = pcm_samples(i*hop_size+j) * pcm_scale * win_funct(j);
}
// fft
kiss_fftr(kiss_status, kiss_pcm, kiss_freq);
// save powerspectrum frame
Eigen::MatrixXf::ColXpr psc(ps.col(i));
for (int j = 0; j < win_size/2+1; j++) {
psc(j) =
std::pow(kiss_freq[j].r, 2) + std::pow(kiss_freq[j].i, 2);
}
}
MINILOG(logTRACE) << "Powerspectrum finished. size=" << ps.rows() << "x"
<< ps.cols();
return ps;
}
int main(int argc, char **argv) {
std::vector<color_keyframe::Ptr> frames;
util U;
U.load("http://localhost/corridor_map2", frames);
timestamp_t t0 = get_timestamp();
std::vector<std::pair<int, int> > overlapping_keyframes;
int size;
int workers = argc - 1;
ros::init(argc, argv, "panorama");
std::vector<
actionlib::SimpleActionClient<rm_multi_mapper::WorkerSlamAction>*> ac_list;
for (int i = 0; i < workers; i++) {
actionlib::SimpleActionClient<rm_multi_mapper::WorkerSlamAction>* ac =
new actionlib::SimpleActionClient<
rm_multi_mapper::WorkerSlamAction>(
std::string(argv[i + 1]), true);
ac_list.push_back(ac);
}
sql::ResultSet *res;
size = frames.size();
get_pairs(overlapping_keyframes);
std::vector<rm_multi_mapper::WorkerSlamGoal> goals;
int keyframes_size = (int) overlapping_keyframes.size();
for (int k = 0; k < workers; k++) {
rm_multi_mapper::WorkerSlamGoal goal;
int last_elem = (keyframes_size / workers) * (k + 1);
if (k == workers - 1)
last_elem = keyframes_size;
for (int i = (keyframes_size / workers) * k; i < last_elem; i++) {
rm_multi_mapper::KeyframePair keyframe;
keyframe.first = overlapping_keyframes[i].first;
keyframe.second = overlapping_keyframes[i].second;
goal.Overlap.push_back(keyframe);
}
goals.push_back(goal);
}
ROS_INFO("Waiting for action server to start.");
for (int i = 0; i < workers; i++) {
ac_list[i]->waitForServer();
}
ROS_INFO("Action server started, sending goal.");
// send a goal to the action
for (int i = 0; i < workers; i++) {
ac_list[i]->sendGoal(goals[i]);
}
//wait for the action to return
std::vector<bool> finished;
for (int i = 0; i < workers; i++) {
bool finished_before_timeout = ac_list[i]->waitForResult(
ros::Duration(30.0));
finished.push_back(finished_before_timeout);
}
bool success = true;
for (int i = 0; i < workers; i++) {
success = finished[i] && success;
}
Eigen::MatrixXf acc_JtJ;
acc_JtJ.setZero(size * 6, size * 6);
Eigen::VectorXf acc_Jte;
acc_Jte.setZero(size * 6);
if (success) {
for (int i = 0; i < workers; i++) {
Eigen::MatrixXf JtJ;
JtJ.setZero(size * 6, size * 6);
Eigen::VectorXf Jte;
Jte.setZero(size * 6);
rm_multi_mapper::Vector rosJte = ac_list[i]->getResult()->Jte;
rm_multi_mapper::Matrix rosJtJ = ac_list[i]->getResult()->JtJ;
vector2eigen(rosJte, Jte);
matrix2eigen(rosJtJ, JtJ);
acc_JtJ += JtJ;
acc_Jte += Jte;
}
} else {
ROS_INFO("Action did not finish before the time out.");
std::exit(0);
}
Eigen::VectorXf update = -acc_JtJ.ldlt().solve(acc_Jte);
float iteration_max_update = std::max(std::abs(update.maxCoeff()),
std::abs(update.minCoeff()));
ROS_INFO("Max update %f", iteration_max_update);
/*for (int i = 0; i < (int)frames.size(); i++) {
frames[i]->get_pos() = Sophus::SE3f::exp(update.segment<6>(i))
* frames[i]->get_pos();
std::string query = "UPDATE `positions` SET `q0` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[0]) +
", `q1` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[1]) +
", `q2` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[2]) +
", `q3` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[3]) +
", `t0` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().translation()[0]) +
", `t1` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().translation()[1]) +
", `t2` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().translation()[2]) +
", `int0` = " +
boost::lexical_cast<std::string>(frames[i]->get_intrinsics().array()[0]) +
", `int1` = " +
boost::lexical_cast<std::string>(frames[i]->get_intrinsics().array()[1]) +
", `int2` = " +
boost::lexical_cast<std::string>(frames[i]->get_intrinsics().array()[2]) +
" WHERE `id` = " +
boost::lexical_cast<std::string>(i) +
";";
res = U.sql_query(query);
delete res;
}*/
timestamp_t t1 = get_timestamp();
double secs = (t1 - t0) / 1000000.0L;
std::cout << secs << std::endl;
return 0;
}
