Method OdomCb_

sloam/src/core/inputNode.cpp:101–141

}

void InputManager::OdomCb_(const nav_msgs::OdometryConstPtr &odom_msg)
{
    odomCounter_++;
    if (odomCounter_ % odomFreqFilter_ != 0)
        return;
    odomCounter_ = 0;

    auto pose = odom_msg->pose.pose;
    ros::Time odomStamp = odom_msg->header.stamp;
    Quat rot(pose.orientation.w, pose.orientation.x,
             pose.orientation.y, pose.orientation.z);
    Vector3 pos(pose.position.x, pose.position.y, pose.position.z);

    SE3 odom = SE3();
    odom.setQuaternion(rot);
    odom.translation() = pos;

    // odom is in cam frame, we need it in robot frame
    geometry_msgs::TransformStamped transform_cam_body;
    try
    {
        auto transform_cam_body = tf_buffer_.lookupTransform(
            odom_msg->child_frame_id, robot_frame_id_, ros::Time(0));
        SE3 cam_body_tf(tf2::transformToEigen(transform_cam_body).matrix().cast<double>());
        odom = odom * cam_body_tf;
    }
    catch (tf2::TransformException &ex)
    {
        ROS_INFO_THROTTLE(10, "Camera to body transform is not found, now setting it as identity... If you're in simulator, ignore this.");
    }

    if (firstOdom_ && pose.position.z < minSLOAMAltitude_)
    {
        ROS_INFO_THROTTLE(5, "Quad is too low, will not call sloam");
        return;
    }

    odomQueue_.emplace_back(odom, odomStamp);
    if (odomQueue_.size() > 10 * maxQueueSize_)
        odomQueue_.pop_front();
}

bool InputManager::Run()
{