// param:
// rate, 裁剪部分面积占全图的比率, 0 < rate < 1
void ImageGenerator::CropImage(const double rate)
{
double roi_sidelen = sqrt(1.0 - rate);
double upleft = (1.0 - roi_sidelen) / 2.0;
cv::Mat image_roi = image_(cv::Rect(image_.cols * upleft, image_.rows * upleft, image_.cols * roi_sidelen, image_.rows * roi_sidelen));
image_roi.copyTo(new_image_);
}
/*
* Spatially decomposes the particle location information into discrete blocks, to prepare for sending
* to the work nodes
*
* arguments:
* x_coords_new: The x coordinates of the particles
* y_coords_new: The y coordinates of the particles
* z_coords_new: The z coordinates of the particles
* n: The number of particles
* indexes(OUT): The number of particles in each block
*/
void organize_data_initial(double* x_coords_new, double* y_coords_new, double* z_coords_new, int n, int indexes[XDiv][YDiv][ZDiv]){
int x, y, z, i;
Particle* p;
// Indexes keeps track of how many particles are currently in each block,
// That is, the next location to put a new particle in in each block
for (x = 0; x < XDiv; ++x) {
for (y = 0; y < YDiv; ++y) {
for (z = 0; z < ZDiv; ++z){
indexes[x][y][z] = 0;
}
}
}
//
for (i = 0; i < n; ++i){
image_(&x_coords_new[i], &y_coords_new[i], &z_coords_new[i]);
//Calculate which block each particle belongs in based on location
//Since the particles can be anywhere in space, but the blocks start
//with 0,0,0, the offsets move the particles, such that they all have
//postive coordinates and the box starts at 0,0,0
x = (x_coords_new[i] + offsetx)/WIDTH_X * XDiv;
y = (y_coords_new[i] + offsety)/WIDTH_Y * YDiv;
z = (z_coords_new[i] + offsetz)/WIDTH_Z * ZDiv;
p = &particles[x][y][z][indexes[x][y][z]];
p->pos[0] = x_coords_new[i];
p->pos[1] = y_coords_new[i];
p->pos[2] = z_coords_new[i];
p->oldPos[0] = x_coords_new[i];
p->oldPos[1] = y_coords_new[i];
p->oldPos[2] = z_coords_new[i];
indexes[x][y][z]++;
p->index = invIndex(i);
p->moved = 0;
}
// Mark that all the empty spots are not valid particles
for (x = 0; x < XDiv; ++x) {
for (y = 0; y < YDiv; ++y) {
for (z = 0; z < ZDiv; ++z){
for (i = indexes[x][y][z]; i < block_size; ++i) {
particles[x][y][z][i].index = -1;
}
}
}
}
}
void MockSbigDetector::Capture(unsigned short start_x,
unsigned short start_y,
unsigned short width,
unsigned short height,
unsigned long exposure_time,
Mat* frame) {
usleep(exposure_time * 1e4);
Mat roi;
Range xrange(start_x, start_x + width),
yrange(start_y, start_y + height);
image_(yrange, xrange).copyTo(*frame);
}
void ObjectAspect::fromFile(const std::string &filename) {
PointCloud::Ptr cloud = PointCloud::Ptr(new PointCloud);
read_binary_PCL(filename, cloud);
sensor_msgs::ImagePtr image_(new sensor_msgs::Image);
pcl::toROSMsg (*cloud, *image_);
cv_bridge::CvImagePtr img_ = cv_bridge::toCvCopy(*image_, "bgr8");
ROS_INFO("loading model aspect: %s",filename.c_str());
calculate(img_->image, cloud);
points = cloud;
// TODO: Load the view point to object base transform from the pointclouds viewpoint;
}
bool
SceneryMap::loadFile()
{
QImage image_(filename_);
if (image_.isNull())
{
qDebug("ERROR 1010081652! Image could not be loaded!");
qDebug(filename_.toUtf8());
loaded_ = false;
return false;
}
else
{
loaded_ = true;
imageWidth_ = image_.width();
imageHeight_ = image_.height();
return true;
}
}
/// callback for Kinect point clouds
void kinect_cb(const sensor_msgs::PointCloud2ConstPtr& pcl_msg) {
boost::mutex::scoped_lock lock(mutex);
if (models.empty()) {
static bool first_call = true;
if (first_call) {
ROS_WARN("no model files loaded!");
first_call = false;
}
return;
}
// convert to PCL and image
PointCloud cloud_;
pcl::fromROSMsg(*pcl_msg, cloud_);
sensor_msgs::ImagePtr image_(new sensor_msgs::Image);
pcl::toROSMsg (cloud_, *image_);
cv_bridge::CvImageConstPtr capture_ = cv_bridge::toCvShare(image_, "bgr8");
ROS_INFO("received point cloud");
// Generate an aspect on the scene
ObjectAspect scene;
scene.calculate(capture_->image,cloud_.makeShared());
Eigen::Matrix4f t;
PointCloud feature_cloud;
sensor_msgs::PointCloud2 feature_msg;
int i = 0;
re_kinect_object_detector::DetectionResult resultMsg;
for(std::map<std::string, RecognitionModel>::iterator it=models.begin(); it!=models.end(); it++, i++) {
RecognitionModel& model = it->second;
re_msgs::DetectedObject detectedObjMsg;
// Match the scene with the model.
if (model.matchAspects(scene, t)) {
// get transformation
ROS_INFO("displaying %d points in frame: %s",(int)scene.match->points->size(),pcl_msg->header.frame_id.c_str());
PointCloud cloudtransformed;
pcl::transformPointCloud(*scene.match->points, cloudtransformed, t);
if (i == 0)
feature_cloud = cloudtransformed;
else
feature_cloud += cloudtransformed;
for (size_t j=0 ; j< cloudtransformed.points.size(); j++ ){
PointType pt = cloudtransformed.points.at(j);
if (!isnan(pt.x)){
int y = pt.y*525.0 / pt.z + capture_->image.size().height / 2;
int x = pt.x*525.0 / pt.z + capture_->image.size().width / 2;
re_msgs::Pixel px;
px.x = x;
px.y = y;
detectedObjMsg.points2d.push_back(px);
geometry_msgs::Point threeD_pt;
threeD_pt.x = pt.x;
threeD_pt.y = pt.y;
threeD_pt.z = pt.z;
detectedObjMsg.points3d.push_back(threeD_pt);
}
}
tf::Transform object_tf = tfFromEigen(t);
tf_broadcaster.sendTransform(tf::StampedTransform(object_tf, pcl_msg->header.stamp, pcl_msg->header.frame_id, model.model_name));
resultMsg.ObjectNames.push_back(model.model_name);
resultMsg.Detections.push_back(detectedObjMsg);
}
cv_bridge::CvImage out_msg;
out_msg.header = pcl_msg->header;
out_msg.encoding = "bgr8";
out_msg.image = capture_->image;
out_msg.toImageMsg(resultMsg.Image);
detected_objects_pub.publish(resultMsg);
}
pcl::toROSMsg(feature_cloud,feature_msg);
feature_msg.header.frame_id = pcl_msg->header.frame_id;
feature_msg.header.stamp = pcl_msg->header.stamp;
features_pub.publish(feature_msg);
}
void ControlItem::on_select_image_button_clicked(){
QString fileName = QFileDialog::getOpenFileName();
if(fileName.isEmpty()){return;}
QImage image_(fileName);
rootItem_->setImage(std::move(image_));
}
int main( int argc, char **argv) {
char fname[256];
char nom[256];
struct image *nf;
Fort_int lfmt[9];
unsigned char *buf , *buf2;
int sb = 0, i, j,z;
struct pt_x* pt;
struct pt_x *res;
struct pt_x * tabPt_x;
char reponse;
float hr , hs;
hr = 20;
hs = 20;
/* Initialisation */
inr_init( argc, argv, version, usage, detail);
/* Lecture des options */
infileopt( fname);
igetopt1("-hs", "%f", &hs);
igetopt1("-hr", "%f", &hr);
igetopt1("-max","%d", &max);
igetopt1("-v","%d",&VOSIN);
igetopt1("-n","%d",&noyau);
igetopt1("-m","%d",&mode);
igetopt1("-d","%d",&debug);
outfileopt(nom);
/*affichage the options */
fprintf(stderr, "=============OPTIONS===========\n");
fprintf(stderr, "hr = %f hs = %f\n",hr,hs);
fprintf(stderr, "Voisnage = %d\n",VOSIN);
if(noyau == 1){
fprintf(stderr, "Noyau = gaussian\n");
}else{
fprintf(stderr, "Noyau = epankovic\n");
}
if(mode == 1){
fprintf(stderr, "Mode = segmentation\n");
}else{
fprintf(stderr, "Mode = debruit\n");
}
printf("Image src = %s\n",fname);
printf("Image target = %s\n",nom);
printf("Mode debug = %d\n",debug);
fprintf(stderr, "===============================\n");
fprintf(stderr, "would you like to continue the execution with current options ? (y,n)\n");
scanf("%c",&reponse);
if(reponse!='y'){
return 0;
}
fprintf(stderr, "Start of execution\n");
/* Ouverture et lecture des images */
nf = image_(fname, "e", "", lfmt);
/* verification du format */
if(TYPE == FIXE && BSIZE==1){
/* allocation memoire adequat */
buf = (unsigned char*)i_malloc( NDIMX * NDIMY* NDIMV *sizeof(unsigned char));
/* cree tableau de pt_x */
tabPt_x = malloc(NDIMX* NDIMY* NDIMV *sizeof(pt_x));
/* lecture image */
c_lect( nf, NDIMY, buf);
}else{
imerror( 6, "codage non conforme\n");
}
/* Remplir de Struct Special */
if(NDIMV == 1){
remplir_basic(buf,tabPt_x,NDIMX,NDIMY);
}
if(NDIMV == 3){
remplir_rgb(buf,tabPt_x,NDIMX,NDIMY);
}
/* Traitement */
/*debruit*/
buf2 = (unsigned char*)i_malloc( NDIMX * NDIMY*NDIMV*sizeof(unsigned char));
if(mode == 0){
if(NDIMV == 1){
debruit_basic(tabPt_x,buf2,hs,hr,1,NDIMX,NDIMY);
}else{
debruit_rgb(tabPt_x,buf2,hs,hr,1,NDIMX,NDIMY);
}
}else{
segmentation(tabPt_x , buf2 , hs , hr , 0.1 , NDIMX, NDIMY);
}
/*sauvgarde*/
printf("sortir \n");
nf = c_image(nom,"c","",lfmt);
c_ecr(nf,DIMY,buf2);
/* fermeture image */
fermnf_( &nf);
//free(&his);
i_Free((void*)&buf);
i_Free((void*)&buf2);
return 0;
}
cv::Mat OpenCVUtility::read(const QString & string_) {
QImage image_(string_) ;
return read( image_ );
}
static int image_(Main_scaleBilinear)(lua_State *L) {
THTensor *Tsrc = luaT_checkudata(L, 1, torch_Tensor);
THTensor *Tdst = luaT_checkudata(L, 2, torch_Tensor);
THTensor *Ttmp;
long dst_stride0, dst_stride1, dst_stride2, dst_width, dst_height;
long src_stride0, src_stride1, src_stride2, src_width, src_height;
long tmp_stride0, tmp_stride1, tmp_stride2, tmp_width, tmp_height;
long i, j, k;
image_(Main_op_validate)(L, Tsrc,Tdst);
int ndims;
if (Tdst->nDimension == 3) ndims = 3;
else ndims = 2;
Ttmp = THTensor_(newWithSize2d)(Tsrc->size[ndims-2], Tdst->size[ndims-1]);
dst_stride0= image_(Main_op_stride)(Tdst,0);
dst_stride1= image_(Main_op_stride)(Tdst,1);
dst_stride2= image_(Main_op_stride)(Tdst,2);
src_stride0= image_(Main_op_stride)(Tsrc,0);
src_stride1= image_(Main_op_stride)(Tsrc,1);
src_stride2= image_(Main_op_stride)(Tsrc,2);
tmp_stride0= image_(Main_op_stride)(Ttmp,0);
tmp_stride1= image_(Main_op_stride)(Ttmp,1);
tmp_stride2= image_(Main_op_stride)(Ttmp,2);
dst_width= Tdst->size[ndims-1];
dst_height= Tdst->size[ndims-2];
src_width= Tsrc->size[ndims-1];
src_height= Tsrc->size[ndims-2];
tmp_width= Ttmp->size[1];
tmp_height= Ttmp->size[0];
for(k=0;k<image_(Main_op_depth)(Tsrc);k++) {
/* compress/expand rows first */
for(j = 0; j < src_height; j++) {
image_(Main_scale_rowcol)(Tsrc,
Ttmp,
0*src_stride2+j*src_stride1+k*src_stride0,
0*tmp_stride2+j*tmp_stride1+k*tmp_stride0,
src_stride2,
tmp_stride2,
src_width,
tmp_width );
}
/* then columns */
for(i = 0; i < dst_width; i++) {
image_(Main_scale_rowcol)(Ttmp,
Tdst,
i*tmp_stride2+0*tmp_stride1+k*tmp_stride0,
i*dst_stride2+0*dst_stride1+k*dst_stride0,
tmp_stride1,
dst_stride1,
tmp_height,
dst_height );
}
}
THTensor_(free)(Ttmp);
return 0;
}
/*
* Organize the particle location information in every iteration past the initial
*
* arguments:
* x_coords_new: The x coordinates of the particles
* y_coords_new: The y coordinates of the particles
* z_coords_new: The z coordinates of the particles
* n: The number of particles
* indexes(OUT): The number of particles in each block
*/
void organize_data_new(double* x_coords_new, double* y_coords_new, double* z_coords_new, int n, int indexes[XDiv][YDiv][ZDiv]){
//Iterate through blocks, update each location. Move between blocks if necessary
int i, j, x, y, z;
int xnew, ynew, znew;
int index;
Particle* p;
// Mark all particles as not having moved to reset the previous iteration's information
for (x = 0; x < XDiv; ++x) {
for (y = 0; y < YDiv; ++y) {
for (z = 0; z < ZDiv; ++z){
for (i = 0; i < block_size; ++i) {
particles[x][y][z][i].moved = 0;
}
}
}
}
// Instead of placing particles in blocks, this goes through each particle already in a block
// updates its location information, and moves it between blocks if necessary
for (x = 0; x < XDiv; ++x) {
for (y = 0; y < YDiv; ++y) {
for (z = 0; z < ZDiv; ++z) {
for (i = 0; i < block_size; ++i) {
p = &particles[x][y][z][i];
if (p->index != -1 && p->moved != 1){
index = pIndex(p->index);
image_(&x_coords_new[index], &y_coords_new[index], &z_coords_new[index]);
xnew = (x_coords_new[index] + offsetx)/WIDTH_X * XDiv;
ynew = (y_coords_new[index] + offsety)/WIDTH_Y * YDiv;
znew = (z_coords_new[index] + offsetz)/WIDTH_Z * ZDiv;
if (x != xnew || y != ynew || z != znew){
//Find a new empty location for the particle
for (j = 0; j < block_size; ++j) {
if (particles[xnew][ynew][znew][j].index == -1) break;
}
particles[xnew][ynew][znew][j] = particles[x][y][z][i];
particles[xnew][ynew][znew][j].moved = 1;
particles[x][y][z][i].index = -1;
p = &particles[xnew][ynew][znew][j];
}
p->pos[0] = x_coords_new[index];
p->pos[1] = y_coords_new[index];
p->pos[2] = z_coords_new[index];
double xDif = p->pos[0] - p->oldPos[0]; //The difference between the current
double yDif = p->pos[1] - p->oldPos[1]; //position and the position the last
double zDif = p->pos[2] - p->oldPos[2]; //time that particle's list was rebuilt
if ((xDif*xDif) + (yDif* yDif) + (zDif * zDif) > MOVE_THRESHOLD_2) {
p->moved = 1;
}
if (p->moved == 1) {
p->oldPos[0] = x_coords_new[index];
p->oldPos[1] = y_coords_new[index];
p->oldPos[2] = z_coords_new[index];
}
}
}
}
}
}
}
