void getCameraRay(const image_geometry::PinholeCameraModel& model, const cv::Point2d& pt, cv::Point3d* ray)
{
cv::Point2d rect_point;
rect_point = model.rectifyPoint(pt);
ROS_DEBUG("Rect Point: %f, %f",rect_point.x,rect_point.y);
*ray = model.projectPixelTo3dRay(rect_point);
}
void FindObjectOnPlane::generateStartPoints(
const cv::Point2f& point_2d,
const image_geometry::PinholeCameraModel& model,
const pcl::ModelCoefficients::Ptr& coefficients,
std::vector<cv::Point3f>& search_points_3d,
std::vector<cv::Point2f>& search_points_2d)
{
NODELET_INFO("generateStartPoints");
jsk_recognition_utils::Plane::Ptr plane
(new jsk_recognition_utils::Plane(coefficients->values));
cv::Point3d ray = model.projectPixelTo3dRay(point_2d);
Eigen::Vector3f projected_point = rayPlaneInteersect(ray, plane);
const double resolution_step = 0.01;
const int resolution = 5;
search_points_3d.clear();
search_points_2d.clear();
for (int i = - resolution; i < resolution; ++i) {
for (int j = - resolution; j < resolution; ++j) {
double x_diff = resolution_step * i;
double y_diff = resolution_step * j;
Eigen::Vector3f moved_point = projected_point + Eigen::Vector3f(x_diff, y_diff, 0);
Eigen::Vector3f projected_moved_point;
plane->project(moved_point, projected_moved_point);
cv::Point3f projected_moved_point_cv(projected_moved_point[0],
projected_moved_point[1],
projected_moved_point[2]);
search_points_3d.push_back(cv::Point3f(projected_moved_point_cv));
cv::Point2d p2d = model.project3dToPixel(projected_moved_point_cv);
search_points_2d.push_back(p2d);
}
}
}
pcl::PointXYZ PointFromPixel(const cv::Point& pixel, const tf::Transform& cameraFrameToWorldFrame, image_geometry::PinholeCameraModel cam) {
cv::Point3d cameraRay = cam.projectPixelTo3dRay(pixel);
tf::Point worldCameraOrigin = cameraFrameToWorldFrame * tf::Vector3(0, 0, 0);
tf::Point worldCameraStep = cameraFrameToWorldFrame * tf::Vector3(cameraRay.x, cameraRay.y, cameraRay.z) - worldCameraOrigin;
double zScale = -worldCameraOrigin.z()/worldCameraStep.z();
tf::Point ret = worldCameraOrigin + zScale * worldCameraStep;
return pcl::PointXYZ(ret.x(), ret.y(), 0);
}
cv::Point2d FindObjectOnPlane::getUyEnd(
const cv::Point2d& ux_start,
const cv::Point2d& ux_end,
const image_geometry::PinholeCameraModel& model,
const jsk_recognition_utils::Plane::Ptr& plane)
{
cv::Point3d start_ray = model.projectPixelTo3dRay(ux_start);
cv::Point3d end_ray = model.projectPixelTo3dRay(ux_end);
Eigen::Vector3f ux_start_3d = rayPlaneInteersect(start_ray, plane);
Eigen::Vector3f ux_end_3d = rayPlaneInteersect(end_ray, plane);
Eigen::Vector3f ux_3d = ux_end_3d - ux_start_3d;
Eigen::Vector3f normal = plane->getNormal();
Eigen::Vector3f uy_3d = normal.cross(ux_3d).normalized();
Eigen::Vector3f uy_end_3d = ux_start_3d + uy_3d;
cv::Point2d uy_end = model.project3dToPixel(cv::Point3d(uy_end_3d[0],
uy_end_3d[1],
uy_end_3d[2]));
return uy_end;
}
size_t project_uv_to_cloud_index(const pcl::PointCloud<PointT>& cloud, const cv::Point2d& image_point,
const image_geometry::PinholeCameraModel camera_model, double& distance)
{
// Assumes camera is at origin, pointed in the normal direction
size_t pt_pcl_index;
cv::Point3d pt_cv;
pt_cv = camera_model.projectPixelTo3dRay(image_point);
Eigen::Vector3f direction_eig(pt_cv.x, pt_cv.y, pt_cv.z);
Eigen::Vector3f origin_eig(0.0, 0.0, 0.0);
pt_pcl_index = closest_point_index_rayOMP(cloud, direction_eig, origin_eig, distance);
return (pt_pcl_index);
}
cv::Mat projectWithEigen()
{
// Transform meshes into camera frame
// For each frame in vector
for (int frame = 0; frame < mMeshFrameIDs.size(); frame++)
{
// Lookup current transform
Eigen::Isometry3 transform;
transform = transforms.at(mMeshFrameIDs[frame]);
// Get copy of mesh for each frame
ap::Mesh* sourceMesh;
ap::Mesh* transformedMesh;
//std::cerr << "Getting frame " << frame << " : " << mMeshFrameIDs[frame] << std::endl;
MeshMap::iterator scene_i = scenes.find(mMeshFrameIDs[frame]);
if (scenes.end() == scene_i) { continue; }
sourceMesh = scene_i->second;
MeshMap::iterator scene_t = transformedScenes.find(mMeshFrameIDs[frame]);
if (transformedScenes.end() == scene_t) { continue; }
transformedMesh = scene_t->second;
// Transform mesh into camera frame
for (int i = 0; i < sourceMesh->vertices.size(); i++)
{
Eigen::Vector3 newVertex = transform * sourceMesh->vertices[i];
//std::cerr << mesh->vertices[i].transpose() << "\t->\t" << newVertex.transpose() << std::endl;
transformedMesh->vertices[i] = newVertex;
}
}
// For each pixel in camera image
cv::Mat robotImage(mCameraModel.cameraInfo().height, mCameraModel.cameraInfo().width, CV_32F);
float* pixelPtr = (float*)robotImage.data;
float maxDepth = 0;
for (int v = 0; v < robotImage.rows; v++)
{
for (int u = 0; u < robotImage.cols; u++)
{
// Create a ray through the pixel
int pixelIdx = u + (v * robotImage.cols);
//std::cerr << "Pixel (" << u << "," << v << ")" << std::endl;
cv::Point2d pixel = cv::Point2d(u, v);
cv::Point3d cvRay = mCameraModel.projectPixelTo3dRay(pixel);
// Convert cvRay to ap::Ray
ap::Ray ray;
ray.point = Eigen::Vector3::Zero();
ray.vector.x() = cvRay.x; ray.vector.y() = cvRay.y; ray.vector.z() = cvRay.z;
ray.vector.normalize();
//std::cerr << ray.vector.transpose() << std::endl;
// For each frame in vector
for (int frame = 0; frame < mMeshFrameIDs.size(); frame++)
{
MeshMap::iterator scene_i = transformedScenes.find(mMeshFrameIDs[frame]);
if (transformedScenes.end() == scene_i)
{
continue;
}
ap::Mesh* mesh = scene_i->second;
// For each triangle in mesh
for (int i = 0; i < mesh->faces.size(); i++)
{
// Check for intersection. If finite, set distance
ap::Triangle triangle(mesh->vertices[mesh->faces[i].vertices[0]],
mesh->vertices[mesh->faces[i].vertices[1]],
mesh->vertices[mesh->faces[i].vertices[2]]);
Eigen::Vector3 intersection = ap::intersectRayTriangle(ray, triangle);
if (std::isfinite(intersection.x()))
{
float d = intersection.norm();
float val = pixelPtr[pixelIdx];
if (val == 0 || val > d)
{
pixelPtr[pixelIdx] = d;
}
if (d > maxDepth)
{
maxDepth = d;
}
}
}
}
}
}
// Return the matrix
if (maxDepth == 0) { maxDepth = 1;}
return robotImage/maxDepth;
}
void FeatureTracker::image_callback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg) {
//need pose data for each picture, need to publish a camera pose
ros::Time acquisition_time = msg->header.stamp;
geometry_msgs::PoseStamped basePose;
geometry_msgs::PoseStamped mapPose;
basePose.pose.orientation.w=1.0;
ros::Duration timeout(3);
basePose.header.frame_id="/base_link";
mapPose.header.frame_id="/map";
try {
tf_listener_.waitForTransform("/camera_1_link", "/map", acquisition_time, timeout);
tf_listener_.transformPose("/map", acquisition_time,basePose,"/camera_1_link",mapPose);
printf("pose #%d %f %f %f\n",pic_number++,mapPose.pose.position.x, mapPose.pose.position.y, tf::getYaw(mapPose.pose.orientation));
}
catch (tf::TransformException& ex) {
ROS_WARN("[map_maker] TF exception:\n%s", ex.what());
printf("[map_maker] TF exception:\n%s", ex.what());
return;
}
cam_model.fromCameraInfo(info_msg);
// printf("callback called\n");
try
{
// if you want to work with color images, change from mono8 to bgr8
if(image_rect==NULL){
image_rect = cvCloneImage(bridge.imgMsgToCv(msg, "mono8"));
last_image= cvCloneImage(bridge.imgMsgToCv(msg, "mono8"));
pyrA=cvCreateImage(cvSize(last_image->width+8,last_image->height/3.0), IPL_DEPTH_32F, 1);
pyrB=cvCloneImage(pyrA);
// printf("cloned image\n");
}
else{
//save the last image
cvCopy(image_rect,last_image);
cvCopy(bridge.imgMsgToCv(msg, "mono8"),image_rect);
// printf("copied image\n");
}
if(output_image==NULL){
output_image =cvCloneImage(image_rect);
}
if(eigImage==NULL){
eigImage =cvCloneImage(image_rect);
}
if(tempImage==NULL){
tempImage =cvCloneImage(image_rect);
}
}
catch (sensor_msgs::CvBridgeException& e)
{
ROS_ERROR("Could not convert from '%s' to 'mono8'.", msg->encoding.c_str());
return;
}
if(image_rect!=NULL) {
cvCopy(image_rect,output_image);
printf("got image\n");
track_features(mapPose);
//draw features on the image
for(int i=0;i<last_feature_count;i++){
CvPoint center=cvPoint((int)features[i].x,(int)features[i].y);
cvCircle(output_image,center,10,cvScalar(150),2);
char strbuf [10];
int n=sprintf(strbuf,"%d",current_feature_id[ i] );
std::string text=std::string(strbuf,n);
CvFont font;
cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX,1,1);
cvPutText(output_image,text.c_str(),cvPoint(center.x,center.y+20),&font,cvScalar(255));
cv::Point3d tempRay;
cv::Point2d tempPoint=cv::Point2d(features[i]);
cam_model.projectPixelTo3dRay(tempPoint,tempRay);
// printf("%f x %f y %f z\n",tempRay.x,tempRay.y,tempRay.z);
}
// featureList[0].print();
//determine error gradient
int min_features=10;
// printf("ypr %f %f %f\n",yaw,pitch,roll);
cv::Point3d error_sum=calc_error(min_features,0, 0, 0);
printf("total error is : %f\n",error_sum.x);
for(int i=0;i<featureList.size();i++){
if(min_features<featureList[i].numFeatures()){
printf("\n\n\nfeature %d\n",i);
printf("mean: %f %f %f\n",featureList[i].currentMean.x, featureList[i].currentMean.y, featureList[i].currentMean.z);
}
}
// double error_up= calc_error(min_features,yalpha, 0, 0);
// printf("total up yaw error is : %f\n",error_up);
// double error_down= calc_error(min_features,-yalpha, 0, 0);
// printf("total down yaw error is : %f\n",error_down);
/*
double yaw_change=0;
if(error_up<error_sum && error_up<error_down){
yaw_change=yalpha;
}else if(error_down<error_sum && error_down<error_up){
yaw_change=-yalpha;
}else if(error_down!=error_sum&&error_sum!=error_up){
yalpha/=2;
}
error_up= calc_error(min_features,0,palpha, 0);
// printf("total up pitch error is : %f\n",error_up);
error_down= calc_error(min_features,0,-palpha, 0);
// printf("total down pitch error is : %f\n",error_down);
double pitch_change=0;
if(error_up<error_sum && error_up<error_down){
pitch_change=palpha;
}else if(error_down<error_sum && error_down<error_up){
pitch_change=-palpha;
}else if(error_down!=error_sum&&error_sum!=error_up){
//palpha/=2;
}
error_up= calc_error(min_features,0,0,ralpha);
// printf("total up roll error is : %f\n",error_up);
error_down= calc_error(min_features,0,0,-ralpha);
// printf("total down roll error is : %f\n",error_down);
double roll_change=0;
if(error_up<error_sum && error_up<error_down){
roll_change=ralpha;
}else if(error_down<error_sum && error_down<error_up){
roll_change=-ralpha;
}else if(error_down!=error_sum&&error_sum!=error_up){
ralpha/=2;
}
// yaw+=yaw_change;
// pitch+=pitch_change;
// roll+=roll_change;
*/
try{
sensor_msgs::Image output_image_cvim =*bridge.cvToImgMsg(output_image, "mono8");
output_image_cvim.header.stamp=msg->header.stamp;
analyzed_pub_.publish(output_image_cvim);
}
catch (sensor_msgs::CvBridgeException& e){
ROS_ERROR("Could not convert from '%s' to 'mono8'.", msg->encoding.c_str());
return;
}
// printf("displaying image\n");
}else{
// printf("null image_rect\n");
}
}
void FeatureTracker::track_features(geometry_msgs::PoseStamped mapPose){
//set the initial number of features to the max number we want to find
int feature_count=num_features;
printf("pose %f %f %f\n",mapPose.pose.position.x, mapPose.pose.position.y, tf::getYaw(mapPose.pose.orientation));
int edge_pixels=5;
//check if there were features from the last image to keep tracking
if(last_feature_count>0){
//if there were call cvCalcOpticalFlowPyrLK();
//find matches between last good features and current image features
// store matches in featuresB
cvCalcOpticalFlowPyrLK(last_image,image_rect,pyrA,pyrB,features,featuresB, last_feature_count,cvSize(win_size,win_size) ,4,last_features_status,track_error, cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,.3),0);
}
printf("got image flow\n");
// assign last_feature_id values for matched features and set the non matched spots to -1
//find new features and subpixel them
//I SHOULD ADD THE IMAGE FLOW VALUES AS FEATURES NOW BEFORE FINDING NEW FEATURES
//find all good features
cvGoodFeaturesToTrack(image_rect, eigImage, tempImage, features, &feature_count, quality_level, min_distance, NULL, block_size);
//subpixel good features
cvFindCornerSubPix(image_rect,features,feature_count,cvSize(win_size,win_size),cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
printf("subpixeled image\n");
//for all the features in features B, find their matches in the newly found features
//add all the matches to their correct featuremanager, for the non matching, make a new
//feature manager and add them to it
//for all features by now we need their ray and the robot pose at that location
//draw dots on image where features are
//set the feature ids to a control value
for(int i=0;i<num_features;i++){
current_feature_id[i]=-1;
}
for(int i=0;i<last_feature_count;i++){
//for the previously found features in list b
if(last_features_status[i]>0){
for(int j=0;j<feature_count;j++){
//for every feature found in this image
//determine if the two overlap in a meaningful way
int xdiff=featuresB[i].x-features[j].x;
int ydiff=featuresB[i].y-features[j].y;
//if the pixels are within some margin of eachother
if(sqrt(xdiff*xdiff + ydiff*ydiff)<pixel_tracking_margin){
//if they do set the current id for j to the id of i
current_feature_id[j]=last_feature_id[i];
printf("feature found %d %d",last_feature_id[i],i);
}
}
}
}
printf("assigned IDs image\n");
for(int i=0;i<feature_count;i++){
printf("looping\n");
if(current_feature_id[i]>=0){
printf("prev feature match\n");
//if we matched a previous feature
//add our new feature to the previous features list
cv::Point3d tempRay;
cv::Point2d tempPoint=cv::Point2d(features[i]);
cam_model.projectPixelTo3dRay(tempPoint,tempRay);
if(tempPoint.x> edge_pixels && tempPoint.x < last_image->width- edge_pixels &&
tempPoint.y> edge_pixels && tempPoint.y<last_image->height- edge_pixels){
featureList[current_feature_id[i]].add(RawFeature(mapPose.pose.position.x, mapPose.pose.position.y, tf::getYaw(mapPose.pose.orientation), tempPoint,tempRay));
}else{
current_feature_id[i]=-1;
}
}else{
printf("new feature \n");
cv::Point3d tempRay;
cv::Point2d tempPoint=cv::Point2d(features[i]);
cam_model.projectPixelTo3dRay(tempPoint,tempRay);
if(tempPoint.x> edge_pixels && tempPoint.x < last_image->width- edge_pixels &&
tempPoint.y> edge_pixels && tempPoint.y<last_image->height- edge_pixels){
printf("new good feature \n");
//if we didn't
//create a new feature group in the list
current_feature_id[i]=feature_number;
//add the new feature to the feature list
featureList.push_back(FeatureManager());
featureList[feature_number].add(RawFeature(mapPose.pose.position.x, mapPose.pose.position.y, tf::getYaw(mapPose.pose.orientation), tempPoint,tempRay));
++feature_number;
}
}
}
// printf("features: ");
for(int i=0;i<num_features;i++){
if(i<feature_count){
last_feature_id[i]=current_feature_id[i];
}
else{
last_feature_id[i]=-1;
}
// printf(" %d ",current_feature_id[i]);
}
printf("\n");
last_feature_count=feature_count;
}
void imageCb(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg) {
MyFirst.clear();
Ccount1++;
cv_bridge::CvImageConstPtr cv_ptr;
double cur = ros::Time::now().toSec();
// lpf_x.set_cutoff_freq(0);
// lpf_x.get_lpf(10);
try
{
cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cam_model_left.fromCameraInfo(info_msg);
// cout << "dist " << info_msg->D[0] << " " << info_msg->D[1] << " " << info_msg->D[2] << " " << endl;
Mat image, gray, temp, mask;
image = cv_ptr->image;
mask = image.clone();
int niters = 1;
dilate(mask, temp, Mat(), Point(-1, -1), niters);
erode(temp, temp, Mat(), Point(-1, -1), niters * 2);
dilate(temp, temp, Mat(), Point(-1, -1), niters);
threshold(temp, temp, 200, 255, CV_THRESH_BINARY);
for (int i = 0; i <= 5; i++) {
for (int j = 1000; j < temp.cols; j++) {
temp.at<uchar>(i, j) = 0;
}
}
vector<vector<Point> > contours1, contours2;
vector<Vec4i> hierarchy;
findContours(temp, contours2, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
int cmin = 0;
int cmax = 100;
for (int i = 0; i < contours2.size(); i++) {
if (contours2[i].size() < cmin || contours2[i].size() > cmax) {
continue;
}
else
contours1.push_back(contours2[i]);
}
image = Scalar(255, 255, 255);
std::vector<std::vector<cv::Point> > ::iterator itc;
itc = contours1.begin();
MyPoint TTEMP;
while (itc != contours1.end()) {
cv::Moments mom = cv::moments(cv::Mat(*itc++));
#ifdef DEBUG
cv::circle(image, cv::Point(mom.m10 / mom.m00, mom.m01 / mom.m00), 2, cv::Scalar(0), 2);
#endif
TTEMP.x = mom.m10 / mom.m00;
TTEMP.y = mom.m01 / mom.m00;
MyFirst.push_back(TTEMP);
}
#ifdef DEBUG
cv::imshow(OPENCV_WINDOW, cv_ptr->image);
//Rect rect(MyFirst[0].x - 50, MyFirst[0].y- 50, 100, 100);
//Mat subimage = cv_ptr->image(rect);
//cv::imshow(OPENCV_WINDOW, subimage);
cv::waitKey(3);
#endif
vector<My3DPoint> myvec;
My3DPoint myTempVec;
#ifdef DEBUG
//cout << Ccount << " " << MyFirst[0].y << " " << MySecond[0].y << endl;
#endif
// cout << "size : " << MyFirst.size() << "," << MySecond.size() << endl;
if (MyFirst.size() && MySecond.size()) {
// cout << MyFirst[0].x << "," << MyFirst[0].y << endl;
cv::Point2d left_pt(MyFirst[0].x, MyFirst[0].y);
cv::Point2d right_pt(MySecond[0].x, MySecond[0].y);
cam_model_left.rectifyPoint(left_pt);
Matrix<float, 3, 1> Z;
Matrix<float, 6, 1> RES_KALMAN;
Z<<
MyFirst[0].x,
MySecond[0].x,
(MyFirst[0].y + MySecond[0].y) / 2.0f;
RES_KALMAN = kalman_xyz.getKalman(Z);
cout << "???" << endl;
cout << RES_KALMAN << endl;;
// cout << cam_model_left.rectifyPoint(left_pt).x << "," << cam_model_left.rectifyPoint(left_pt).y << "rectified" << endl;
// cout << cam_model_right.rectifyPoint(right_pt).x << "," << cam_model_right.rectifyPoint(right_pt).y << "rectified" << endl;
// cout << "notcal :: " << MyFirst[0].y - MySecond[0].y << endl;
// cout << "calibr :: " << cam_model_left.rectifyPoint(left_pt).y - cam_model_right.rectifyPoint(right_pt).y << endl;
float leftpt_y = MyFirst[0].y;
Matrix<float, 2, 1> X_kalman = kalman_x.getKalman(leftpt_y);
// cout << "KALMAN" << kalman_x.getKalman_1(leftpt_y) << "," << leftpt_y << endl;;
geometry_msgs::Point drone1_msg;
drone1_msg.x = leftpt_y;//get_lpf(&lpf_x,10);
drone1_msg.y = X_kalman(0, 0); //get_lpf(&lpf_z,10);
drone1_msg.z = X_kalman(1, 0); //get_lpf(&lpf_y,5);
pub_drone[0].publish(drone1_msg);
cv::Point3d ptr_left = cam_model_left.projectPixelTo3dRay(left_pt);
cv::Point3d ptr_right = cam_model_right.projectPixelTo3dRay(right_pt);
// getKalman_1();
// cout << ptr_left.y - ptr_right.y << endl;
// cout << "left 3d :: " << ptr_left.x << "," << ptr_left.y << "," << ptr_left.z << endl;
// cout << "right 3d :: " << ptr_right.x << "," << ptr_right.y << "," << ptr_right.z << endl << endl;
ptr_left = cam_model_left.projectPixelTo3dRay(cam_model_left.rectifyPoint(left_pt));
ptr_right = cam_model_right.projectPixelTo3dRay(cam_model_right.rectifyPoint(right_pt));
// cout << ptr_left.y - ptr_right.y << endl;
// cout << "left 3d :: " << ptr_left.x << "," << ptr_left.y << "," << ptr_left.z << endl;
// cout << "right 3d :: " << ptr_right.x << "," << ptr_right.y << "," << ptr_right.z << endl;
}
// cout << MySecond[0].x << "," << MySecond[0].y << endl;
if (Ccount == 1) {
sort(MyFirst.begin(), MyFirst.end(), MyCompare);
sort(MySecond.begin(), MySecond.end(), MyCompare);
Dron_size = MyFirst.size();
for (int i = 0; i < Dron_size; i++) {
double a1, b1, c1;
// cout << "hello " << MyFirst[i].x - MySecond[i].x << endl;
c1 = 2.97 * 450 / ((MyFirst[i].x - MySecond[i].x) * 0.00375);
a1 = (2.97 * 450 / ((MyFirst[i].x - MySecond[i].x) * 0.00375)) * (0.00375 * (MyFirst[i].x + MySecond[i].x - 1280)) / (2 * 2.97);
b1 = (-0.00375) * ((MyFirst[i].y + MySecond[i].y) / 2 - 480) * (2.97 * 450 / ((MyFirst[i].x - MySecond[i].x) * 0.00375)) / 2.97;
Current_Loc[i].x = a1;
Current_Loc[i].y = b1 * cos(Angle * PI / 180.0) - c1 * sin(Angle * PI / 180.0);
Current_Loc[i].z = b1 * sin(Angle * PI / 180.0) + c1 * cos(Angle * PI / 180.0);
a1 = Current_Loc[i].x;
b1 = Current_Loc[i].y;
c1 = Current_Loc[i].z;
Current_Loc[i].x = a1 * cos(Angle2 * PI / 180.0) - b1 * sin(Angle2 * PI / 180.0);
Current_Loc[i].y = a1 * sin(Angle2 * PI / 180.0) + b1 * cos(Angle2 * PI / 180.0);
Current_Loc[i].z = c1;
a1 = Current_Loc[i].x;
b1 = Current_Loc[i].y;
c1 = Current_Loc[i].z;
Current_Loc[i].x = a1 * cos(Angle3 * PI / 180.0) + c1 * sin(Angle3 * PI / 180.0);
Current_Loc[i].y = b1;
Current_Loc[i].z = -a1 * sin(Angle3 * PI / 180.0) + c1 * cos(Angle3 * PI / 180.0);
}
for (int i = 0; i < Dron_size; i++) {
Real_Dron_Loc[i].x = (Current_Loc[i].x);
Real_Dron_Loc[i].y = (Current_Loc[i].y);
Real_Dron_Loc[i].z = (Current_Loc[i].z);
}
Frame_S_time = clock();
for (int i = 0; i < Dron_size; i++) {
Velocity[i].x = 0;
Velocity[i].y = 0;
Velocity[i].z = 0;
}
}
else {
if (MyFirst.size() != Dron_size || MySecond.size() != Dron_size)
return;
Frame_E_time = clock();
Time = (double)(Frame_E_time - Frame_S_time) / CLOCKS_PER_SEC;
Frame_S_time = Frame_E_time;
for (int i = 0; i < Dron_size; i++) {
Acummulate_Time[i] += Time;
Track_Loc[i].x = Current_Loc[i].x;// + Acummulate_Time[i] * Velocity[i].x;
Track_Loc[i].y = Current_Loc[i].y;// + Acummulate_Time[i] * Velocity[i].y;
Track_Loc[i].z = Current_Loc[i].z;// + Acummulate_Time[i] * Velocity[i].z;
}
for (int k = 0; k < Dron_size; k++) {
double closest_dis = 987654321;
My3DPoint closest_3DPoint;
closest_3DPoint.x = 0;
closest_3DPoint.y = 0;
closest_3DPoint.z = 0;
int flag = 0;
int AA = 999, BB = 888;
for (int i = 0; i < MyFirst.size(); i++) {
for (int j = 0; j < MySecond.size(); j++) {
double a1, b1, c1;
double aa1, bb1, cc1;
cout << "depth : " << MyFirst[i].x - MySecond[j].x << endl;
c1 = 2.97 * 450.0 / ((MyFirst[i].x - MySecond[j].x) * 0.00375);
a1 = (2.97 * 450.0 / ((MyFirst[i].x - MySecond[j].x) * 0.00375)) * (0.00375 * (MyFirst[i].x + MySecond[j].x - 1280.0)) / (2.0 * 2.97);
b1 = (-0.00375) * ((MyFirst[i].y + MySecond[j].y) / 2 - 480.0) * (2.97 * 450.0 / ((MyFirst[i].x - MySecond[j].x) * 0.00375)) / 2.97;
aa1 = a1;
bb1 = b1 * cos(Angle * PI / 180.0) - c1 * sin(Angle * PI / 180.0);
cc1 = b1 * sin(Angle * PI / 180.0) + c1 * cos(Angle * PI / 180.0);
a1 = aa1;
b1 = bb1;
c1 = cc1;
aa1 = a1 * cos(Angle2 * PI / 180.0) - b1 * sin(Angle2 * PI / 180.0);
bb1 = a1 * sin(Angle2 * PI / 180.0) + b1 * cos(Angle2 * PI / 180.0);
cc1 = c1;
a1 = aa1;
b1 = bb1;
c1 = cc1;
aa1 = a1 * cos(Angle3 * PI / 180.0) + c1 * sin(Angle3 * PI / 180.0);
bb1 = b1;
cc1 = -a1 * sin(Angle3 * PI / 180.0) + c1 * cos(Angle3 * PI / 180.0);
if ((sqrt(pow(Track_Loc[k].x - aa1, 2) + pow(Track_Loc[k].y - bb1, 2) + pow(Track_Loc[k].z - cc1, 2)) < closest_dis) && sqrt(pow(Track_Loc[k].x - aa1, 2) + pow(Track_Loc[k].y - bb1, 2) + pow(Track_Loc[k].z - cc1, 2)) < 300 ) {
closest_dis = sqrt(pow(Track_Loc[k].x - aa1, 2) + pow(Track_Loc[k].y - bb1, 2) + pow(Track_Loc[k].z - cc1, 2));
closest_3DPoint.x = aa1;
closest_3DPoint.y = bb1;
closest_3DPoint.z = cc1;
AA = i;
BB = j;
}
}
}
#ifdef DEBUG
cout << MyFirst.size() << " " << MySecond.size() << endl;
cout << AA << " " << BB << endl;
cout << closest_dis << endl;
cout << closest_3DPoint.x << " " << closest_3DPoint.y << " " << closest_3DPoint.z << endl;
#endif
if (closest_dis == 987654321) {
continue;
}
else {
// pass
Acummulate_Time[k] = 0;
Current_Loc[k].x = closest_3DPoint.x;
Current_Loc[k].y = closest_3DPoint.y;
Current_Loc[k].z = closest_3DPoint.z;
}
cout << "Curren " << Current_Loc[k].x << " " << Current_Loc[k].y << " " << Current_Loc[k].z << endl;
cout << "-------------------------------------------------------------------------------" << endl << endl;
}
std_msgs::Float64 drone1_x_msg;
std_msgs::Float64 drone1_y_msg;
std_msgs::Float64 drone1_z_msg;
geometry_msgs::Point drone1_msg;
geometry_msgs::Point drone2_msg;
for (int i = 0; i < Dron_size; i++) {
Real_Dron_Loc[i].x = Current_Loc[i].x;
Real_Dron_Loc[i].y = Current_Loc[i].y;
Real_Dron_Loc[i].z = Current_Loc[i].z;
#ifdef DEBUG
cout << setw(10) << Real_Dron_Loc[i].x << " " << setw(10) << Real_Dron_Loc[i].y << " " << setw(10) << Real_Dron_Loc[i].z << endl;
#endif
static lpf_t lpf_x = {0, };
static lpf_t lpf_y = {0, };
static lpf_t lpf_z = {0, };
lpf_x.cur_time = lpf_y.cur_time = lpf_z.cur_time = ros::Time::now().toSec();
lpf_x.input = Real_Dron_Loc[i].x;
lpf_y.input = Real_Dron_Loc[i].y;
lpf_z.input = Real_Dron_Loc[i].z;
drone1_msg.x = Real_Dron_Loc[i].x;//get_lpf(&lpf_x,10);
drone1_msg.y = Real_Dron_Loc[i].z;//get_lpf(&lpf_z,10);
drone1_msg.z = Real_Dron_Loc[i].y;//get_lpf(&lpf_y,5);
drone2_msg.x = get_lpf(&lpf_x, 10);
drone2_msg.y = get_lpf(&lpf_z, 10);
drone2_msg.z = get_lpf(&lpf_y, 5);
// pub_drone[i].publish(drone1_msg);
// Nasang[i].publish(drone2_msg);
}
}
cout << "cb1 cycletime : " << ros::Time::now().toSec() - cur << endl;;
}
void depthCallback(const sensor_msgs::ImageConstPtr& original_image, const sensor_msgs::CameraInfoConstPtr& info){
if(need_for_wall==2){
need_for_wall = 0;
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(original_image, enc::TYPE_16UC1);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("tutorialROSOpenCV::main.cpp::cv_bridge exception: %s", e.what());
return;
}
cam_model.fromCameraInfo(info);
Mat depth_image = cv_ptr->image;
float x = 0;
float y = 360;
float wall_height = 0;
geometry_msgs::PointStamped xy_point_stamped, pipe_point_stamped;
walls_detection::Walls walls_all_message;
for(int i = 100; i<600; i+=110){
wall_height = 0;
y = 360;
x = i;
unsigned short wall_depth;
while(wall_height < 0.05 && y > 0){
cv::Point2d uv_point(x,y);
unsigned short wall_depth;
wall_depth = depth_image.at<unsigned short>(y, x);
cv::Point3d xy_point;
xy_point = cam_model.projectPixelTo3dRay(uv_point);
xy_point = xy_point * wall_depth;
xy_point_stamped.header.frame_id = "/camera_rgb_optical_frame";
xy_point_stamped.header.stamp = ros::Time::now();
xy_point_stamped.point.x = 0.001*xy_point.x;
xy_point_stamped.point.y = 0.001*xy_point.y;
xy_point_stamped.point.z = 0.001*xy_point.z;
try {
tf_listener->waitForTransform("/pipe_link", "/camera_rgb_optical_frame", ros::Time::now(), ros::Duration(10.0) );
tf_listener->transformPoint("/pipe_link", xy_point_stamped, pipe_point_stamped);
}
catch (tf::TransformException ex) {
ROS_ERROR("%s",ex.what());
}
wall_height = pipe_point_stamped.point.z;
y = y - 120;
}
std::cout<<"Sciana nr "<< i <<" - x: "<< pipe_point_stamped.point.x <<", y: "<< pipe_point_stamped.point.y <<", z: "<< pipe_point_stamped.point.z <<"\n";
if(wall_height < 0.05){
std::cout << "Nie ma sciany\n";
pipe_point_stamped.point.x = 0;
pipe_point_stamped.point.y = 0;
pipe_point_stamped.point.z = 0;
}
switch(i){
case 100:
walls_all_message.wall1 = pipe_point_stamped;
break;
case 210:
walls_all_message.wall2 = pipe_point_stamped;
break;
case 320:
walls_all_message.wall3 = pipe_point_stamped;
break;
case 430:
walls_all_message.wall4 = pipe_point_stamped;
break;
case 540:
walls_all_message.wall5 = pipe_point_stamped;
break;
}
}
walls_all.publish(walls_all_message);
}
}
void depthCallback(const sensor_msgs::ImageConstPtr& original_image, const sensor_msgs::CameraInfoConstPtr& info)
{
if(is_robot_running==0 && balls_written==1 && ball_chosen==0){
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(original_image, sensor_msgs::image_encodings::TYPE_16UC1);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("tutorialROSOpenCV::main.cpp::cv_bridge exception: %s", e.what());
return;
}
cam_model.fromCameraInfo(info);
Mat depth_image = cv_ptr->image;
if(balls_written == 1){
for( size_t i = 0; i < circles_all.size(); i++ ){
if (pilki[i]!=0){
cv::Point2d uv_point(circles_all[i][0], circles_all[i][1]);
unsigned short ball_depth;
ball_depth = depth_image.at<unsigned short>(circles_all[i][1], circles_all[i][0])+20;
cv::Point3d xy_point;
xy_point = cam_model.projectPixelTo3dRay(uv_point);
xy_point = xy_point * ball_depth;
geometry_msgs::PointStamped xy_point_stamped, odom_point_stamped;
xy_point_stamped.header.frame_id = "/camera_rgb_optical_frame";
xy_point_stamped.header.stamp = ros::Time::now();
xy_point_stamped.point.x = 0.001*xy_point.x;
xy_point_stamped.point.y = 0.001*xy_point.y;
xy_point_stamped.point.z = 0.001*xy_point.z;
try {
tf_listener->waitForTransform("/base_link", "/camera_rgb_optical_frame", ros::Time::now(), ros::Duration(10.0) );
tf_listener->transformPoint("/base_link", xy_point_stamped, odom_point_stamped);
}
catch (tf::TransformException ex) {
ROS_ERROR("%s",ex.what());
}
if(odom_point_stamped.point.z > 0.05 || odom_point_stamped.point.z < 0.001){
std::cout << "Srodek pilki na wysokosci: " << odom_point_stamped.point.z << ".\n";
std::cout << "Skondensowane pilki: to nie pilka, jest za wysoko albo za nisko! \n";
pilki[i]=0;
}
}
}
depth_written = 1;
std::cout << "Policzylem wysokosc dla skondensowanych pilek \n";
int closest_ball = choose_closest_ball();
if(closest_ball == -1){
std::cout << "NIE WYBRANO ZADNEJ PILKI \n";
send_no_balls();
}
if(closest_ball != -1){
int wybrana_x = circles_all[closest_ball][0];
int wybrana_y = circles_all[closest_ball][1];
int wybrana_z = circles_all[closest_ball][2];
ball_chosen = 1;
std::cout << "Wybrana jedna pilka \n";
last_circle[0] = wybrana_x;
last_circle[1] = wybrana_y;
last_circle[2] = wybrana_z;
wybrana.x = last_circle[0];
wybrana.y = last_circle[1];
wybrana.z = last_circle[2];
draw_balls = 1;
cv::Point2d uv_point(wybrana.x, wybrana.y);
unsigned short ball_depth;
ball_depth = depth_image.at<unsigned short>(wybrana.y,wybrana.x)+20;
geometry_msgs::Point message_selected;
cv::Point3d xy_point;
xy_point = cam_model.projectPixelTo3dRay(uv_point);
xy_point = xy_point * ball_depth;
geometry_msgs::PointStamped xy_point_stamped, odom_point_stamped;
xy_point_stamped.header.frame_id = "/camera_rgb_optical_frame";
xy_point_stamped.header.stamp = ros::Time::now();
xy_point_stamped.point.x = 0.001*xy_point.x;
xy_point_stamped.point.y = 0.001*xy_point.y;
xy_point_stamped.point.z = 0.001*xy_point.z;
try {
tf_listener->waitForTransform("/odom", "/camera_rgb_optical_frame", ros::Time::now(), ros::Duration(10.0) );
tf_listener->transformPoint("/odom", xy_point_stamped, odom_point_stamped);
}
catch (tf::TransformException ex) {
ROS_ERROR("%s",ex.what());
}
if(odom_point_stamped.point.z > 0.05){
std::cout << "Wybrana pilka to nie pilka, jest za wysoko! \n";
}
std::cout << "Policzylem dla wybranej pilki i publikuje \n";
//balls.publish(wybrana);
selected_ball.publish(odom_point_stamped);
licznik_depth = 0;
ball_chosen = 0;
circles_all.clear();
pilki.clear();
licznik_wybrane = 0;
no_balls_counter = 0;
balls_written = 0;
depth_written = 0;
is_robot_running = 1;
}
}
}
}
void locator()
{
namedWindow("Tracking");
int hMin, hMax, sMin, sMax, vMin, vMax,area_min;
hMin = 0;
//hMax = 124; // night values/???
hMax = 255;
//sMin = 95;
sMin = 126;
sMax = 255;
//vMin = 139;
vMin = 173;
vMax = 255;
area_min = 100;
Mat smoothed, hsvImg, t_img;
createTrackbar("blob min area","Tracking" ,&area_min ,1000);
createTrackbar("Hue Min", "Tracking", &hMin, 255);
createTrackbar("Hue Max", "Tracking", &hMax, 255);
createTrackbar("Sat Min", "Tracking", &sMin, 255);
createTrackbar("Sat Max", "Tracking", &sMax, 255);
createTrackbar("Val Min", "Tracking", &vMin, 255);
createTrackbar("Val MaX", "Tracking", &vMax, 255);
while(ros::ok())
{
Mat source = imageB;
Mat copy = imageB.clone();
GaussianBlur(source, smoothed, Size(9,9), 4);
cvtColor(smoothed, hsvImg, CV_BGR2HSV);
inRange(hsvImg, Scalar(hMin, sMin, vMin), Scalar(hMax, sMax, vMax), t_img);
CBlobResult blob;
IplImage i_img = t_img;
blob = CBlobResult(&i_img,NULL,0);
int num_blobs = blob.GetNumBlobs();
blob.Filter(blob, B_INCLUDE, CBlobGetArea(), B_INSIDE, area_min, blob_area_absolute_max_);
num_blobs = blob.GetNumBlobs();
std::string reference_frame = "/virtual_table"; // Table frame at ball_radius above the actual table plane
tf::StampedTransform transform;
tf_.waitForTransform(reference_frame, model.tfFrame(), ros::Time(0), ros::Duration(0.5));
tf_.lookupTransform(reference_frame, model.tfFrame(), ros::Time(0), transform);
for(int i =0;i<num_blobs;i++)
{
CBlob* bl = blob.GetBlob(i);
Point2d uv(CBlobGetXCenter()(*bl), CBlobGetYCenter()(*bl));
//Use the width as the height
uv.y = bl->MinY() + (bl->MaxX() - bl->MinX()) * 0.5;
circle(copy,uv,50,Scalar(255,0,0),5);
cv::Point3d xyz;
model.projectPixelTo3dRay(uv, xyz);
// Intersect ray with plane in virtual table frame
//Origin of camera frame wrt virtual table frame
tf::Point P0 = transform.getOrigin();
//Point at end of unit ray wrt virtual table frame
tf::Point P1 = transform * tf::Point(xyz.x, xyz.y, xyz.z);
// Origin of virtual table frame
tf::Point V0 = tf::Point(0.0,0.0,0.0);
// normal to the table plane
tf::Vector3 n(0, 0, 1);
// finding scaling value
double scale = (n.dot(V0-P0))/(n.dot(P1-P0));
tf::Point ball_pos = P0 + (P1-P0)*scale;
cout <<ball_pos.x() << " " << ball_pos.y() << " " << ball_pos.z() <<endl;
}
imshow(WINDOW, copy);
waitKey(3);
imshow("edited", t_img);
waitKey(3);
ros::spinOnce();
}
}