float mmf::OptSO3MMFvMF::conjugateGradientCUDA_impl(Matrix3f& R, float res0,
uint32_t n, uint32_t maxIter) {
float f = 0;
std::cout << this->cld_.xSums() << std::endl;
for (uint32_t k=0; k<K(); ++k) {
Eigen::Matrix3f N = Eigen::Matrix3f::Zero();
for (uint32_t j=k*6; j<6*(k+1); ++j) {
Eigen::Vector3f m = Eigen::Vector3f::Zero();
m((j%6)/2) = (j%6)%2==0?-1.:1.;
N += m*this->cld_.xSums().col(j).transpose();
}
Eigen::JacobiSVD<Eigen::Matrix3f> svd(N,Eigen::ComputeThinU|Eigen::ComputeThinV);
Rs_[k] = svd.matrixV()*svd.matrixU().transpose();
if (Rs_[k].determinant() < 0.) Rs_[k] *= -1.;
f += (N*R).trace();
std::cout << N << std::endl;
std::cout << Rs_[k] << std::endl;
}
Eigen::VectorXf mfCounts = Eigen::VectorXf::Zero(K());
for (uint32_t k=0; k<K()*6; ++k) {
mfCounts(k/6) += this->cld_.counts()(k);
}
cout<<"counts: "<<endl<<this->cld_.counts().transpose()<<endl;
std::cout << " mf counts: " << mfCounts.transpose() << std::endl;
uint32_t iMax = 0;
mfCounts.maxCoeff(&iMax);
R = Rs_[iMax];
// std::cout << "N" << std::endl << N << std::endl;
// std::cout << "R" << std::endl << R << std::endl;
return f;
}
///////////////////////
///// Inference /////
///////////////////////
void expAndNormalize ( Eigen::MatrixXf & out, const Eigen::MatrixXf & in ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<out.cols(); i++ ){
Eigen::VectorXf b = in.col(i);
b.array() -= b.maxCoeff();
b = b.array().exp();
out.col(i) = b / b.array().sum();
}
}
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;
}
int main(int argc, char *argv[])
{
// Load a mesh
igl::readOBJ(TUTORIAL_SHARED_PATH "/inspired_mesh.obj", V, F);
printf("--Initialization--\n");
V_border = igl::is_border_vertex(V,F);
igl::adjacency_list(F, VV);
igl::vertex_triangle_adjacency(V,F,VF,VFi);
igl::triangle_triangle_adjacency(F,TT,TTi);
igl::edge_topology(V,F,E,F2E,E2F);
// Generate "subdivided" mesh for visualization of curl terms
igl::false_barycentric_subdivision(V, F, Vbs, Fbs);
// Compute scale for visualizing fields
global_scale = .2*igl::avg_edge_length(V, F);
//Compute scale for visualizing texture
uv_scale = 0.6/igl::avg_edge_length(V, F);
// Compute face barycenters
igl::barycenter(V, F, B);
// Compute local basis for faces
igl::local_basis(V,F,B1,B2,B3);
//Generate random vectors for constraints
generate_constraints();
// Interpolate a 2-PolyVector field to be used as the original field
printf("--Initial solution--\n");
igl::n_polyvector(V, F, b, bc, two_pv_ori);
// Post process original field
// Compute curl_minimizing matchings and curl
Eigen::MatrixXi match_ab, match_ba; // matchings across interior edges
printf("--Matchings and curl--\n");
double avgCurl = igl::polyvector_field_matchings(two_pv_ori, V, F, true, match_ab, match_ba, curl_ori);
double maxCurl = curl_ori.maxCoeff();
printf("original curl -- max: %.5g, avg: %.5g\n", maxCurl, avgCurl);
printf("--Singularities--\n");
// Compute singularities
igl::polyvector_field_singularities_from_matchings(V, F, V_border, VF, TT, E2F, F2E, match_ab, match_ba, singularities_ori);
printf("original #singularities: %ld\n", singularities.rows());
printf("--Cuts--\n");
// Get mesh cuts based on singularities
igl::polyvector_field_cut_mesh_with_singularities(V, F, VF, VV, TT, TTi, singularities_ori, cuts_ori);
printf("--Combing--\n");
// Comb field
Eigen::MatrixXd combed;
igl::polyvector_field_comb_from_matchings_and_cuts(V, F, TT, E2F, F2E, two_pv_ori, match_ab, match_ba, cuts_ori, combed);
printf("--Cut mesh--\n");
// Reconstruct integrable vector fields from combed field
igl::cut_mesh(V, F, VF, VFi, TT, TTi, V_border, cuts_ori, Vcut_ori, Fcut_ori);
printf("--Poisson--\n");
double avgPoisson = igl::polyvector_field_poisson_reconstruction(Vcut_ori, Fcut_ori, combed, scalars_ori, two_pv_poisson_ori, poisson_error_ori);
double maxPoisson = poisson_error_ori.maxCoeff();
printf("poisson error -- max: %.5g, avg: %.5g\n", maxPoisson, avgPoisson);
// Set the curl-free 2-PolyVector to equal the original field
two_pv = two_pv_ori;
singularities = singularities_ori;
curl = curl_ori;
cuts = cuts_ori;
two_pv_poisson = two_pv_poisson_ori;
poisson_error = poisson_error_ori;
Vcut = Vcut_ori;
Fcut = Fcut_ori;
scalars = scalars_ori;
printf("--Integrable - Precomputation--\n");
// Precompute stuff for solver
igl::integrable_polyvector_fields_precompute(V, F, b, bc, blevel, two_pv_ori, ipfdata);
cerr<<"Done. Press keys 1-0 for various visualizations, 'A' to improve integrability." <<endl;
igl::viewer::Viewer viewer;
viewer.callback_key_down = &key_down;
viewer.core.show_lines = false;
key_down(viewer,'2',0);
// Replace the standard texture with an integer shift invariant texture
line_texture(texture_R, texture_G, texture_B);
viewer.launch();
return 0;
}
bool key_down(igl::viewer::Viewer& viewer, unsigned char key, int modifier)
{
if (key == '1')
{
display_mode = 1;
update_display(viewer);
}
if (key == '2')
{
display_mode = 2;
update_display(viewer);
}
if (key == '3')
{
display_mode = 3;
update_display(viewer);
}
if (key == '4')
{
display_mode = 4;
update_display(viewer);
}
if (key == '5')
{
display_mode = 5;
update_display(viewer);
}
if (key == '6')
{
display_mode = 6;
update_display(viewer);
}
if (key == '7')
{
display_mode = 7;
update_display(viewer);
}
if (key == '8')
{
display_mode = 8;
update_display(viewer);
}
if (key == '9')
{
display_mode = 9;
update_display(viewer);
}
if (key == '0')
{
display_mode = 0;
update_display(viewer);
}
if (key == 'A')
{
//do a batch of iterations
printf("--Improving Curl--\n");
for (int bi = 0; bi<5; ++bi)
{
printf("\n\n **** Batch %d ****\n", iter);
igl::integrable_polyvector_fields_solve(ipfdata, params, two_pv, iter ==0);
iter++;
params.wSmooth *= params.redFactor_wsmooth;
}
// Post process current field
// Compute curl_minimizing matchings and curl
printf("--Matchings and curl--\n");
Eigen::MatrixXi match_ab, match_ba; // matchings across interior edges
double avgCurl = igl::polyvector_field_matchings(two_pv, V, F, true, match_ab, match_ba, curl);
double maxCurl = curl.maxCoeff();
printf("curl -- max: %.5g, avg: %.5g\n", maxCurl, avgCurl);
// Compute singularities
printf("--Singularities--\n");
igl::polyvector_field_singularities_from_matchings(V, F, match_ab, match_ba, singularities);
printf("#singularities: %ld\n", singularities.rows());
// Get mesh cuts based on singularities
printf("--Cuts--\n");
igl::polyvector_field_cut_mesh_with_singularities(V, F, singularities, cuts);
// Comb field
printf("--Combing--\n");
Eigen::MatrixXd combed;
igl::polyvector_field_comb_from_matchings_and_cuts(V, F, two_pv, match_ab, match_ba, cuts, combed);
// Reconstruct integrable vector fields from combed field
printf("--Cut mesh--\n");
igl::cut_mesh(V, F, cuts, Vcut, Fcut);
printf("--Poisson--\n");
double avgPoisson = igl::polyvector_field_poisson_reconstruction(Vcut, Fcut, combed, scalars, two_pv_poisson, poisson_error);
double maxPoisson = poisson_error.maxCoeff();
printf("poisson error -- max: %.5g, avg: %.5g\n", maxPoisson, avgPoisson);
update_display(viewer);
}
return false;
}
