//グリッド化
void make_elevation(object_map& m)
{
std::cout << "make_elevation" << std::endl;
double max_x = -10.0; double max_y = -10.0; double max_z = -10.0;
double min_x = 10.0; double min_y = 10.0; double min_z = 10.0;
int int_max_x = -100; int int_min_x = 100; int int_max_y = -100; int int_min_y = 100;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
*tmp_cloud = m.cloud_rgb;
for(int i=0;i<tmp_cloud->size();i++){
if(tmp_cloud->points[i].x > max_x) max_x = tmp_cloud->points[i].x;
if(tmp_cloud->points[i].x < min_x) min_x = tmp_cloud->points[i].x;
tmp_cloud->points[i].x = (int)(tmp_cloud->points[i].x*100);
if(tmp_cloud->points[i].y > max_y) max_y = tmp_cloud->points[i].y;
if(tmp_cloud->points[i].y < min_y) min_y = tmp_cloud->points[i].y;
tmp_cloud->points[i].y = (int)(tmp_cloud->points[i].y*100);
if(tmp_cloud->points[i].z > max_z) max_z = tmp_cloud->points[i].z;
if(tmp_cloud->points[i].z < min_z) min_z = tmp_cloud->points[i].z;
tmp_cloud->points[i].z = (int)(tmp_cloud->points[i].z*100);
}
m.max_x = max_x; m.min_x = min_x; m.max_y = max_y; m.min_y = min_y; m.max_z = max_z; m.min_z = min_z;
int_min_x = 100*min_x; int_min_y = 100*min_y;
int_max_x = 100*max_x; int_max_y = 100*max_y;
downsampling(*tmp_cloud, 1.0f);
m.size_x = int_max_x - int_min_x + 1;
m.size_y = int_max_y - int_min_y + 1;
// std::cout << m.min_x << " " << m.max_x << std::endl;
// std::cout << m.min_y << " " << m.max_y << std::endl;
// std::cout << m.size_x << " " << m.size_y << std::endl;
for(int i=0;i<MAP_X;i++){
for(int j=0;j<MAP_Y;j++){
m.map[i][j] = 0.0;
}
}
for(int i=0;i<tmp_cloud->size();i++){
if(tmp_cloud->points[i].z > m.map[(int)tmp_cloud->points[i].x-int_min_x][(int)tmp_cloud->points[i].y-int_min_y])
m.map[(int)tmp_cloud->points[i].x-int_min_x][(int)tmp_cloud->points[i].y-int_min_y] = tmp_cloud->points[i].z;
}
return;
}
void cb_rgb(const pcl::PCLPointCloud2ConstPtr& input)
{
static int cnt = 0;
// input
pCloud_input.reset(new Cloud);
pCloud.reset(new Cloud);
pcl::fromPCLPointCloud2(*input, *pCloud_input);
if(isdebug) cout<<"I heard RGB, # of points: "<<pCloud_input->points.size()<<endl;
// downsampling
lastT = pcl::getTime ();
downsampling(cont_sampling);
if(isdebug) cout<<"downsampling computation time : "<<pcl::getTime()-lastT<<endl;
if(isdebug) cout<<"downsampling end, # of points: "<<pCloud_input->points.size()<<endl;
// transform to the given frame
lastT = pcl::getTime ();
transform(param_frame_id);
if(isdebug) cout<<"transform computation time : "<<pcl::getTime()-lastT<<endl;
// workspace
lastT = pcl::getTime ();
workingspace();
if(isdebug) cout<<"workingspace computation time : "<<pcl::getTime()-lastT<<endl;
if(isdebug) cout<<"workingspace end, # of points: "<<pCloud_input->points.size()<<endl;
// // planeExtraction
// lastT = pcl::getTime ();
// planeExtraction(1);
// if(isdebug) cout<<"planeExtraction computation time : "<<pcl::getTime()-lastT<<endl;
// if(isdebug) cout<<"planeExtraction end, # of points: "<<pCloud_input->points.size()<<endl;
// tracking
lastT = pcl::getTime ();
pctracking->run(pCloud_input);
nowT = pcl::getTime ();
if(isdebug) cout<<"total tracking computation time : "<<nowT-lastT<<endl;
// publish filtered pointcloud
pcl::PCLPointCloud2 output_filtered;
pcl::toPCLPointCloud2(*pctracking->getFilteredPC(), output_filtered);
output_filtered.header.frame_id=param_frame_id.data();
pub_filtered.publish (output_filtered);
// publish segmented pointcloud
pcl::PCLPointCloud2 output_segmented;
pcl::toPCLPointCloud2(*pctracking->getSegmentedPC(), output_segmented);
output_segmented.header.frame_id=param_frame_id.data();
pub_segmented.publish (output_segmented);
// publish model pointcloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pCloudOut_model;
pCloudOut_model.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
pctracking->object_tracks->toPointCloudXYZI_model(*pCloudOut_model);
pcl::PCLPointCloud2 output_model;
pcl::toPCLPointCloud2(*pCloudOut_model, output_model);
output_model.header.frame_id=param_frame_id.data();
pub_objectmodel.publish (output_model);
// output tracks pointcloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pCloudOut;
pCloudOut.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
pctracking->object_tracks->toPointCloudXYZI(*pCloudOut);
// ROS_INFO("# of track points: %d", pCloudOut->points.size());
// publish pointcloud(currentT) of tracks
pcl::PCLPointCloud2 output;
pcl::toPCLPointCloud2(*pCloudOut, output);
output.header.frame_id=param_frame_id.data();
pub_track.publish (output);
// // publish lastmodel
// pcl::PCLPointCloud2 output_lastmodel;
// pcl::toPCLPointCloud2(*pctracking->getLastModel(), output_lastmodel);
// output_lastmodel.header.frame_id=param_frame_id.data();
// pub_lastmodel.publish (output_lastmodel);
// // publish particles
// pcl::PCLPointCloud2 output_particles;
// pcl::toPCLPointCloud2(*pctracking->getParticles(), output_particles);
// output_particles.header.frame_id=param_frame_id.data();
// pub_particles.publish (output_particles);
// publish gaussians
pub_gaussians.publish(pctracking->object_tracks->oldGaussians());
pub_gaussians.publish(pctracking->object_tracks->toMarkerGaussians());
// publish gmms
// pub_gmms.publish(pctracking->object_tracks->toMarkerGMMs());
// pub_edges.publish(pctracking->object_tracks->toMarkerEdges());
// publish delete id marker of deleted tracks
pub_trackID.publish(pctracking->object_tracks->oldMarkerIDs());
pub_trackID.publish(pctracking->object_tracks->toMarkerIDs());
pCloudOut.reset();
cnt++;
}
/**
* @function main
*/
int main( int argc, char* argv[] ) {
// Initialize, in case user does not enter values of e1 and e2
double a, b, c, e1, e2, px,py,pz,ra,pa,ya;
a = 0.1; b = 0.1; c = 0.1; e1= 1.0; e2 = 1.0; px = 0; py = 0; pz = 0;
ra = 0; pa = 0; ya = 0;
gFilename = std::string("omp_result.txt");
int v;
while( (v=getopt(argc, argv, "a:b:c:e:f:x:y:z:r:p:q:n:h")) != -1 ) {
switch(v) {
case 'h': {
printf("Executable to evaluate downsampling effect in one single sample \n");
printf("Usage: ./Executable -a -b -c -e -f -x -y -z -r -p -q -n output_filename.txt \n");
return 0;
} break;
case 'a' : {
a = atof(optarg);
} break;
case 'b' : {
b = atof(optarg);
} break;
case 'c' : {
c = atof(optarg);
} break;
case 'x' : {
px = atof(optarg);
} break;
case 'y' : {
py = atof(optarg);
} break;
case 'z' : {
pz = atof(optarg);
} break;
case 'r' : {
ra = atof(optarg);
} break;
case 'p' : {
pa = atof(optarg);
} break;
case 'q' : {
ya = atof(optarg);
} break;
case 'e' : {
e1 = atof(optarg);
} break;
case 'f' : {
e2 = atof(optarg);
} break;
case 'n' : {
gFilename.assign( optarg );
} break;
} // switch end
}
struct timespec start, finish;
double dt;
clock_gettime(CLOCK_MONOTONIC, &start);
std::ofstream output( gFilename.c_str(), std::ofstream::out );
SQ_parameters par, par_res;
evaluated_sqs es;
gD = 0;
// Create perfect pointcloud
pcl::PointCloud<PointT>::Ptr input( new pcl::PointCloud<PointT>() );
pcl::PointCloud<PointT>::Ptr down( new pcl::PointCloud<PointT>() );
// Dimensions
par.dim[0] = a; par.dim[1] = b; par.dim[2] = c;
// Translation
par.trans[0] = px; par.trans[1] = py; par.trans[2] = pz;
// Rotation
par.rot[0] = ra; par.rot[1] = pa; par.rot[2] = ya;
// E parameters
par.e[0] = e1; par.e[1] = e2;
// Store original data
output_sq base;
base.par = par;
base.er_r = 0; base.er_g = 0; base.t = 0; base.er_v = 0;
base.er_e1 = 0; base.er_e2 = 0;
saveParams( output, base.par, 0, 0, 0, 0, 0, 0 );
// 1. Generate clean pointcloud
input = sampleSQ_uniform<PointT>( par );
// Loop
double xer_e1, xer_e2, vc, vr, xer_v;
struct timespec ts, tf;
double elapsed;
double er_g, er_r, er_d;
for( int i = 1; i < gnD; ++i ) {
gD = 0 + gdD*i;
if( gD == 0 ) { down = input; }
else { down = downsampling( input, gD ); }
// Try it out with methods
clock_gettime(CLOCK_MONOTONIC, &ts);
es.minimize( down, par_res, er_g, er_r, er_d, SQ_FX_RADIAL );
clock_gettime(CLOCK_MONOTONIC, &tf);
error_metric<PointT>( par_res,input, er_g, er_r, er_d );
elapsed = (tf.tv_sec - ts.tv_sec);
elapsed += (tf.tv_nsec - ts.tv_nsec) / 1000000000.0;
xer_e1 = fabs(base.par.e[0] - par_res.e[0]);
xer_e2 = fabs(base.par.e[1] - par_res.e[1]);
vc = volSQ(par_res.dim[0], par_res.dim[1], par_res.dim[2], par_res.e[0], par_res.e[1]);
vr = volSQ(base.par.dim[0],base.par.dim[1],base.par.dim[2], base.par.e[0], base.par.e[1] );
xer_v = (vc - vr)/vr*100.0;
output << down->points.size() << std::endl;
saveParams( output, par_res, elapsed, er_g, er_r, xer_e1, xer_e2, xer_v);
}
clock_gettime(CLOCK_MONOTONIC, &finish);
dt = (finish.tv_sec - start.tv_sec);
dt += (finish.tv_nsec - start.tv_nsec) / 1000000000.0;
printf("* Total time: %f \n", dt );
output.close();
return 0;
}
void GetP3Responses(ap::template_2d_array<float,true>& signal,
ap::template_1d_array<short int,true>& trialnr,
ap::template_1d_array<double,true>& windowlen,
ap::template_1d_array<unsigned short int, true>& stimulusCode,
ap::template_1d_array<unsigned short int, true>& stimulusType,
const ap::template_1d_array<unsigned char, true>& Flashing,
const ap::template_1d_array<double, true>& channels,
const int MAfilter,
const int DecFact)
{
////////////////////////////////////////////////////////////////////////////
// Section: Define variables
int bound_min, bound_max, row, col, row_windowlen,
row_channels, numchannels, lenflash, sig_ds_len, siglen;
double val_temp;
ap::template_1d_array<float, true> vect;
ap::template_1d_array<float, true> vect_r;
ap::template_1d_array<float, true> a; //variable to store filter coefficients
ap::template_1d_array<float, true> b; //Variable to store filter coefficients
ap::template_2d_array<float, true> sig; //filtered and downsampled signal
ap::template_1d_array<float, true> y_downsampled;
ap::template_1d_array<unsigned short int, true> Code;
ap::template_1d_array<unsigned short int, true> Type;
ap::template_1d_array<short int, true> trial;
vector<int> tmp;
////////////////////////////////////////////////////////////////////////////
// Section: Get dimensions
row = signal.gethighbound(1)+1;
col = signal.gethighbound(0)+1;
row_windowlen = windowlen.gethighbound(1)+1;
row_channels = channels.gethighbound(1)+1;
numchannels = col;
lenflash = row;
////////////////////////////////////////////////////////////////////////////
// Section: Check if windowlen has only one value (e.g. windlen = 800)
if (row_windowlen == 1)
{
val_temp = windowlen(0);
windowlen.setbounds(0, row_windowlen);
windowlen(0) = 0;
windowlen(1) = val_temp;
}
////////////////////////////////////////////////////////////////////////////
// Section: Identify changes in Flashing
for (int i=1; i<lenflash; i++)
{
if (Flashing(i-1) == 0 && Flashing(i) ==1)
{
if((i+windowlen(1)-2)<lenflash)
tmp.push_back(i);
}
}
Code.setbounds(0,static_cast<int>(tmp.size())-1);
Type.setbounds(0,static_cast<int>(tmp.size())-1);
trial.setbounds(0, static_cast<int>(tmp.size())-1);
////////////////////////////////////////////////////////////////////////////
// Section: Filter and downsample the signal
sig_ds_len = ap::iceil((windowlen(1)-windowlen(0))/DecFact);
siglen = sig_ds_len * numchannels;
sig.setbounds(0,static_cast<int>(tmp.size())-1,0, siglen-1);
y_downsampled.setbounds(0, sig_ds_len-1);
// Define filter coefficients (Moving Average Filter)
a.setbounds(0, MAfilter-1);
b.setbounds(0, MAfilter-1);
for (int i=0; i<MAfilter; i++)
{
a(i) = (float) 1;
b(i) = (float) 1/MAfilter;
}
// Extract the signal according to the specified window
// then downsample and filtering it.
for(int j=0; j<static_cast<int>(tmp.size()); j++)
{
bound_min = static_cast<int>( tmp[j] + windowlen(0) - 1 );
bound_max = static_cast<int>( tmp[j] + windowlen(1) - 2 );
vect.setbounds(0, bound_max-bound_min);
vect_r.setbounds(0, bound_max-bound_min);
for (int i=0; i<col; i++)
{
ap::vmove(vect.getvector(0, bound_max-bound_min), signal.getcolumn(i, bound_min, bound_max));
filter(MAfilter-1, a, b, bound_max-bound_min, vect, vect_r);
downsampling(vect_r, DecFact, y_downsampled);
ap::vmove(sig.getrow(j, i*sig_ds_len, ((i+1)*sig_ds_len)-1), y_downsampled.getvector(0, sig_ds_len-1));
}
Code(j) = stimulusCode(tmp[j]);
Type(j) = stimulusType(tmp[j]);
trial(j) = trialnr(tmp[j]);
}
// Overwrite signal and states
signal.setbounds(0,static_cast<int>(tmp.size())-1,0, siglen-1);
stimulusCode.setbounds(0, static_cast<int>(tmp.size())-1);
stimulusType.setbounds(0, static_cast<int>(tmp.size())-1);
trialnr.setbounds(0, static_cast<int>(tmp.size())-1);
signal = sig;
stimulusCode = Code;
stimulusType = Type;
trialnr = trial;
}
bool change_detection (tms_msg_ss::ods_furniture::Request &req, tms_msg_ss::ods_furniture::Response &res)
{
//***************************
// local variable declaration
//***************************
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr model (new pcl::PointCloud<pcl::PointXYZRGB>);
std::cout << mkdir("/tmp/tms_ss_ods", S_IRUSR | S_IWUSR | S_IXUSR) << std::endl;
std::cout << mkdir("/tmp/tms_ss_ods/ods_change_detection", S_IRUSR | S_IWUSR | S_IXUSR) << std::endl;
std::cout << mkdir("/tmp/tms_ss_ods/ods_change_detection/table", S_IRUSR | S_IWUSR | S_IXUSR) << std::endl;
//***************************
// capture kinect data
//***************************
tms_msg_ss::ods_pcd srv;
srv.request.id = 3;
if(commander_to_kinect_capture.call(srv)){
pcl::fromROSMsg (srv.response.cloud, *cloud1);
}
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/input.pcd", *cloud1);
//pcl::fromROSMsg (req.model, *model);
pcl::io::loadPCDFile("/tmp/tms_ss_ods/ods_change_detection/table/input.pcd", *cloud1);
TABLE = req.id;
if(TABLE == 1)
pcl::io::loadPCDFile ("/tmp/tms_ss_ods/ods_change_detection/table/model_table1.pcd", *model);
else if(TABLE == 2)
pcl::io::loadPCDFile ("/tmp/tms_ss_ods/ods_change_detection/table/model_table2.pcd", *model);
//***************************
// Fill in the cloud data
//***************************
// table.x = req.furniture.position.x/1000.0;
// table.y = req.furniture.position.y/1000.0;
// table.z = req.furniture.position.z/1000.0;
// table.theta = req.furniture.orientation.z;
// robot.x = req.robot.x/1000.0;
// robot.y = req.robot.y/1000.0;
// robot.theta = req.robot.theta;
// sensor.x = req.sensor.position.x;
// sensor.y = req.sensor.position.y;
// sensor.z = req.sensor.position.z;
// sensor.theta = req.sensor.orientation.y;
table.x = 1.0;
table.y = 1.5;
table.z = 0.7;
table.theta = 0.0;
robot.x = 2.6;
robot.y = 1.9;
robot.theta = 180.0;
sensor.x = 0.0;
sensor.y = 0.0;
sensor.z = 1.0;
sensor.theta = 20.0;
std::cout << robot.x << " " << robot.y << " " << robot.theta << std::endl;
//***************************
// transform input cloud
//***************************
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tfm_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
transformation(*cloud1, *model);
for(int i=0;i<model->points.size();i++){
model->points[i].r = 255;
model->points[i].g = 0;
model->points[i].b = 0;
}
*tfm_cloud = *model + *cloud1;
if(!(tfm_cloud->points.size())){
std::cout << "tfm_cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/tfm_cloud1.pcd", *tfm_cloud);
//***************************
// filtering by using model
//***************************
std::cout << "filtering" << std::endl;
filtering(*cloud1, *model);
if(!(cloud1->points.size())){
std::cout << "filtered cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/filter.pcd", *cloud1);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr dsp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud1, *dsp_cloud);
downsampling(*dsp_cloud, 0.01);
//***************************
// registration between two input pcd data
//***************************
std::cout << "registration" << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgs_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr view_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Matrix4f m;
int n = 0;
while (1){
registration(*dsp_cloud, *model, *rgs_cloud, *cloud1, m);
if((double)(m(0,0)+m(1,1)+m(2,2)+m(3,3)) >= 4){
if(n > 2) break;
}
n++;
}
pcl::copyPointCloud(*cloud1, *view_cloud);
if(!(rgs_cloud->points.size())){
std::cout << "registered cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/rgs_cloud.pcd", *rgs_cloud);
//***************************
// init t_voxel
//***************************
std::cout << "init table_voxel" << std::endl;
make_tablevoxel(*model);
//***************************
// remove table
//***************************
std::cout << "remove table" << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rmc_cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rmc_cloud2 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile("/tmp/tms_ss_ods/ods_change_detection/table/memory.pcd", *rmc_cloud2);
remove_table(*cloud1, *rmc_cloud1);
if(!(rmc_cloud1->size() != 0)){
std::cout << "removed table cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/remove_table1.pcd", *rmc_cloud1, false);
//***************************
// segmentation
//***************************
segmentation(*rmc_cloud1, 0.015, 1);
if(rmc_cloud2->size() != 0)
segmentation(*rmc_cloud2, 0.015, 2);
if(rmc_cloud1->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/memory.pcd", *rmc_cloud1, true);
if(rmc_cloud2->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/obj_cloud2.pcd", *rmc_cloud2, true);
//***************************
// compare segments with memory
//***************************
segment_matching();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_rgb1 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_rgb2 (new pcl::PointCloud<pcl::PointXYZRGB>);
//***************************************
// rewrite Object data on TMS database
//***************************************
tms_msg_db::tmsdb_ods_object_data srv3;
ros::Time time = ros::Time::now() + ros::Duration(9*60*60);
int c=0;
float colors[6][3] ={{255, 0, 0}, {0,255,0}, {0,0,255}, {255,255,0}, {0,255,255}, {255,0,255}};
for(int i=0;i<Object_Map.size();i++){
std::cout << i << " → " << Object_Map[i].tf << std::endl;
if((Object_Map[i].tf == 1)||(Object_Map[i].tf == 2)){
std::stringstream ss;
ss << "/tmp/tms_ss_ods/ods_change_detection/table/result_rgb" << i << ".pcd";
if(Object_Map[i].cloud_rgb.size() != 0){
pcl::io::savePCDFile(ss.str (), Object_Map[i].cloud_rgb);
for(int j=0;j<Object_Map[i].cloud_rgb.points.size();j++){
Object_Map[i].cloud_rgb.points[j].r = colors[c%6][0];
Object_Map[i].cloud_rgb.points[j].g = colors[c%6][1];
Object_Map[i].cloud_rgb.points[j].b = colors[c%6][2];
}
c++;
}
else
std::cout << "no cloud_rgb data" << std::endl;
if(Object_Map[i].tf == 1){
*tmp_rgb1 += Object_Map[i].cloud_rgb;
tms_msg_db::tmsdb_data data;
data.tMeasuredTime = time;
data.iType = 5;
data.iID = 53;
data.fX = 1000.0 * (Object_Map[i].g.x - 1.0);
data.fY = 1000.0 * (Object_Map[i].g.y - 1.5);
data.fZ = 700.0;
data.fTheta = 0.0;
data.iPlace = 14;
data.iState = 1;
srv3.request.srvTMSInfo.push_back(data);
geometry_msgs::Pose pose;
pose.position.x = Object_Map[i].g.x; pose.position.y = Object_Map[i].g.y; pose.position.z = Object_Map[i].g.z;
pose.orientation.x = 0.0; pose.orientation.y = 0.0; pose.orientation.z = 0.0; pose.orientation.w = 0.0;
res.objects.poses.push_back(pose);
std::cout << Object_Map[i].g.x << " " << Object_Map[i].g.y << " " << Object_Map[i].g.z << std::endl;
}
else if(Object_Map[i].tf == 2)
*tmp_rgb2 += Object_Map[i].cloud_rgb;
}
}
if(srv3.request.srvTMSInfo.size() != 0){
if(commander_to_ods_object_data.call(srv3))
std::cout << "Rewrite TMS database!!" << std::endl;
}
if((tmp_rgb1->size() == 0) && (tmp_rgb2->size() == 0)){
std::cout << "No change on table!!" << std::endl;
return true;
}
if(tmp_rgb1->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/result1.pcd", *tmp_rgb1, true);
if(tmp_rgb2->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/result2.pcd", *tmp_rgb2, true);
*view_cloud += *tmp_rgb1;
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/view_result.pcd", *view_cloud, true);
//pcl::toROSMsg (*cloud1, res.cloud);
Object_Map.clear();
return true;
}
/************************************************************************************************************************************
void update_yuv(unsigned char** Y, unsigned char** U, unsigned char** V, int height, int width)
{
FILE* fp;
int outer_r, inner_r, outer_c, inner_c;
if((fp = fopen("Down_Sampled_YUV","rb")) == NULL)
{
printf("\n In update_yuv() Error opening DCT_output file !");
exit(0);
}
for(outer_r =0; outer_r < height; outer_r += 8)
{
for(outer_c = 0; outer_c < width; outer_c += 8)
{
for(inner_r = outer_r; inner_r < outer_r+8; inner_r++)
{
for(inner_c = outer_c; inner_c < outer_c+8; inner_c++)
{
fscanf(fp,"%d\t",Y[inner_r][inner_c]);
}
}
}
}
for(outer_r =0; outer_r < height; outer_r += 8)
{
for(outer_c = 0; outer_c < width; outer_c += 8)
{
for(inner_r = outer_r; inner_r < outer_r+8; inner_r++)
{
for(inner_c = outer_c; inner_c < outer_c+8; inner_c++)
{
fscanf(fp,"%d\t",U[inner_r][inner_c]);
}
}
}
}
for(outer_r =0; outer_r < height; outer_r += 8)
{
for(outer_c = 0; outer_c < width; outer_c += 8)
{
for(inner_r = outer_r; inner_r < outer_r+8; inner_r++)
{
for(inner_c = outer_c; inner_c < outer_c+8; inner_c++)
{
fscanf(fp,"%d\t",V[inner_r][inner_c]);
}
}
}
}
}
***************************************************************************************************************************************/
int main(int argc, char const *argv[])
{
FILE*fp, *image_rgb, *image_yuv, *down_sampled, *DCT, *image;
unsigned char** red, **green, **blue, **Y, **U, **V, *image_contents_rgb, *image_contents_yuv, *image_contents_rgbt;
int i,ERROR;
Header1 header1;
Header2 header2;
if((image_rgb = fopen("Image_Contents_RGB","wb")) == NULL) /*For storing RGB contents*/
{
printf("\n Error creating Image_Contents_RGB file !!");
return -1;
}
if((image_yuv = fopen("Image_Contents_YUV","wb")) == NULL) /* For storing YUV contents*/
{
printf("\n Error creating Image_Contents_YUV file !!");
return -1;
}
if((down_sampled = fopen("Down_Sampled_YUV","wb")) == NULL) /* For storing down sampled YUV contents*/
{
printf("\n Error creating Down_Sampled_YUV file !!");
return -1;
}
if((DCT = fopen("DCT_Output","wb")) == NULL) /* For storing DCT contents*/
{
printf("\n Error creating Down_Sampled_YUV file !!");
return -1;
}
if(argc!= 2) /* syntax error check*/
{
printf("\n The format is ./quantizer [file-name]");
return -1;
}
if((fp = fopen(argv[1],"rb"))== NULL) /* open the .bmp file for reading*/
{
printf("\n Error opening the file specified. Please check if the file exists !!\n");
fclose(fp);
return -1;
}
if(fread(&header1,sizeof(header1),1,fp)!=1) /* Read the primary header from the bmp file */
{
printf("\n Error reading header1 \n");
fclose(fp);
return -1;
}
if(header1.type!= 19778) /* check if its a valid bmp file*/
{
printf("\n Not a bmp file. Exiting !! \n");
fclose(fp);
return -1;
}
if(fread(&header2,sizeof(header2),1,fp)!=1 )
{
printf("\n Error reading header 2");
fclose(fp);
return -1;
}
image_contents_rgb = (unsigned char*)malloc(sizeof(char) * header2.imagesize);
image_contents_rgbt = (unsigned char*)malloc(sizeof(char) * header2.imagesize); /*allocate memory to store image data*/
image_contents_yuv = (unsigned char*)malloc(sizeof(char) * header2.imagesize);
fseek(fp,header1.offset,SEEK_SET); /* To assign file pointer to start of image byte*/
if((ERROR = fread(image_contents_rgb,header2.imagesize,1,fp))!=1)
{
printf("\nError reading contents\n");
free(image_contents_rgb);
fclose(fp);
return -1;
}
fclose(fp);
red = (unsigned char**)malloc(sizeof(char*) * header2.height);
green = (unsigned char**)malloc(sizeof(char*) * header2.height);
blue = (unsigned char**)malloc(sizeof(char*) * header2.height);
Y = (unsigned char**)malloc(sizeof(char*) * header2.height);
U = (unsigned char**)malloc(sizeof(char*) * header2.height);
V = (unsigned char**)malloc(sizeof(char*) * header2.height);
for(i=0 ; i<header2.height ;i++)
{
red[i] = (unsigned char*)malloc( sizeof(char) * header2.width);
green[i]= (unsigned char*)malloc( sizeof(char) * header2.width);
blue[i]= (unsigned char*)malloc( sizeof(char) * header2.width);
Y[i] = (unsigned char*)malloc( sizeof(char) * header2.width);
U[i] = (unsigned char*)malloc( sizeof(char) * header2.width);
V[i] = (unsigned char*)malloc( sizeof(char) * header2.width);
}
vector2matrix(red, green, blue, image_contents_rgb, header2.height, header2.width); /* Store image contents as matrix */
FileWrite(red, green, blue, image_rgb, header2.height, header2.width); /* Optional step*/
rgb2yuv(red,green,blue,Y,U,V,header2.height,header2.width); /* RGB to YUV conversion*/
FileWrite(Y, U, V, image_yuv, header2.height, header2.width); /* Writing YUV into file*/
downsampling(Y, U, V, header2.height, header2.width); /* 4:2:0 Downsampling and update YUV */
FileWrite(Y, U, V, down_sampled, header2.height, header2.width); /* Write downsampled YUV into file*/
dct(DCT, header2.height, header2.width); /* Perform dct and store the result into a file*/
printf("\n");
yuv2rgb(red,green,blue,Y,U,V,header2.height,header2.width); /* Optional step*/
matrix2vector_rgb(red,green,blue,image_contents_rgbt,header2.height,header2.width); /* convert back from matrix to vector*/
matrix2vector_yuv(Y,U,V,image_contents_yuv,header2.height,header2.width);
if((image = fopen("Output_image","wb")) == NULL)
{
printf("\n ERROR opening the file to write quantized image !!\n");
fclose(image);
return -1;
}
if(fwrite(&header1,sizeof(header1),1,image)!= 1)
{
printf("\n ERROR writing header 1 into destination file !!\n");
fclose(image);
return -1;
}
if(fwrite(&header2,sizeof(header2),1,image)!= 1)
{
printf("\n ERROR writing header 2 into destination file !! \n");
fclose(image);
return -1;
}
fseek(image, header1.offset, SEEK_SET);
if(fwrite(image_contents_rgbt,header2.imagesize,1,image)!=1) /* Change the vector to write into the bmp file here*/
{
printf("\n ERROR writing image contents into destination file \n");
fclose(image);
return -1;
}
free(red);
free(green);
free(blue);
free(Y);
free(U);
free(V);
fclose(image);
return 0;
}
