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