|
|
楼主|
发表于 2020-1-19 16:02:12
本帖最后由 hwang 于 2020-1-19 16:42 编辑
我这边使用二维码检测定位在gazebo进行了仿真实验，输出的数据抖动太厉害无人机下降过程一直在乱晃，无法实现着落，我把代码贴到下边，麻烦大佬看一下是哪里有问题，能不能给点优化思路，拜托。
//-----------------利用Euler角进行三次旋转得到无人机相对目标的位置------------------
void CodeRotateByZ(double x, double y, double thetaz, double &outx, double &outy)
{
double x1 = x; //将变量拷贝一次，保证&x == &outx这种情况下也能计算正确
double y1 = y;
double rz = thetaz * CV_PI / 180;
outx = cos(rz) * x1 - sin(rz) * y1;
outy = sin(rz) * x1 + cos(rz) * y1;
}
void CodeRotateByY(double x, double z, double thetay, double &outx, double &outz)
{
double x1 = x;
double z1 = z;
double ry = thetay * CV_PI / 180;
outx = cos(ry) * x1 + sin(ry) * z1;
outz = cos(ry) * z1 - sin(ry) * x1;
}
void CodeRotateByX(double y, double z, double thetax, double &outy, double &outz)
{
double y1 = y; //将变量拷贝一次，保证&y == &y这种情况下也能计算正确
double z1 = z;
double rx = thetax * CV_PI / 180;
outy = cos(rx) * y1 - sin(rx) * z1;
outz = cos(rx) * z1 + sin(rx) * y1;
}
int main(int argc, char **argv)
{
//Initiate ROS
ros::init(argc, argv, "pose_estimate_one");
ros::NodeHandle nh_;
ros::Rate loop_rate(30);
precision_landing = nh_.advertise<urtrans::PrecisionLanding>("/precision_landing", 10);
cv::Mat camera_matrix;
cv::Mat distortion_coefficients;
//相机内参
camera_matrix = cv::Mat(3, 3, CV_64FC1, cv::Scalar::all(0)); //初始化矩阵的头,设置元素初始值
camera_matrix.ptr<double>(0)[0] = 582.611780;
camera_matrix.ptr<double>(0)[2] = 355.598968;
camera_matrix.ptr<double>(1)[1] = 582.283970;
camera_matrix.ptr<double>(1)[2] = 259.508932;
camera_matrix.ptr<double>(2)[2] = 1.0f;
//相机畸变参数
distortion_coefficients = cv::Mat(5, 1, CV_64FC1, cv::Scalar::all(0));
distortion_coefficients.ptr<double>(0)[0] = -0.409105;
distortion_coefficients.ptr<double>(1)[0] = 0.200162;
distortion_coefficients.ptr<double>(2)[0] = 0.003778;
distortion_coefficients.ptr<double>(3)[0] = 0.000588;
distortion_coefficients.ptr<double>(4)[0] = 0.0;
//ArUco Marker字典选择以及旋转向量和平移向量初始化
Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(7); //创建字典对象
vector<double> rv(3), tv(3);
cv::Mat rvec(rv), tvec(tv);
VehicleState::SmartSensor smart_sensor_;
VehicleStateSet::SetpointPosition set_position_;
cv::Mat image;
sensor_msgs::ImagePtr msg_ellipse;
sensor_msgs::Image msg;
urtrans::PrecisionLanding pre_land;
while (ros::ok())
{
msg = smart_sensor_.get_down_cam_img();
if (msg.width == 0)
{
ROS_INFO_STREAM("get_fro_cam_img() is null");
}
else
{
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
image = cv_ptr->image;
std::vector<int> markerids;
vector<vector<Point2f>> markerCorners, rejectedCandidate;
Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();
cv::aruco::detectMarkers(image, dictionary, markerCorners, markerids, parameters, rejectedCandidate); //检测函数，返回检测出的markers
//待检测图像，字典对象，检测出的图像的角的列表，检测出的所有marker的id列表，检测阶段的所有参数，返回所有marker候选。
if (markerids.size() > 0) //如果检测到marker，下一步多marker位置进行求解
{
cv::Mat RoteM, TransM;
cv::Point3f Theta_C2W;
cv::Point3f Theta_W2C;
cv::Point3f Position_OcInW;
cv::aruco::estimatePoseSingleMarkers(markerCorners, 1, camera_matrix, distortion_coefficients, rvec, tvec); //检测所有探测到的marker的pose
//marke的角向量，二维码的大小（m）相机内参，相机畸变参数，marker的旋转量，平移量
double rm[9];
RoteM = cv::Mat(3, 3, CV_64FC1, rm);
Rodrigues(rvec, RoteM); //罗德里旋转得到旋转矩阵
double r11 = RoteM.ptr<double>(0)[0];
double r12 = RoteM.ptr<double>(0)[1];
double r13 = RoteM.ptr<double>(0)[2];
double r21 = RoteM.ptr<double>(1)[0];
double r22 = RoteM.ptr<double>(1)[1];
double r23 = RoteM.ptr<double>(1)[2];
double r31 = RoteM.ptr<double>(2)[0];
double r32 = RoteM.ptr<double>(2)[1];
double r33 = RoteM.ptr<double>(2)[2];
TransM = tvec; //平移向量
double thetaz = atan2(r21, r11) / CV_PI * 180;
double thetay = atan2(-1 * r31, sqrt(r32 * r32 + r33 * r33)) / CV_PI * 180;
double thetax = atan2(r32, r33) / CV_PI * 180;
Theta_C2W.z = thetaz;
Theta_C2W.y = thetay;
Theta_C2W.x = thetax;
Theta_W2C.x = -1 * thetax;
Theta_W2C.y = -1 * thetay;
Theta_W2C.z = -1 * thetaz;
double tx = tvec.ptr<double>(0)[0];
double ty = tvec.ptr<double>(0)[1];
double tz = tvec.ptr<double>(0)[2];
double x = tx, y = ty, z = tz;
//ZYX axis rotate
CodeRotateByZ(x, y, -1 * thetaz, x, y);
CodeRotateByY(x, z, -1 * thetay, x, z);
CodeRotateByX(y, z, -1 * thetax, y, z);
Position_OcInW.x = x * -1;
Position_OcInW.y = y * -1;
Position_OcInW.z = z * -1;
//计算偏航角之差
Eigen::Matrix3d rotateMatrix;
rotateMatrix << r11, r12, r13, r21, r22, r23, r31, r32, r33;
Eigen::Vector3d eulerVec;
eulerVec(0) = (Theta_C2W.z + 90) / 180 * CV_PI;
//设置是否检测到标志的flage
distinguish_flag = true;
pre_land.is_located = distinguish_flag;
// pre_land.des_dx_body = Position_OcInW.x;
// pre_land.des_dy_body = Position_OcInW.y;
// pre_land.des_dz_body = Position_OcInW.z;
pre_land.des_dx = Position_OcInW.x;
pre_land.des_dy = Position_OcInW.y;
pre_land.des_dz = Position_OcInW.z;
pre_land.locate_precision = 1;
//set_position_.set_position(Position_OcInW.x, Position_OcInW.y, Position_OcInW.z, eulerVec(0));
cout << "pos_target: [X Y Z] : "
<< " " << Position_OcInW.x << " [m] " << Position_OcInW.y << " [m] " << Position_OcInW.z << " [m] " << endl;
}
else
{
distinguish_flag = false;
pre_land.is_located = distinguish_flag;
// pre_land.des_dx_body = 0;
// pre_land.des_dy_body = 0;
// pre_land.des_dz_body = 0;
pre_land.des_dx = 0;
pre_land.des_dy = 0;
pre_land.des_dz = 0;
pre_land.locate_precision = 0;
//set_position_.set_position(0, 0, 0, 0);
// cout << "can not detected qrcode!" << endl;
}
precision_landing.publish(pre_land);
Mat img_copy;
image.copyTo(img_copy);
cv::aruco::drawDetectedMarkers(img_copy, markerCorners, markerids); //绘制检测出来的图像
//cv::aruco::drawAxis(img_copy, camera_matrix, distortion_coefficients, rvec, tvec, 0.1);
cv::imshow("pose_estimate_one", img_copy);
cv::waitKey(1);
}
ros::spinOnce();
loop_rate.sleep();
}
}
|
|
发表于 2020-1-10 18:11:55
33536 浏览 18 回复
