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;
};