void EditPoint::handleEvent(Event *event) {
if(event->getDispatcher() == CoreServices::getInstance()->getCore()->getInput()) {
switch(event->getEventCode()) {
case InputEvent::EVENT_MOUSEMOVE:
if(dragging) {
if(draggingPoint) {
Vector2 newPosition = CoreServices::getInstance()->getCore()->getInput()->getMousePosition();
Vector2 translateAmt = Vector2(basePosition.x - newPosition.x, basePosition.y - newPosition.y);
if(type != TYPE_POINT && draggingPoint == pointHandle) {
// don't let drag start and end control points
translateAmt.x = 0.0;
}
draggingPoint->setPosition(basePointPosition.x - translateAmt.x, basePointPosition.y - translateAmt.y);
if(draggingPoint == pointHandle) {
controlHandle1->setPosition(baseControl1.x - translateAmt.x, baseControl1.y - translateAmt.y);
controlHandle2->setPosition(baseControl2.x - translateAmt.x, baseControl2.y - translateAmt.y);
}
limitPoint(pointHandle);
limitPoint(controlHandle1);
limitPoint(controlHandle2);
updateCurvePoint();
dispatchEvent(new Event(), Event::CHANGE_EVENT);
}
}
break;
}
}
if(event->getDispatcher() == pointHandle || event->getDispatcher() == controlHandle1 || event->getDispatcher() == controlHandle2) {
switch(event->getEventCode()) {
case InputEvent::EVENT_MOUSEDOWN:
if(mode == CurveEditor::MODE_SELECT) {
draggingPoint = (ScreenImage*)event->getDispatcher();
dragging = true;
basePosition = CoreServices::getInstance()->getCore()->getInput()->getMousePosition();
basePointPosition = draggingPoint->getPosition2D();
baseControl1 = controlHandle1->getPosition2D();
baseControl2 = controlHandle2->getPosition2D();
}
if(mode == CurveEditor::MODE_REMOVE) {
if(type == TYPE_POINT) {
dispatchEvent(new Event(), Event::CANCEL_EVENT);
}
}
break;
case InputEvent::EVENT_MOUSEUP:
case InputEvent::EVENT_MOUSEUP_OUTSIDE:
dragging = false;
draggingPoint = NULL;
break;
}
}
}
void vad_cb(const sensor_msgs::PointCloud2ConstPtr& cloud) {
if ((cloud->width * cloud->height) == 0)
return;
//ROS_INFO ("Received %d data points in frame %s with the following fields: %s", (int)cloud->width * cloud->height, cloud->header.frame_id.c_str (), pcl::getFieldsList (*cloud).c_str ());
//std::cout << "fromROSMsg?" << std::endl;
pcl::fromROSMsg (*cloud, cloud_xyz_);
//std::cout << " fromROSMsg done." << std::endl;
t0 = my_clock();
if( limitPoint( cloud_xyz_, cloud_xyz, distance_th ) > 10 ){
//std::cout << " limit done." << std::endl;
std::cout << "compute normals and voxelize...." << std::endl;
//****************************************
//* compute normals
n3d.setInputCloud (cloud_xyz.makeShared());
n3d.setRadiusSearch (normals_radius_search);
normals_tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
n3d.setSearchMethod (normals_tree);
n3d.compute (cloud_normal);
pcl::concatenateFields (cloud_xyz, cloud_normal, cloud_xyz_normal);
t0_2 = my_clock();
//* voxelize
getVoxelGrid( grid, cloud_xyz_normal, cloud_downsampled, voxel_size );
std::cout << " ...done.." << std::endl;
const int pnum = cloud_downsampled.points.size();
float x_min = 10000000, y_min = 10000000, z_min = 10000000;
float x_max = -10000000, y_max = -10000000, z_max = -10000000;
for( int p=0; p<pnum; p++ ){
if( cloud_downsampled.points[ p ].x < x_min ) x_min = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y < y_min ) y_min = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z < z_min ) z_min = cloud_downsampled.points[ p ].z;
if( cloud_downsampled.points[ p ].x > x_max ) x_max = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y > y_max ) y_max = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z > z_max ) z_max = cloud_downsampled.points[ p ].z;
}
//std::cout << x_min << " " << y_min << " " << z_min << std::endl;
//std::cout << x_max << " " << y_max << " " << z_max << std::endl;
//std::cout << grid.getMinBoxCoordinates() << std::endl;
std::cout << "search start..." << std::endl;
//****************************************
//* object detection start
t1 = my_clock();
search_obj.cleanData();
search_obj.setGRSD( dim, grid, cloud_xyz_normal, cloud_downsampled, voxel_size, box_size );
t1_2 = my_clock();
if( ( search_obj.XYnum() != 0 ) && ( search_obj.Znum() != 0 ) )
search_obj.searchWithoutRotation();
t2 = my_clock();
//* object detection end
//****************************************
std::cout << " ...search done." << std::endl;
tAll += t2 - t0;
process_count++;
std::cout << "normal estimation :"<< t0_2 - t0 << " sec" << std::endl;
std::cout << "voxelize :"<< t1 - t0_2 << " sec" << std::endl;
std::cout << "feature extraction : "<< t1_2 - t1 << " sec" <<std::endl;
std::cout << "search : "<< t2 - t1_2 << " sec" <<std::endl;
std::cout << "all processes : "<< t2 - t0 << " sec" << std::endl;
std::cout << "AVERAGE : "<< tAll / process_count << " sec" << std::endl;
marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker_range", 1);
marker_array_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
visualization_msgs::MarkerArray marker_array_msg_;
//* show the limited space
visualization_msgs::Marker marker_;
marker_.header.frame_id = "openni_rgb_optical_frame";
marker_.header.stamp = ros::Time::now();
marker_.ns = "BoxEstimation";
marker_.id = -1;
marker_.type = visualization_msgs::Marker::CUBE;
marker_.action = visualization_msgs::Marker::ADD;
marker_.pose.position.x = (x_max+x_min)/2;
marker_.pose.position.y = (y_max+y_min)/2;
marker_.pose.position.z = (z_max+z_min)/2;
marker_.pose.orientation.x = 0;
marker_.pose.orientation.y = 0;
marker_.pose.orientation.z = 0;
marker_.pose.orientation.w = 1;
marker_.scale.x = x_max-x_min;
marker_.scale.y = x_max-x_min;
marker_.scale.z = x_max-x_min;
marker_.color.a = 0.1;
marker_.color.r = 1.0;
marker_.color.g = 0.0;
marker_.color.b = 0.0;
marker_.lifetime = ros::Duration();
marker_pub_.publish(marker_);
for( int q=0; q<rank_num; q++ ){
if( search_obj.maxDot( q ) < detect_th ) break;
std::cout << search_obj.maxX( q ) << " " << search_obj.maxY( q ) << " " << search_obj.maxZ( q ) << std::endl;
std::cout << "dot " << search_obj.maxDot( q ) << std::endl;
//if( (search_obj.maxX( q )!=0)||(search_obj.maxY( q )!=0)||(search_obj.maxZ( q )!=0) ){
//* publish marker
visualization_msgs::Marker marker_;
//marker_.header.frame_id = "base_link";
marker_.header.frame_id = "openni_rgb_optical_frame";
marker_.header.stamp = ros::Time::now();
marker_.ns = "BoxEstimation";
marker_.id = q;
marker_.type = visualization_msgs::Marker::CUBE;
marker_.action = visualization_msgs::Marker::ADD;
marker_.pose.position.x = search_obj.maxX( q ) * region_size + sliding_box_size_x/2 + x_min;
marker_.pose.position.y = search_obj.maxY( q ) * region_size + sliding_box_size_y/2 + y_min;
marker_.pose.position.z = search_obj.maxZ( q ) * region_size + sliding_box_size_z/2 + z_min;
marker_.pose.orientation.x = 0;
marker_.pose.orientation.y = 0;
marker_.pose.orientation.z = 0;
marker_.pose.orientation.w = 1;
marker_.scale.x = sliding_box_size_x;
marker_.scale.y = sliding_box_size_y;
marker_.scale.z = sliding_box_size_z;
marker_.color.a = 0.5;
marker_.color.r = 0.0;
marker_.color.g = 1.0;
marker_.color.b = 0.0;
marker_.lifetime = ros::Duration();
// std::cerr << "BOX MARKER COMPUTED, WITH FRAME " << marker_.header.frame_id << " POSITION: "
// << marker_.pose.position.x << " " << marker_.pose.position.y << " "
// << marker_.pose.position.z << std::endl;
// marker_pub_.publish (marker_);
// }
marker_array_msg_.markers.push_back(marker_);
}
//std::cerr << "MARKER ARRAY published with size: " << marker_array_msg_.markers.size() << std::endl;
marker_array_pub_.publish(marker_array_msg_);
}
std::cout << "Waiting msg..." << std::endl;
}
void vad_cb(const sensor_msgs::PointCloud2ConstPtr& cloud) {
if ((cloud->width * cloud->height) == 0)
return;
pcl::fromROSMsg (*cloud, cloud_xyzrgb_);
t0 = my_clock();
if( limitPoint( cloud_xyzrgb_, cloud_xyzrgb, distance_th ) > 10 ){
std::cout << "compute normals and voxelize...." << std::endl;
#ifdef CCHLAC_TEST
getVoxelGrid( grid, cloud_xyzrgb, cloud_downsampled, voxel_size );
#else
//****************************************
//* compute normals
computeNormal( cloud_xyzrgb, cloud_normal );
t0_2 = my_clock();
//* voxelize
getVoxelGrid( grid, cloud_normal, cloud_downsampled, voxel_size );
#endif
std::cout << " ...done.." << std::endl;
const int pnum = cloud_downsampled.points.size();
float x_min = 10000000, y_min = 10000000, z_min = 10000000;
float x_max = -10000000, y_max = -10000000, z_max = -10000000;
for( int p=0; p<pnum; p++ ){
if( cloud_downsampled.points[ p ].x < x_min ) x_min = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y < y_min ) y_min = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z < z_min ) z_min = cloud_downsampled.points[ p ].z;
if( cloud_downsampled.points[ p ].x > x_max ) x_max = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y > y_max ) y_max = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z > z_max ) z_max = cloud_downsampled.points[ p ].z;
}
//std::cout << x_min << " " << y_min << " " << z_min << std::endl;
//std::cout << x_max << " " << y_max << " " << z_max << std::endl;
//std::cout << grid.getMinBoxCoordinates() << std::endl;
std::cout << "search start..." << std::endl;
//****************************************
//* object detection start
t1 = my_clock();
search_obj.cleanData();
#ifdef CCHLAC_TEST
search_obj.setC3HLAC( dim, color_threshold_r, color_threshold_g, color_threshold_b, grid, cloud_downsampled, voxel_size, box_size );
#else
search_obj.setVOSCH( dim, color_threshold_r, color_threshold_g, color_threshold_b, grid, cloud_normal, cloud_downsampled, voxel_size, box_size );
#endif
t1_2 = my_clock();
if( ( search_obj.XYnum() != 0 ) && ( search_obj.Znum() != 0 ) )
#ifdef CCHLAC_TEST
search_obj.search();
#else
search_obj.searchWithoutRotation();
#endif
search_obj.removeOverlap();
t2 = my_clock();
//* object detection end
//****************************************
std::cout << " ...search done." << std::endl;
tAll += t2 - t0;
process_count++;
#ifdef CCHLAC_TEST
std::cout << "voxelize :"<< t1 - t0 << " sec" << std::endl;
#else
std::cout << "normal estimation :"<< t0_2 - t0 << " sec" << std::endl;
std::cout << "voxelize :"<< t1 - t0_2 << " sec" << std::endl;
#endif
std::cout << "feature extraction : "<< t1_2 - t1 << " sec" <<std::endl;
std::cout << "search : "<< t2 - t1_2 << " sec" <<std::endl;
std::cout << "all processes : "<< t2 - t0 << " sec" << std::endl;
std::cout << "AVERAGE : "<< tAll / process_count << " sec" << std::endl;
marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker_range", 1);
marker_array_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
visualization_msgs::MarkerArray marker_array_msg_;
#if 0
//* show the limited space
marker_pub_.publish( setMarker( -1, (x_max+x_min)/2, (y_max+y_min)/2, (z_max+z_min)/2, x_max-x_min, y_max-y_min, z_max-z_min, 0.1, 1.0, 0.0, 0.0 ) );
#endif
for( int m=0; m<model_num; m++ ){
for( int q=0; q<rank_num; q++ ){
//if( search_obj.maxDot( m, q ) < detect_th ) break;
std::cout << search_obj.maxX( m, q ) << " " << search_obj.maxY( m, q ) << " " << search_obj.maxZ( m, q ) << std::endl;
std::cout << "dot " << search_obj.maxDot( m, q ) << std::endl;
//if( (search_obj.maxX( m, q )!=0)||(search_obj.maxY( m, q )!=0)||(search_obj.maxZ( m, q )!=0) ){
//* publish markers for detected regions
if( search_obj.maxDot( m, q ) < detect_th )
marker_array_msg_.markers.push_back( setMarker( m, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ) );
else{
marker_array_msg_.markers.push_back( setMarker( m, search_obj.maxX( m, q ) * region_size + sliding_box_size_x/2 + x_min, search_obj.maxY( m, q ) * region_size + sliding_box_size_y/2 + y_min, search_obj.maxZ( m, q ) * region_size + sliding_box_size_z/2 + z_min, sliding_box_size_x, sliding_box_size_y, sliding_box_size_z, 0.5, marker_color_r[ m ], marker_color_g[ m ], marker_color_b[ m ] ) );
}
}
}
//std::cerr << "MARKER ARRAY published with size: " << marker_array_msg_.markers.size() << std::endl;
marker_array_pub_.publish(marker_array_msg_);
}
std::cout << "Waiting msg..." << std::endl;
}
void vad_cb(const sensor_msgs::PointCloud2ConstPtr& cloud) {
if ((cloud->width * cloud->height) == 0)
return;
pcl::fromROSMsg (*cloud, cloud_xyzrgb_);
t0 = my_clock();
if( limitPoint( cloud_xyzrgb_, cloud_xyzrgb, distance_th ) > 10 ){
std::cout << "voxelize...." << std::endl;
getVoxelGrid( grid, cloud_xyzrgb, cloud_downsampled, voxel_size );
std::cout << " ...done.." << std::endl;
const int pnum = cloud_downsampled.points.size();
float x_min = 10000000, y_min = 10000000, z_min = 10000000;
float x_max = -10000000, y_max = -10000000, z_max = -10000000;
for( int p=0; p<pnum; p++ ){
if( cloud_downsampled.points[ p ].x < x_min ) x_min = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y < y_min ) y_min = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z < z_min ) z_min = cloud_downsampled.points[ p ].z;
if( cloud_downsampled.points[ p ].x > x_max ) x_max = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y > y_max ) y_max = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z > z_max ) z_max = cloud_downsampled.points[ p ].z;
}
//std::cout << x_min << " " << y_min << " " << z_min << std::endl;
//std::cout << x_max << " " << y_max << " " << z_max << std::endl;
//std::cout << grid.getMinBoxCoordinates() << std::endl;
std::cout << "search start..." << std::endl;
//****************************************
//* object detection start
t1 = my_clock();
search_obj.cleanData();
search_obj.setC3HLAC( dim, color_threshold_r, color_threshold_g, color_threshold_b, grid, cloud_downsampled, voxel_size, box_size );
t1_2 = my_clock();
if( ( search_obj.XYnum() != 0 ) && ( search_obj.Znum() != 0 ) )
search_obj.search();
t2 = my_clock();
//* object detection end
//****************************************
std::cout << " ...search done." << std::endl;
//* show the processing time
tAll += t2 - t0;
process_count++;
std::cout << "voxelize :"<< t1 - t0 << " sec" << std::endl;
std::cout << "feature extraction : "<< t1_2 - t1 << " sec" <<std::endl;
std::cout << "search : "<< t2 - t1_2 << " sec" <<std::endl;
std::cout << "all processes : "<< t2 - t0 << " sec" << std::endl;
std::cout << "AVERAGE : "<< tAll / process_count << " sec" << std::endl;
marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker_range", 1);
marker_array_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
visualization_msgs::MarkerArray marker_array_msg_;
//* show the limited space
//setMarker( id_, pos_x, pos_y, pos_z, scale_x, scale_y, scale_z, ca, cr, cg, cb )
marker_pub_.publish( setMarker( -1, (x_max+x_min)/2, (y_max+y_min)/2, (z_max+z_min)/2, x_max-x_min, y_max-y_min, z_max-z_min, 0.1, 1.0, 0.0, 0.0 ) );
//* publish markers for detected regions
for( int q=0; q<rank_num; q++ ){
if( search_obj.maxDot( q ) < detect_th )
marker_array_msg_.markers.push_back( setMarker( q, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ) );
else{
std::cout << search_obj.maxX( q ) << " " << search_obj.maxY( q ) << " " << search_obj.maxZ( q ) << std::endl;
std::cout << "dot " << search_obj.maxDot( q ) << std::endl;
marker_array_msg_.markers.push_back( setMarker( q, search_obj.maxX( q ) * region_size + sliding_box_size_x/2 + x_min, search_obj.maxY( q ) * region_size + sliding_box_size_y/2 + y_min, search_obj.maxZ( q ) * region_size + sliding_box_size_z/2 + z_min, sliding_box_size_x, sliding_box_size_y, sliding_box_size_z, 0.5, 0.0, 1.0, 0.0 ) );
}
}
//std::cerr << "MARKER ARRAY published with size: " << marker_array_msg_.markers.size() << std::endl;
marker_array_pub_.publish(marker_array_msg_);
}
std::cout << "Waiting msg..." << std::endl;
}