void trackAndDrawObjects(cv::Mat& image, int frameNumber, std::vector<cv::LatentSvmDetector::ObjectDetection> detections, std::vector<kstate>& kstates, std::vector<bool>& active, std::vector<cv::Scalar> colors, const sensor_msgs::Image& image_source) { std::vector<kstate> tracked_detections; cv::TickMeter tm; tm.start(); //std::cout << endl << "START tracking..."; doTracking(detections, frameNumber, kstates, active, image, tracked_detections, colors); tm.stop(); //std::cout << "END Tracking time = " << tm.getTimeSec() << " sec" << endl; //ROS int num = tracked_detections.size(); std::vector<cv_tracker::image_rect_ranged> rect_ranged_array; std::vector<int> real_data(num,0); std::vector<int> obj_id(num, 0); std::vector<int> lifespan(num, 0); //ENDROS for (size_t i = 0; i < tracked_detections.size(); i++) { kstate od = tracked_detections[i]; cv_tracker::image_rect_ranged rect_ranged; //od.rect contains x,y, width, height rectangle(image, od.pos, od.color, 3); putText(image, SSTR(od.id), cv::Point(od.pos.x + 4, od.pos.y + 13), cv::FONT_HERSHEY_SIMPLEX, 0.55, od.color, 2); //ROS obj_id[i] = od.id; // ? rect_ranged.rect.x = od.pos.x; rect_ranged.rect.y = od.pos.y; rect_ranged.rect.width = od.pos.width; rect_ranged.rect.height = od.pos.height; rect_ranged.range = od.range; rect_ranged.min_height = od.min_height; rect_ranged.max_height = od.max_height; rect_ranged_array.push_back(rect_ranged); real_data[i] = od.real_data; lifespan[i] = od.lifespan; //ENDROS } //more ros cv_tracker::image_obj_tracked kf_objects_msg; kf_objects_msg.type = object_type; kf_objects_msg.total_num = num; copy(rect_ranged_array.begin(), rect_ranged_array.end(), back_inserter(kf_objects_msg.rect_ranged)); // copy vector copy(real_data.begin(), real_data.end(), back_inserter(kf_objects_msg.real_data)); // copy vector copy(obj_id.begin(), obj_id.end(), back_inserter(kf_objects_msg.obj_id)); // copy vector copy(lifespan.begin(), lifespan.end(), back_inserter(kf_objects_msg.lifespan)); // copy vector kf_objects_msg.header = image_source.header; image_objects.publish(kf_objects_msg); //cout << "."<< endl; }
void SingleParticle2dx::Methods::CUDAProjectionMethod::prepareForProjections(SingleParticle2dx::DataStructures::ParticleContainer& cont) { cudaSetDevice(getMyGPU()); cudaStreamCreate(&m_stream); cudaChannelFormatDesc channelDesc = cudaCreateChannelDesc(32, 0, 0, 0, cudaChannelFormatKindFloat); cudaExtent VS = make_cudaExtent(m_size, m_size, m_size); if( m_alloc_done == false ) { cudaMalloc3DArray(&m_cuArray, &channelDesc, VS); } SingleParticle2dx::real_array3d_type real_data( boost::extents[m_size][m_size][m_size] ); m_context->getRealSpaceData(real_data); unsigned int size = m_size*m_size*m_size*sizeof(float); if( m_alloc_done == false ) { res_data_h = (float*)malloc(m_size*m_size*sizeof(float)); cudaMalloc((void**)&res_data_d, m_size*m_size*sizeof(float)); m_alloc_done = true; } cudaMemcpy3DParms copyParams = {0}; copyParams.srcPtr = make_cudaPitchedPtr((void*)real_data.origin(), VS.width*sizeof(float), VS.width, VS.height); copyParams.dstArray = m_cuArray; copyParams.extent = VS; copyParams.kind = cudaMemcpyHostToDevice; // cudaMemcpy3D(©Params); cudaMemcpy3DAsync(©Params, m_stream); struct cudaResourceDesc resDesc; memset(&resDesc, 0, sizeof(resDesc)); resDesc.resType = cudaResourceTypeArray; resDesc.res.array.array = m_cuArray; struct cudaTextureDesc texDesc; memset(&texDesc, 0, sizeof(texDesc)); texDesc.addressMode[0] = cudaAddressModeClamp; texDesc.addressMode[1] = cudaAddressModeClamp; texDesc.addressMode[2] = cudaAddressModeClamp; texDesc.filterMode = cudaFilterModeLinear; texDesc.readMode = cudaReadModeElementType; texDesc.normalizedCoords = 0; if(m_alloc_done == true) { cudaDestroyTextureObject(m_texObj); } m_texObj = 0; cudaCreateTextureObject(&m_texObj, &resDesc, &texDesc, NULL); }
void image_callback(const sensor_msgs::Image& image_source) { cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(image_source, sensor_msgs::image_encodings::BGR8); cv::Mat image_track = cv_image->image; cv::LatentSvmDetector::ObjectDetection empty_detection(cv::Rect(0,0,0,0),0,0); unsigned int i; std::vector<bool> tracker_matched(obj_trackers_.size(), false); std::vector<bool> object_matched(obj_detections_.size(), false); //check object detections vs current trackers for (i =0; i< obj_detections_.size(); i++) { for (unsigned int j = 0; j < obj_trackers_.size(); j++) { if (tracker_matched[j] || object_matched[i]) continue; cv::LatentSvmDetector::ObjectDetection tmp_detection = obj_detections_[i]; int area = tmp_detection.rect.width * tmp_detection.rect.height; cv::Rect intersection = tmp_detection.rect & obj_trackers_[j]->GetTrackedObject().rect; if ( (intersection.width * intersection.height) > area*0.3 ) { obj_trackers_[j]->Track(image_track, obj_detections_[i], true); tracker_matched[j] = true; object_matched[i] = true; //std::cout << "matched " << i << " with " << j << std::endl; } } } //run the trackers not matched for(i = 0; i < obj_trackers_.size(); i++) { if(!tracker_matched[i]) { obj_trackers_[i]->Track(image_track, empty_detection, false); } } //create trackers for those objects not being tracked yet for(unsigned int i=0; i<obj_detections_.size(); i++) { if (!object_matched[i])//if object wasn't matched by overlapping area, create a new tracker { if (num_trackers_ >10) num_trackers_=0; LkTracker* new_tracker = new LkTracker(++num_trackers_, min_heights_[i], max_heights_[i], ranges_[i]); new_tracker->Track(image_track, obj_detections_[i], true); //std::cout << "added new tracker" << std::endl; obj_trackers_.push_back(new_tracker); } } ApplyNonMaximumSuppresion(obj_trackers_, 0.3); //remove those trackers with its lifespan <=0 std::vector<LkTracker*>::iterator it; for(it = obj_trackers_.begin(); it != obj_trackers_.end();) { if ( (*it)->GetRemainingLifespan()<=0 ) { it = obj_trackers_.erase(it); //std::cout << "deleted a tracker " << std::endl; } else it++; } //copy results to ros msg unsigned int num = obj_trackers_.size(); std::vector<cv_tracker_msgs::image_rect_ranged> rect_ranged_array;//tracked rectangles std::vector<int> real_data(num,0);//boolean array to show if data in rect_ranged comes from tracking or detection std::vector<unsigned int> obj_id(num, 0);//id number for each rect_range std::vector<unsigned int> lifespan(num, 0);//remaining lifespan of each rectranged for(i=0; i < num; i++) { cv_tracker_msgs::image_rect_ranged rect_ranged; LkTracker tracker_tmp = *obj_trackers_[i]; rect_ranged.rect.x = tracker_tmp.GetTrackedObject().rect.x; rect_ranged.rect.y = tracker_tmp.GetTrackedObject().rect.y; rect_ranged.rect.width = tracker_tmp.GetTrackedObject().rect.width; rect_ranged.rect.height = tracker_tmp.GetTrackedObject().rect.height; rect_ranged.rect.score = tracker_tmp.GetTrackedObject().score; rect_ranged.max_height = tracker_tmp.max_height_; rect_ranged.min_height = tracker_tmp.min_height_; rect_ranged.range = tracker_tmp.range_; rect_ranged_array.push_back(rect_ranged); lifespan[i] = tracker_tmp.GetRemainingLifespan(); obj_id[i] = tracker_tmp.object_id; if(lifespan[i]==tracker_tmp.DEFAULT_LIFESPAN_) real_data[i] = 1; cv::rectangle(image_track, tracker_tmp.GetTrackedObject().rect, cv::Scalar(0,255,0), 2); } //std::cout << "TRACKERS: " << obj_trackers_.size() << std::endl; obj_detections_.clear(); cv_tracker_msgs::image_obj_tracked ros_objects_msg; ros_objects_msg.type = tracked_type_; ros_objects_msg.total_num = num; copy(rect_ranged_array.begin(), rect_ranged_array.end(), back_inserter(ros_objects_msg.rect_ranged)); // copy vector copy(real_data.begin(), real_data.end(), back_inserter(ros_objects_msg.real_data)); // copy vector copy(obj_id.begin(), obj_id.end(), back_inserter(ros_objects_msg.obj_id)); // copy vector copy(lifespan.begin(), lifespan.end(), back_inserter(ros_objects_msg.lifespan)); // copy vector ros_objects_msg.header = image_source.header; publisher_tracked_objects_.publish(ros_objects_msg); //cv::imshow("KLT",image_track); //cv::waitKey(1); ready_ = false; }
void SingleParticle2dx::Methods::BasicCalcCCMethod::calculateCrossCorrelation( SingleParticle2dx::DataStructures::Particle& part, SingleParticle2dx::DataStructures::Projection2d& proj, real_array2d_type& res) { value_type nx = part.getSizeX(); value_type ny = part.getSizeY(); fft_array2d_type fft_data( boost::extents[nx][ny] ); real_array2d_type real_data( boost::extents[nx][ny] ); real_array2d_type real_data_shifted( boost::extents[nx][ny] ); fft_type part_image(0,0); fft_type ref_image(0,0); value_type r2; value_type w; SingleParticle2dx::ConfigContainer* config = SingleParticle2dx::ConfigContainer::Instance(); for (size_type i=0; i<part.getSizeX(); i++) { for (size_type j=0; j<part.getSizeY(); j++) { r2 = sqrt( (i-nx/2)*(i-nx/2) + (j-ny/2)*(j-ny/2) ); { if ( (r2<config->getMaxCCFreq()) && (r2>config->getMinCCFreq()) ) { w = 1; } else if ( r2<(config->getMaxCCFreq()+config->getCCFreqSigma()) ) { value_type dr = r2-config->getMaxCCFreq(); w = 1 - dr/config->getCCFreqSigma(); } else { w = 0; } part_image = part(i,j); ref_image = proj(i,j); fft_data[i][j].real(part_image.real() * ref_image.real() + part_image.imag() * ref_image.imag()); fft_data[i][j].imag(-part_image.real() * ref_image.imag() + part_image.imag() * ref_image.real()); fft_data[i][j] *= w; } } } SingleParticle2dx::Utilities::FFTCalculator::performBackwardFFT (&fft_data, &real_data); size_type half_window = floor(SingleParticle2dx::ConfigContainer::Instance()->getCrossCorrelationWindowSize()/2.); int ii,jj; for (int i=0; i<nx; i++) { for (int j=0; j<ny; j++) { ii = i - nx/2; jj = j - nx/2; if (ii<0) { ii += nx; } if (jj<0) { jj += ny; } if (ii >= nx) { ii -= nx; } if (jj >= ny) { jj -= ny; } real_data_shifted[i][j] = real_data[ii][jj] / nx; } } res = real_data_shifted[boost::indices[range(nx/2-half_window,nx/2+half_window+1)][range(ny/2-half_window,ny/2+half_window+1)]]; }
int main() { SingleParticle2dx::ConfigContainer* config = SingleParticle2dx::ConfigContainer::Instance(); int n = config->getParticleSize(); std::string cont_path = config->getContainerFilePath(); std::string cont_bin_folder = cont_path + "/ParticleContainers/cont_part2"; std::vector<std::string> folder_content; SingleParticle2dx::Utilities::UtilityFunctions::getContentOfFolder(cont_bin_folder, folder_content, ".bin"); SingleParticle2dx::DataStructures::ParticleContainer cont_ave; int counter = 0; int ceil_sqrt_np = ceil(sqrt(static_cast<float>(folder_content.size()))); SingleParticle2dx::real_array2d_type all_data( boost::extents[ceil_sqrt_np*n][ceil_sqrt_np*n] ); typedef SingleParticle2dx::real_array2d_type::index_range range; std::string cont_stat_file = cont_path + "/ContainerStatsTXT/cont_stat_analyze.txt"; if(boost::filesystem::exists(cont_stat_file)) { boost::filesystem::remove(cont_stat_file); } int np = 0; #pragma omp parallel for schedule(dynamic,1) reduction(+:np) for(int i=0; i< static_cast<int>(folder_content.size()); i++) { SingleParticle2dx::DataStructures::ParticleContainer cont; SingleParticle2dx::DataStructures::ParticleContainer::deserializeContainerFromDisk(folder_content[i], cont, true); np += cont.getNumberOfParticles(); SingleParticle2dx::DataStructures::ParticleContainer cont_ave_local; cont.generateAverageContainer(cont_ave_local, true, false); SingleParticle2dx::DataStructures::Particle part2add = cont_ave_local(0); SingleParticle2dx::Utilities::AverageWeighter ave_weighter(config->getAveWeightMode(), cont.getNumberOfParticles(), part2add.getNewOrientation().getTLTANG()); part2add.setWeight(ave_weighter.getWeight()); cont_ave.addParticle(part2add); #pragma omp critical(output_during_cont_analyze) { cont.writeStatToFile(cont_stat_file, true); int proc = static_cast<int>( 100*(counter+1.0)/folder_content.size()); SingleParticle2dx::Utilities::UtilityFunctions::setProgressBar( proc ); counter++; } SingleParticle2dx::real_array2d_type real_data( boost::extents[n][n] ); cont_ave_local(0).getRealSpaceData(&real_data); int index_x = i / ceil_sqrt_np; int index_y = i % ceil_sqrt_np; all_data[boost::indices[range(index_x*n,index_x*n+n)] [range(index_y*n,index_y*n+n)] ] = real_data; } cont_ave.setParticleNumbers(); SingleParticle2dx::DataStructures::Reconstruction3d rec3d_ave(n, n, n); rec3d_ave.generateInitialModel(cont_ave); rec3d_ave.applyMask(); rec3d_ave.applyLowPassFilter(); rec3d_ave.writeToFile(config->getContainerName() + "/Rec_3d/rec_initial.map" ); SingleParticle2dx::Utilities::UtilityFunctions::generateImageOutput(config->getContainerName() + "/Rec_3d/rec_initial.map", "MAP: Initial 3D reconstruction", config->getScriptName(), true, false); SingleParticle2dx::DataStructures::Orientation o_topview(0,0,0); SingleParticle2dx::DataStructures::Projection2d top_proj(n,n); SingleParticle2dx::DataStructures::ParticleContainer c_dummy; rec3d_ave.forceProjectionPreparation(c_dummy); rec3d_ave.calculateProjection(o_topview, top_proj); top_proj.writeToFile(config->getContainerName() + "/Div_output/topview_proj.mrc"); SingleParticle2dx::Utilities::UtilityFunctions::generateImageOutput(config->getContainerName() + "/Div_output/topview_proj.mrc", "MAP: TopView", config->getScriptName(), false, false); SingleParticle2dx::Utilities::MRCFileIO::writeToMrc(&all_data, config->getContainerName() + "/Div_output/particles.mrc"); SingleParticle2dx::Utilities::UtilityFunctions::generateImageOutput(config->getContainerName() + "/Div_output/particles.mrc", "MAP: Particles", config->getScriptName(), true, false); SingleParticle2dx::Utilities::UtilityFunctions::writeAveContainerStatToFile(cont_ave, cont_path + "/ContainerStatsTXT/picking_stat.txt"); SingleParticle2dx::Utilities::UtilityFunctions::generate2dxOutput("Number of particles: " + SingleParticle2dx::Utilities::StringFunctions::TtoString(np), 1); SingleParticle2dx::Utilities::UtilityFunctions::generateVariableOutput("Particle_Number", np, config->getScriptName(), false); SingleParticle2dx::Utilities::UtilityFunctions::generateVariableOutput("Images", folder_content.size(), config->getScriptName(), false); SingleParticle2dx::Utilities::UtilityFunctions::forceReload(); return 0; }