rs2::frame pointcloud::process_depth_frame(const rs2::frame_source& source, const rs2::depth_frame& depth)
{
auto res = allocate_points(source, depth);
auto pframe = (librealsense::points*)(res.get());
auto depth_data = (const uint16_t*)depth.get_data();
float2* tex_ptr = pframe->get_texture_coordinates();
const float3* points;
points = depth_to_points(res, (uint8_t*)pframe->get_vertices(), *_depth_intrinsics, depth_data, *_depth_units);
auto vid_frame = depth.as<rs2::video_frame>();
// Pixels calculated in the mapped texture. Used in post-processing filters
float2* pixels_ptr = _pixels_map.data();
rs2_intrinsics mapped_intr;
rs2_extrinsics extr;
bool map_texture = false;
{
if (_extrinsics && _other_intrinsics)
{
mapped_intr = *_other_intrinsics;
extr = *_extrinsics;
map_texture = true;
}
}
if (map_texture)
{
auto height = vid_frame.get_height();
auto width = vid_frame.get_width();
get_texture_map(res, points, width, height, mapped_intr, extr, tex_ptr, pixels_ptr);
if (_occlusion_filter->active())
{
_occlusion_filter->process(pframe->get_vertices(), pframe->get_texture_coordinates(), _pixels_map);
}
}
return res;
}
IMPSTATISTICS_BEGIN_INTERNAL_NAMESPACE
KMData::KMData(int d, int n) : dim_(d) { points_ = allocate_points(n, dim_); }
IMPSTATISTICS_BEGIN_INTERNAL_NAMESPACE
KMCenters::KMCenters(int k, KMData *p)
: data_points_(p) {
centers_ = allocate_points(k,data_points_->get_dim());
}
void KMLProxy::run(Particles *initial_centers) {
IMP_INTERNAL_CHECK(is_init_,"The proxy was not initialized");
IMP_LOG(VERBOSE,"KMLProxy::run start \n");
//use the initial centers if provided
KMPointArray *kmc=nullptr;
if (initial_centers != nullptr) {
IMP_INTERNAL_CHECK(kcenters_ == initial_centers->size(),
"the number of initial points differs from the number of required"
<<" centers\n");
IMP_LOG(VERBOSE,"KMLProxy::run initial centers provided : \n");
kmc = allocate_points(kcenters_,atts_.size());
for (unsigned int i=0;i<kcenters_;i++){
Particle *cen=(*initial_centers)[i];
for(unsigned int j=0;j<atts_.size();j++) {
(*(*kmc)[i])[j]=cen->get_value(atts_[j]);
}
}
}
IMP_LOG(VERBOSE,"KMLProxy::run load initial guess \n");
//load the initail guess
KMFilterCenters ctrs(kcenters_, data_, kmc,damp_factor_);
//apply lloyd search
IMP_LOG(VERBOSE,"KMLProxy::run load lloyd \n");
lloyd_alg_ = new KMLocalSearchLloyd(&ctrs,&term_);
log_header();
IMP_CHECK_CODE(clock_t start = clock());
IMP_LOG(VERBOSE,"KMLProxy::run excute lloyd \n");
lloyd_alg_->execute();
IMP_LOG(VERBOSE,"KMLProxy::run analyse \n");
KMFilterCentersResults best_clusters = lloyd_alg_->get_best();
IMP_CHECK_CODE(Float exec_time = elapsed_time(start));
// print summary
IMP_LOG_WRITE(TERSE,log_summary(&best_clusters,exec_time));
IMP_LOG_WRITE(TERSE,best_clusters.show(IMP_STREAM));
IMP_INTERNAL_CHECK(kcenters_
== (unsigned int) best_clusters.get_number_of_centers(),
"The final number of centers does not match the requested one");
IMP_INTERNAL_CHECK (dim_ == (unsigned int) best_clusters.get_dim(),
"The dimension of the final clusters is wrong");
//TODO clear the centroids list
//set the centroids:
Particle *p;
IMP_LOG(VERBOSE,"KMLProxy::run load best results \n");
for (unsigned int ctr_ind = 0; ctr_ind < kcenters_; ctr_ind++) {
KMPoint *kmp = best_clusters[ctr_ind];
//create a new particle
p = new Particle(m_);
centroids_.push_back(p);
for (unsigned int att_ind = 0; att_ind < dim_; att_ind++) {
p->add_attribute(atts_[att_ind],(*kmp)[att_ind],false);
}
}
//set the assignment of particles to centers
//array of number of all points
//TODO - return this
IMP_LOG(VERBOSE,"KMLProxy::run get assignments \n");
const Ints *close_center = best_clusters.get_assignments();
IMP_LOG(VERBOSE,"KMLProxy::run get assignments 2\n");
for (int i=0;i<data_->get_number_of_points();i++) {
//std::cout<<"ps number i: " << i << " close center : "
//<< (*close_center)[i] << std::endl;
assignment_[ps_[i]]=(*close_center)[i];
}
}