CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 21:19:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

aruco_map: align marker map by z axis

Browse Source
This commit is contained in:
Oleg Kalachev
2019-02-28 23:33:05 +03:00
parent b2a53e5872
commit db9d3cb398
1 changed files with 8 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
aruco_pose/src/aruco_map.cpp
View File

@@ -174,8 +174,8 @@ public:
cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
if (obj_points.empty()) goto publish_debug;
double center_x = 0, center_y = 0;
alignObjPointsToCenter(obj_points, center_x, center_y);
double center_x = 0, center_y = 0, center_z = 0;
alignObjPointsToCenter(obj_points, center_x, center_y, center_z);
valid = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false);
if (!valid) goto publish_debug;
@@ -192,6 +192,7 @@ public:
geometry_msgs::TransformStamped shift;
shift.transform.translation.x = -center_x;
shift.transform.translation.y = -center_y;
shift.transform.translation.z = -center_z;
shift.transform.rotation.w = 1;
tf2::doTransform(shift, transform_, transform_);
@@ -228,23 +229,27 @@ publish_debug:
}
// TODO consider z
void alignObjPointsToCenter(Mat &obj_points, double &center_x, double &center_y) const
void alignObjPointsToCenter(Mat &obj_points, double &center_x, double &center_y, double &center_z) const
{
// Align object points to the center of mass
double sum_x = 0;
double sum_y = 0;
double sum_z = 0;
for (int i = 0; i < obj_points.rows; i++) {
sum_x += obj_points.at<float>(i, 0);
sum_y += obj_points.at<float>(i, 1);
sum_z += obj_points.at<float>(i, 2);
}
center_x = sum_x / obj_points.rows;
center_y = sum_y / obj_points.rows;
center_z = sum_z / obj_points.rows;
for (int i = 0; i < obj_points.rows; i++) {
obj_points.at<float>(i, 0) -= center_x;
obj_points.at<float>(i, 1) -= center_y;
obj_points.at<float>(i, 2) -= center_z;
}
}