Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Implemented calibration bundle in ROS+CPP #74

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Implemented calibration bundle in ROS+CPP #74

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions apriltag_ros/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,10 @@ target_link_libraries(${PROJECT_NAME}_continuous_detector ${PROJECT_NAME}_common
add_library(${PROJECT_NAME}_single_image_detector src/single_image_detector.cpp)
target_link_libraries(${PROJECT_NAME}_single_image_detector ${PROJECT_NAME}_common ${catkin_LIBRARIES})

add_executable(${PROJECT_NAME}_calibration_bundle src/${PROJECT_NAME}_calibration_bundle.cpp)
add_dependencies(${PROJECT_NAME}_calibration_bundle ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(${PROJECT_NAME}_calibration_bundle ${catkin_LIBRARIES})

add_executable(${PROJECT_NAME}_continuous_node src/${PROJECT_NAME}_continuous_node.cpp)
add_dependencies(${PROJECT_NAME}_continuous_node ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(${PROJECT_NAME}_continuous_node ${PROJECT_NAME}_continuous_detector ${catkin_LIBRARIES})
Expand All @@ -120,6 +124,8 @@ add_dependencies(${PROJECT_NAME}_single_image_client_node ${PROJECT_NAME}_genera
target_link_libraries(${PROJECT_NAME}_single_image_client_node ${PROJECT_NAME}_common ${catkin_LIBRARIES})




#############
## Install ##
#############
Expand All @@ -143,6 +149,7 @@ install(TARGETS
${PROJECT_NAME}_single_image_client_node
${PROJECT_NAME}_single_image_detector
${PROJECT_NAME}_single_image_server_node
${PROJECT_NAME}_calibration_bundle
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
Expand Down
5 changes: 5 additions & 0 deletions apriltag_ros/launch/calibration_bundle.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
<launch>
<node pkg="apriltag_ros" type="apriltag_ros_calibration_bundle" name="calibration_bundle" clear_params="true" output="screen">
<param name="input_frame_count" type="int" value="500" />
</node>
</launch>
204 changes: 204 additions & 0 deletions apriltag_ros/src/apriltag_ros_calibration_bundle.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,204 @@
#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <apriltag_ros/AprilTagDetectionArray.h>
#include <apriltag_ros/AprilTagDetection.h>
#include <Eigen/Dense>

using namespace apriltag_ros;
#define MASTER_ID 0

int g_frame_cnt = 0;
int input_frame_count = 0;

std::vector<apriltag_ros::AprilTagDetectionArray> detection_array_deque;

Eigen::Matrix3d quat2Rotmap(const geometry_msgs::Quaternion &ros_q)
{
Eigen::Quaterniond q(ros_q.w, ros_q.x, ros_q.y, ros_q.z);
return q.matrix();
}

Eigen::Matrix4d createTransform(const geometry_msgs::PoseWithCovarianceStamped &pc)
{
Eigen::Affine3d transform = Eigen::Affine3d::Identity();
Eigen::Vector3d t(pc.pose.pose.position.x, pc.pose.pose.position.y, pc.pose.pose.position.z);
Eigen::Quaterniond q(pc.pose.pose.orientation.w,
pc.pose.pose.orientation.x,
pc.pose.pose.orientation.y,
pc.pose.pose.orientation.z);
transform.rotate(q);
transform.pretranslate(t);
return transform.matrix();
}

void tagDetectionCallback(const apriltag_ros::AprilTagDetectionArrayConstPtr &detection_array)
{
// Use 500 frame detections to generate a calibration result
if (g_frame_cnt++ > input_frame_count)
{
double master_size;
std::map<int, std::vector<Eigen::Matrix4d>> rel_mats; // 某帧某个tag
for (const AprilTagDetectionArray &detections : detection_array_deque)
{
auto it = std::find_if(detections.detections.begin(), detections.detections.end(), [&](const AprilTagDetection &d) {
return d.id.front() == MASTER_ID;
});
// Master not detected in this detection, so this particular
// detection is useless
if (it == detections.detections.end())
continue;

// Get the master tag's rigid body transform to the camera frame
Eigen::Matrix4d T_cm = createTransform(it->pose);
master_size = it->size.front();

// Get the rigid body transform of every other tag to the camera frame
std::vector<Eigen::Matrix4d> mats;
for (const auto &detection : detections.detections)
{
// Skip the master, but get its size first
if (detection.id.front() == MASTER_ID)
continue;
//We already have the rigid body transform from the master tag to the camera frame (T_cm)
int other_id = detection.id.front();

//Get this tag's rigid body transform to the camera frame
Eigen::Matrix4d other_T_cm = createTransform(detection.pose);

// Deduce this tag's rigid body transform to the master tag's frame
auto tt = T_cm.inverse();
Eigen::Matrix4d T_m = T_cm.inverse() * other_T_cm;

// Save the relative position and orientation of this tag to the master tag
rel_mats[detection.id.front()].push_back(T_m);
}
}

// Compute the mean position as the initial value for the minimization problem
auto meanTranspose = [&](const std::vector<Eigen::Matrix4d> &mats) -> Eigen::Vector3d {
double mean_x(0), mean_y(0), mean_z(0);
for (const auto &mat : mats)
{
Eigen::Affine3d a3d(mat);
mean_x += a3d.translation().x();
mean_y += a3d.translation().y();
mean_z += a3d.translation().z();
}
mean_x /= mats.size();
mean_y /= mats.size();
mean_z /= mats.size();
return Eigen::Vector3d(mean_x, mean_y, mean_z);
};

auto quatmult = [&](const Eigen::Quaterniond &a, const Eigen::Quaterniond &b) -> Eigen::Quaterniond {
double w = a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z();
double x = a.w() * b.x() + a.x() * b.w() - a.y() * b.z() + a.z() * b.y();
double y = a.w() * b.y() + a.y() * b.w() + a.x() * b.z() - a.z() * b.x();
double z = a.w() * b.z() - a.x() * b.y() + a.y() * b.x() + a.z() * b.w();
return Eigen::Quaterniond(w, x, y, z);
};

auto meanQuaternions = [&](const std::vector<Eigen::Matrix4d> &mats) -> Eigen::Quaterniond {
double mean_x(0), mean_y(0), mean_z(0), mean_w(0);

for (int i = 0; i < mats.size(); ++i)
{
Eigen::Affine3d a4d(mats[i]);
Eigen::Quaterniond q1(Eigen::Affine3d(mats[i]).rotation());
for (int j = 0; j < mats.size(); ++j)
{
if (i == j)
continue;

Eigen::Quaterniond q2(Eigen::Affine3d(mats[j]).rotation());
auto q_error = quatmult(q1.inverse(), q2);
double q_error_w = std::min<double>(1.0, std::max<double>(q_error.w(), -1.0));
if (2 * std::acos(q_error_w) >= M_PI / 2.0)
printf("Quaternion pair q_%d and q_%d are more than 90 degrees apart!", i, j);
}
}
};

// Compute the geometric median
std::map<int, Eigen::Affine3d> rel_mats_median;
for (const auto &ms : rel_mats)
{
// avereage transpose
auto p_0 = meanTranspose(ms.second);
// TODO In matlab code, it applied a optimization search algorithm to search a result
// In this version, use mean result only
rel_mats_median[ms.first] = Eigen::Affine3d::Identity();
rel_mats_median[ms.first].pretranslate(p_0);
}

// Compute the quaternion median
for (const auto &ms : rel_mats)
{
auto q_0 = meanQuaternions(ms.second);
Eigen::MatrixXd Q(4, ms.second.size());
for (size_t i = 0; i < ms.second.size(); ++i)
{
Eigen::Affine3d a3d(ms.second[i]);
Eigen::Quaterniond qq(a3d.rotation());
double w = qq.w();
double x = qq.x();
double y = qq.y();
double z = qq.z();
Q.col(i) = Eigen::Vector4d(w, x, y, z);
}

Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(Q * Q.transpose());
Eigen::Vector4d D = eigensolver.eigenvalues();
Eigen::Matrix4d V = eigensolver.eigenvectors();
double max_value(-9999999);
int imax;
for (int j = 0; j < 4; j++)
{
if (D[j] > max_value)
{
imax = j;
max_value = D[j];
}
}
Eigen::Quaterniond q(V.col(imax)[0], V.col(imax)[1], V.col(imax)[2], V.col(imax)[3]);
if (q.w() < 0)
q.coeffs()[0] = -q.w();
rel_mats_median[ms.first].rotate(q);
}

for (const std::pair<int, Eigen::Affine3d> &trans : rel_mats_median)
{
std::cout << "{id: " << trans.first
<< ", size: " << 0.1077
<< ", x: " << trans.second.translation()[0]
<< ", y: " << trans.second.translation()[1]
<< ", z: " << trans.second.translation()[2];
Eigen::Quaterniond q(trans.second.rotation());
std::cout << ", qw: " << q.w()
<< ", qx: " << q.x()
<< ", qy: " << q.y()
<< ", qz: " << q.z()
<< "}"
<< std::endl;
}

g_frame_cnt = 0;
detection_array_deque.clear();
}
else
{
detection_array_deque.emplace_back(*detection_array);
}
}

int main(int argc, char *argv[])
{
ros::init(argc, argv, "calib_bundel");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
private_nh.param("input_frame_count", input_frame_count, 100);

ros::Subscriber dection_sub = nh.subscribe<apriltag_ros::AprilTagDetectionArray>("/tag_detections", 1, tagDetectionCallback);

ros::spin();
}