void makeSendDataDetectedObj(vector<OBJPOS> car_position_vector, vector<OBJPOS>::iterator cp_iterator, LOCATION mloc, ANGLE angle, cv_tracker::obj_label& send_data) { geometry_msgs::Point tmpPoint; for(uint i=0; i<car_position_vector.size() ; i++, cp_iterator++){ //middle of right-lower and left-upper double U = cp_iterator->x1 + cp_iterator->x2/2; double V = cp_iterator->y1 + cp_iterator->y2/2; //convert from "image" coordinate system to "camera" coordinate system ol.setOriginalValue(U,V,cp_iterator->distance); LOCATION ress = ol.cal(); /* convert from "camera" coordinate system to "map" coordinate system */ tf::Vector3 pos_in_camera_coord(ress.X, ress.Y, ress.Z); tf::Vector3 converted = transformCam2Map * pos_in_camera_coord; tmpPoint.x = converted.x(); tmpPoint.y = converted.y(); tmpPoint.z = converted.z(); send_data.reprojected_pos.push_back(tmpPoint); } }
void makeSendDataDetectedObj(vector<OBJPOS> car_position_vector, vector<OBJPOS>::iterator cp_iterator, autoware_msgs::obj_label& send_data) { geometry_msgs::Point tmpPoint; for(uint i=0; i<car_position_vector.size() ; i++, cp_iterator++){ //middle of right-lower and left-upper double U = cp_iterator->x1 + cp_iterator->x2/2; double V = cp_iterator->y1 + cp_iterator->y2/2; //convert from "image" coordinate system to "camera" coordinate system ol.setOriginalValue(U,V,cp_iterator->distance); LOCATION ress = ol.cal(); /* convert from "camera" coordinate system to "map" coordinate system */ tf::Vector3 pos_in_camera_coord(ress.X, ress.Y, ress.Z); static tf::TransformListener listener; try { listener.lookupTransform("map", camera_id_str, ros::Time(0), transformCam2Map); } catch (tf::TransformException ex) { ROS_INFO("%s", ex.what()); return; } tf::Vector3 converted = transformCam2Map * pos_in_camera_coord; tmpPoint.x = converted.x(); tmpPoint.y = converted.y(); tmpPoint.z = converted.z(); send_data.reprojected_pos.push_back(tmpPoint); send_data.obj_id.push_back(cp_iterator->id); } }