//==============================================================================
void ShapeNode::setRelativeTransform(const Eigen::Isometry3d& transform)
{
if(transform.matrix() == FixedFrame::mAspectProperties.mRelativeTf.matrix())
return;
const Eigen::Isometry3d oldTransform = getRelativeTransform();
FixedFrame::setRelativeTransform(transform);
dirtyJacobian();
dirtyJacobianDeriv();
mRelativeTransformUpdatedSignal.raise(
this, oldTransform, getRelativeTransform());
}
//==============================================================================
void ShapeNode::setRelativeRotation(const Eigen::Matrix3d& rotation)
{
Eigen::Isometry3d transform = getRelativeTransform();
transform.linear() = rotation;
setRelativeTransform(transform);
}
//==============================================================================
void ShapeNode::setRelativeTranslation(const Eigen::Vector3d& translation)
{
Eigen::Isometry3d transform = getRelativeTransform();
transform.translation() = translation;
setRelativeTransform(transform);
}
//==============================================================================
void ZeroDofJoint::addChildArtInertiaImplicitTo(
Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia)
{
// Add child body's articulated inertia to parent body's articulated inertia.
// Note that mT should be updated.
_parentArtInertia += math::transformInertia(getRelativeTransform().inverse(),
_childArtInertia);
}
//==============================================================================
void EndEffector::updateEffectorJacobianSpatialDeriv() const
{
mEffectorJacobianSpatialDeriv =
math::AdInvTJac(getRelativeTransform(),
mBodyNode->getJacobianSpatialDeriv());
mIsBodyJacobianSpatialDerivDirty = false;
}
//==============================================================================
void ZeroDofJoint::addChildBiasImpulseTo(
Eigen::Vector6d& _parentBiasImpulse,
const Eigen::Matrix6d& /*_childArtInertia*/,
const Eigen::Vector6d& _childBiasImpulse)
{
// Add child body's bias force to parent body's bias impulse. Note that mT
// should be updated.
_parentBiasImpulse += math::dAdInvT(getRelativeTransform(), _childBiasImpulse);
}
//==============================================================================
void ZeroDofJoint::addChildBiasForceTo(
Eigen::Vector6d& _parentBiasForce,
const Eigen::Matrix6d& _childArtInertia,
const Eigen::Vector6d& _childBiasForce,
const Eigen::Vector6d& _childPartialAcc)
{
// Add child body's bias force to parent body's bias force. Note that mT
// should be updated.
_parentBiasForce += math::dAdInvT(getRelativeTransform(), _childBiasForce
+ _childArtInertia*_childPartialAcc);
}
int main(int argc, char *argv[])
{
auto options = std::unique_ptr<getopt_t, void(*)(getopt_t*)> (getopt_create(), getopt_destroy);
// getopt_t *options = getopt_create();
getopt_add_bool(options.get(), 'h', "help", 0, "Show this help");
getopt_add_bool(options.get(), 'd', "debug", 0, "Enable debugging output (slow)");
getopt_add_bool(options.get(), 'w', "window", 1, "Show the detected tags in a window");
getopt_add_bool(options.get(), 'q', "quiet", 0, "Reduce output");
getopt_add_int(options.get(), '\0', "border", "1", "Set tag family border size");
getopt_add_int(options.get(), 't', "threads", "4", "Use this many CPU threads");
getopt_add_double(options.get(), 'x', "decimate", "1.0", "Decimate input image by this factor");
getopt_add_double(options.get(), 'b', "blur", "0.0", "Apply low-pass blur to input");
getopt_add_bool(options.get(), '0', "refine-edges", 1, "Spend more time trying to align edges of tags");
getopt_add_bool(options.get(), '1', "refine-decode", 0, "Spend more time trying to decode tags");
getopt_add_bool(options.get(), '2', "refine-pose", 0, "Spend more time trying to precisely localize tags");
getopt_add_double(options.get(), 's', "size", "0.04047", "Physical side-length of the tag (meters)");
getopt_add_int(options.get(), 'c', "camera", "0", "Camera ID");
getopt_add_int(options.get(), 'i', "tag_id", "-1", "Tag ID (-1 for all tags in family)");
if (!getopt_parse(options.get(), argc, argv, 1) || getopt_get_bool(options.get(), "help")) {
printf("Usage: %s [options]\n", argv[0]);
getopt_do_usage(options.get());
exit(0);
}
AprilTagDetector tag_detector(options);
auto lcm = std::make_shared<lcm::LCM>();
Eigen::Matrix3d camera_matrix = Eigen::Matrix3d::Identity();
// camera_matrix(0,0) = bot_camtrans_get_focal_length_x(mCamTransLeft);
// camera_matrix(1,1) = bot_camtrans_get_focal_length_y(mCamTransLeft);
// camera_matrix(0,2) = bot_camtrans_get_principal_x(mCamTransLeft);
// camera_matrix(1,2) = bot_camtrans_get_principal_y(mCamTransLeft);
camera_matrix(0,0) = 535.04778754;
camera_matrix(1,1) = 533.37100256;
camera_matrix(0,2) = 302.83654976;
camera_matrix(1,2) = 237.69023961;
Eigen::Vector4d distortion_coefficients(-7.74010810e-02, -1.97835565e-01, -4.47956948e-03, -5.42361499e-04);
// camera matrix:
// [[ 535.04778754 0. 302.83654976]
// [ 0. 533.37100256 237.69023961]
// [ 0. 0. 1. ]]
// distortion coefficients: [ -7.74010810e-02 -1.97835565e-01 -4.47956948e-03 -5.42361499e-04
// 9.30985112e-01]
cv::VideoCapture capture(getopt_get_int(options.get(), "camera"));
if (!capture.isOpened()) {
std::cout << "Cannot open the video cam" << std::endl;
return -1;
}
cv::Mat frame;
Eigen::Isometry3d tag_to_camera = Eigen::Isometry3d::Identity();
crazyflie_t::webcam_pos_t tag_to_camera_msg;
while (capture.read(frame)) {
std::vector<TagMatch> tags = tag_detector.detectTags(frame);
if (tags.size() > 0) {
tag_to_camera = getRelativeTransform(tags[0], camera_matrix, distortion_coefficients, tag_detector.getTagSize());
tag_to_camera_msg = encodeWebcamPos(tag_to_camera);
tag_to_camera_msg.frame_id = 1;
} else {
tag_to_camera_msg = encodeWebcamPos(tag_to_camera);
tag_to_camera_msg.frame_id = -1;
}
tag_to_camera_msg.timestamp = timestamp_now();
lcm->publish("WEBCAM_POS", &tag_to_camera_msg);
}
return 0;
}
//==============================================================================
const Eigen::Isometry3d& Joint::getLocalTransform() const
{
return getRelativeTransform();
}
//==============================================================================
Eigen::Vector3d ShapeNode::getRelativeTranslation() const
{
return getRelativeTransform().translation();
}
//==============================================================================
Eigen::Matrix3d ShapeNode::getRelativeRotation() const
{
return getRelativeTransform().linear();
}
