void RealtimeMF_openni::normals_cb(float* d_normals, uint8_t* haveData, uint32_t w, uint32_t h) { tLog_.tic(-1); // reset all timers int32_t nComp = 0; float* d_nComp = this->normalExtract->d_normalsComp(nComp); Matrix3f kRwBefore = kRw_; tLog_.toctic(0,1); optSO3_->updateExternalGpuNormals(d_nComp,nComp,3,0); double residual = optSO3_->conjugateGradientCUDA(kRw_,nCGIter_); double D_KL = optSO3_->D_KL_axisUnif(); tLog_.toctic(1,2); { boost::mutex::scoped_lock updateLock(this->updateModelMutex); this->normalsImg_ = this->normalExtract->normalsImg(); if(z_.rows() != w*h) z_.resize(w*h); this->normalExtract->uncompressCpu(optSO3_->z().data(), optSO3_->z().rows(), z_.data(), z_.rows()); mfAxes_ = MatrixXf::Zero(3,6); for(uint32_t k=0; k<6; ++k){ int j = k/2; // which of the rotation columns does this belong to float sign = (- float(k%2) +0.5f)*2.0f; // sign of the axis mfAxes_.col(k) = sign*kRw_.col(j); } D_KL_= D_KL; residual_ = residual; this->update_ = true; updateLock.unlock(); } tLog_.toc(2); // total time tLog_.logCycle(); cout<<"delta rotation kRw_ = \n"<<kRwBefore*kRw_.transpose()<<endl; cout<<"---------------------------------------------------------------------------"<<endl; tLog_.printStats(); cout<<" residual="<<residual_<<"\t D_KL="<<D_KL_<<endl; cout<<"---------------------------------------------------------------------------"<<endl; fout_<<D_KL_<<" "<<residual_<<endl; fout_.flush(); //// return kRw_; // { // boost::mutex::scoped_lock updateLock(this->updateModelMutex); //// pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr nDispPtr = //// normalExtract->normalsPc(); //// nDisp_ = pcl::PointCloud<pcl::PointXYZRGB>::Ptr( new //// pcl::PointCloud<pcl::PointXYZRGB>(*nDispPtr)); //// this->normalsImg_ = this->normalExtract->normalsImg(); // this->update_ = true; // } };
void RealtimeMF::compute_() { // get compressed normals tLog_.toctic(1,2); int32_t nComp = 0; float* d_nComp = normalExtract_->d_normalsComp(nComp); cout<<" -- compressed to "<<nComp<<" normals"<<endl; // normalsImg_ = normalExtract_->normalsImg(); // optimize normals tLog_.toc(2); // total time optSO3_->updateExternalGpuNormals(d_nComp,nComp,3,0); residual_ = optSO3_->conjugateGradientCUDA(cRmf_,nCGIter_); cRmfs_ = optSO3_->GetRs(); if (mode_.compare("mmfvmf") == 0 && cRmfs_.size() == 1) cRmf_ = cRmfs_[0]; std::cout << "have " << cRmfs_.size() << " MFs" << std::endl; D_KL_ = optSO3_->D_KL_axisUnif(); cout<<" -- optimized rotation D_KL to unif "<<D_KL_<<endl <<cRmf_<<endl; tLog_.setDt(3,optSO3_->dtPrep()); tLog_.setDt(4,optSO3_->dtCG()); tLog_.logCycle(); haveLabels_ = false; };