6-7 Deque(25 point(s))

本文介绍了一种双向队列(deque)的数据结构实现方法,使用双向链表作为底层存储结构,支持在队首和队尾进行插入和删除操作,所有操作的时间复杂度均为O(1)。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

6-7 Deque(25 point(s))

A "deque" is a data structure consisting of a list of items, on which the following operations are possible:

  • Push(X,D): Insert item X on the front end of deque D.
  • Pop(D): Remove the front item from deque D and return it.
  • Inject(X,D): Insert item X on the rear end of deque D.
  • Eject(D): Remove the rear item from deque D and return it.Write routines to support the deque that take O(1) time per operation.

Format of functions:

Deque CreateDeque();
int Push( ElementType X, Deque D );
ElementType Pop( Deque D );
int Inject( ElementType X, Deque D );
ElementType Eject( Deque D );

where Deque is defined as the following:

typedef struct Node *PtrToNode;
struct Node {
    ElementType Element;
    PtrToNode Next, Last;
};
typedef struct DequeRecord *Deque;
struct DequeRecord {
    PtrToNode Front, Rear;
};

Here the deque is implemented by a doubly linked list with a header. Front and Rear point to the two ends of the deque respectively. Front always points to the header. The deque is empty when Front and Rear both point to the same dummy header.Note: Push and Inject are supposed to return 1 if the operations can be done successfully, or 0 if fail. If the deque is empty, Pop and Eject must return ERROR which is defined by the judge program.

Sample program of judge:

#include <stdio.h>
#include <stdlib.h>

#define ElementType int
#define ERROR 1e5
typedef enum { push, pop, inject, eject, end } Operation;

typedef struct Node *PtrToNode;
struct Node {
    ElementType Element;
    PtrToNode Next, Last;
};
typedef struct DequeRecord *Deque;
struct DequeRecord {
    PtrToNode Front, Rear;
};
Deque CreateDeque();
int Push( ElementType X, Deque D );
ElementType Pop( Deque D );
int Inject( ElementType X, Deque D );
ElementType Eject( Deque D );

Operation GetOp();          /* details omitted */
void PrintDeque( Deque D ); /* details omitted */

int main()
{
    ElementType X;
    Deque D;
    int done = 0;

    D = CreateDeque();
    while (!done) {
        switch(GetOp()) {
        case push: 
            scanf("%d", &X);
            if (!Push(X, D)) printf("Memory is Full!\n");
            break;
        case pop:
            X = Pop(D);
            if ( X==ERROR ) printf("Deque is Empty!\n");
            break;
        case inject: 
            scanf("%d", &X);
            if (!Inject(X, D)) printf("Memory is Full!\n");
            break;
        case eject:
            X = Eject(D);
            if ( X==ERROR ) printf("Deque is Empty!\n");
            break;
        case end:
            PrintDeque(D);
            done = 1;
            break;
        }
    }
    return 0;
}

/* Your function will be put here */

Sample Input:

Pop
Inject 1
Pop
Eject
Push 1
Push 2
Eject
Inject 3
End

Sample Output:

Deque is Empty!
Deque is Empty!
Inside Deque: 2 3
这个题只要能明白双向队列的功能原理实现就好说了
//模型:front不动,每次插入的时候是在front和它下一个节点之间插入,而rear每次都是移动的,每次都是插到后面,并且rear每次都要
Deque CreateDeque(){//移动指向最后一个上。
    Deque p;
    p = (Deque)malloc(sizeof(struct DequeRecord));//创建头尾指针
    p->Front = (PtrToNode)malloc(sizeof(struct Node));//在头上先创建一个虚的节点
    p->Front->Last = NULL;//前一个为null说明队列左边没东西
    p->Rear = p->Front;//左右相同指向同一个
    p->Rear->Next = NULL;//后一个为null说明右边没有东西
    return p;
}
int Push( ElementType X, Deque D ){//每次在front和下一个节点之间插入
    struct Node* temp;
    temp = (struct Node*)malloc(sizeof(struct Node));
    if(!temp)return 0;//内存满了,申请失败
    temp->Element = X;//赋值
    if(D->Front==D->Rear){//如果是个空双端队列
        D->Front->Next = temp;//插入第一个点
        temp->Last = D->Front;//回指向front
        D->Rear = temp;//rear指针移动指向第一个新插入的,代表从左边进入的第一个最靠近右边
        temp->Next = NULL;//第一次在左边进入的第一个点下一个将不再指向任何东西,因为每次
        return 1;         //都在前一个插入,第一个点会越来越远
    }
    //一般情况
    temp->Next = D->Front->Next;//新节点下一个指向原来front指向的下一个
    temp->Last = D->Front;//新节点的前一个指向fronr
    D->Front->Next->Last = temp;//front原来所值元素的前一个指向新的节点
    D->Front->Next = temp;//front的下一个指向新节点
    return 1;
}
ElementType Pop( Deque D ){
    if(D->Front==D->Rear)
        return ERROR;//如果空队列返回错误
    int temp = D->Front->Next->Element;//保存pop出的值
    struct Node* t = D->Front->Next;//保存它是为了最后把它内存释放掉
    if(D->Front->Next==D->Rear){//队列中只有一个元素的时候
        D->Rear = D->Front;//删除后rear前移使得rear和front相等
        D->Rear->Next = NULL;//虚节点指向空
        free(t);
        return temp;
    }
    //一般情况
    D->Front->Next->Next->Last = D->Front;//我们要删除front的前一个所以删除之后front前一个的前一个的Last应该指回Front
    D->Front->Next = D->Front->Next->Next;//同理front下一个应该是原来没删前下一个的下一个
    free(t);
    return temp;
}
int Inject( ElementType X, Deque D ){//从右边插入就直接插到后面,然后rear后移
    struct Node* temp = (struct Node*)malloc(sizeof(struct Node));
    if(!temp)return 0;
    temp->Element = X;
    if(D->Front==D->Rear){//空双端队列
        D->Front->Next = temp;
        temp->Last = D->Front;
        D->Rear = temp;
        return 1;//和push的一样
    }
    //一般情况
    D->Rear->Next = temp;//rear下一个等于新节点
    temp->Last = D->Rear;//新节点前一个等于现在rear指的点
    temp->Next = NULL;//temp的下一个指向空
    D->Rear = temp;//rear右移到当前点
    return 1;
}
ElementType Eject( Deque D ){
    if(D->Front==D->Rear){//空队列返回错误
        return ERROR;
    }
    int temp = D->Rear->Element;//保存值
    struct Node* t = D->Rear;
    D->Rear = D->Rear->Last;//删掉节点rear指回前一个节点
    D->Rear->Next = NULL;//现在节点为最后一个,所以指向空
    free(t);
    return temp;

}


 
#include <ros/ros.h> #include <nav_msgs/OccupancyGrid.h> #include <sensor_msgs/Image.h> #include <sensor_msgs/RegionOfInterest.h> #include <sensor_msgs/LaserScan.h> #include <tf2/LinearMath/Quaternion.h> #include <tf2_ros/transform_broadcaster.h> #include <tf2_ros/transform_listener.h> #include <tf2_geometry_msgs/tf2_geometry_msgs.h> #include <tf2/LinearMath/Matrix3x3.h> #include <geometry_msgs/TransformStamped.h> #include <geometry_msgs/PoseWithCovarianceStamped.h> #include <cv_bridge/cv_bridge.h> #include <opencv2/opencv.hpp> #include <std_msgs/String.h> #include <std_srvs/Empty.h> #include <vector> #include <cmath> #include <tf2/LinearMath/Matrix3x3.h> #include <deque> #include <nav_msgs/Odometry.h> // 添加Odometry消息 nav_msgs::OccupancyGrid map_msg; cv::Mat map_cropped; cv::Mat map_temp; cv::Mat map_match; sensor_msgs::RegionOfInterest map_roi_info; std::vector<cv::Point2f> scan_points; std::vector<cv::Point2f> best_transform; ros::ServiceClient clear_costmaps_client; std::string base_frame; std::string odom_frame; std::string laser_frame; std::string laser_topic; float lidar_x = 250, lidar_y = 250, lidar_yaw = 0; float deg_to_rad = M_PI / 180.0; int cur_sum = 0; int clear_countdown = -1; int scan_count = 0; // 全局:保存上一次滤波后的状态 double s_x = 0.0, s_y = 0.0, s_yaw = 0.0; bool s_init = false; double alpha; // 在 main 里通过 param 读取 // 添加全局发布器 ros::Publisher robot_pose_pub; // 用于发布机器人位姿 // 初始姿态回调函数 void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) { try { static tf2_ros::Buffer tfBuffer; static tf2_ros::TransformListener tfListener(tfBuffer); // 1. 查询从 base_frame 到 laser_frame 的转换 geometry_msgs::TransformStamped transformStamped = tfBuffer.lookupTransform(base_frame, laser_frame, ros::Time(0), ros::Duration(1.0)); // 2. 创建一个 stamped pose 用于转换 geometry_msgs::PoseStamped base_pose, laser_pose; base_pose.header = msg->header; base_pose.pose = msg->pose.pose; // 3. 将 base_frame 的位姿转换为 laser_frame 的位姿 tf2::doTransform(base_pose, laser_pose, transformStamped); // 4. 从转换后的消息中提取位置和方向信息 double x = msg->pose.pose.position.x; double y = msg->pose.pose.position.y; tf2::Quaternion q( laser_pose.pose.orientation.x, laser_pose.pose.orientation.y, laser_pose.pose.orientation.z, laser_pose.pose.orientation.w); // 5. 将四元数转换为yaw角度 tf2::Matrix3x3 m(q); double roll, pitch, yaw; m.getRPY(roll, pitch, yaw); // 6. 将地图坐标转换为裁切后的地图坐标 lidar_x = (x - map_msg.info.origin.position.x) / map_msg.info.resolution - map_roi_info.x_offset; lidar_y = (y - map_msg.info.origin.position.y) / map_msg.info.resolution - map_roi_info.y_offset; // 7. 设置yaw角度 lidar_yaw = -yaw; clear_countdown = 30; } catch (tf2::TransformException &ex) { ROS_WARN("无法获取从 %s 到 %s 的转换: %s", base_frame.c_str(), laser_frame.c_str(), ex.what()); } } void crop_map(); void processMap(); void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg) { map_msg = *msg; crop_map(); processMap(); } void crop_map() { // 显示地图信息 std_msgs::Header header = map_msg.header; nav_msgs::MapMetaData info = map_msg.info; //用来统计地图有效区域的变量 int xMax,xMin,yMax,yMin ; xMax=xMin= info.width/2; yMax=yMin= info.height/2; bool bFirstPoint = true; // 把地图数据转换成图片 cv::Mat map_raw(info.height, info.width, CV_8UC1, cv::Scalar(128)); // 灰色背景 for(int y = 0; y < info.height; y++) { for(int x = 0; x < info.width; x++) { int index = y * info.width + x; // 直接使用map_msg.data中的值 map_raw.at<uchar>(y, x) = static_cast<uchar>(map_msg.data[index]); // 统计有效区域 if(map_msg.data[index] == 100) { if(bFirstPoint) { xMax = xMin = x; yMax = yMin = y; bFirstPoint = false; continue; } xMin = std::min(xMin, x); xMax = std::max(xMax, x); yMin = std::min(yMin, y); yMax = std::max(yMax, y); } } } // 计算有效区域的中心点坐标 int cen_x = (xMin + xMax)/2; int cen_y = (yMin + yMax)/2; // 按照有效区域对地图进行裁剪 int new_half_width = abs(xMax - xMin)/2 + 50; int new_half_height = abs(yMax - yMin)/2 + 50; int new_origin_x = cen_x - new_half_width; int new_origin_y = cen_y - new_half_height; int new_width = new_half_width*2; int new_height = new_half_height*2; if(new_origin_x < 0) new_origin_x = 0; if((new_origin_x + new_width) > info.width) new_width = info.width - new_origin_x; if(new_origin_y < 0) new_origin_y = 0; if((new_origin_y + new_height) > info.height) new_height = info.height - new_origin_y; cv::Rect roi(new_origin_x, new_origin_y, new_width, new_height); cv::Mat roi_map = map_raw(roi).clone(); map_cropped = roi_map; // 地图的裁减信息 map_roi_info.x_offset = new_origin_x; map_roi_info.y_offset = new_origin_y; map_roi_info.width = new_width; map_roi_info.height = new_height; geometry_msgs::PoseWithCovarianceStamped init_pose; init_pose.pose.pose.position.x = 0.0; init_pose.pose.pose.position.y = 0.0; init_pose.pose.pose.position.y = 0.0; init_pose.pose.pose.orientation.x = 0.0; init_pose.pose.pose.orientation.y = 0.0; init_pose.pose.pose.orientation.z = 0.0; init_pose.pose.pose.orientation.w = 1.0; geometry_msgs::PoseWithCovarianceStamped::ConstPtr init_pose_ptr(new geometry_msgs::PoseWithCovarianceStamped(init_pose)); initialPoseCallback(init_pose_ptr); } bool check(float x, float y, float yaw); void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg) { scan_points.clear(); double angle = msg->angle_min; for (size_t i = 0; i < msg->ranges.size(); ++i) { if (msg->ranges[i] >= msg->range_min && msg->ranges[i] <= msg->range_max) { float x = msg->ranges[i] * cos(angle) / map_msg.info.resolution; float y = -msg->ranges[i] * sin(angle) / map_msg.info.resolution; scan_points.push_back(cv::Point2f(x, y)); } angle += msg->angle_increment; } if(scan_count == 0) scan_count ++; while (ros::ok()) { if (!map_cropped.empty()) { // 计算三种情况下的雷达点坐标数组 std::vector<cv::Point2f> transform_points, clockwise_points, counter_points; int max_sum = 0; float best_dx = 0, best_dy = 0, best_dyaw = 0; for (const auto& point : scan_points) { // 情况一:原始角度 float rotated_x = point.x * cos(lidar_yaw) - point.y * sin(lidar_yaw); float rotated_y = point.x * sin(lidar_yaw) + point.y * cos(lidar_yaw); transform_points.push_back(cv::Point2f(rotated_x + lidar_x, lidar_y - rotated_y)); // 情况二:顺时针旋转1度 float clockwise_yaw = lidar_yaw + deg_to_rad; rotated_x = point.x * cos(clockwise_yaw) - point.y * sin(clockwise_yaw); rotated_y = point.x * sin(clockwise_yaw) + point.y * cos(clockwise_yaw); clockwise_points.push_back(cv::Point2f(rotated_x + lidar_x, lidar_y - rotated_y)); // 情况三:逆时针旋转1度 float counter_yaw = lidar_yaw - deg_to_rad; rotated_x = point.x * cos(counter_yaw) - point.y * sin(counter_yaw); rotated_y = point.x * sin(counter_yaw) + point.y * cos(counter_yaw); counter_points.push_back(cv::Point2f(rotated_x + lidar_x, lidar_y - rotated_y)); } // 计算15种变换方式的匹配值 std::vector<cv::Point2f> offsets = {{0,0}, {1,0}, {-1,0}, {0,1}, {0,-1}}; std::vector<std::vector<cv::Point2f>> point_sets = {transform_points, clockwise_points, counter_points}; std::vector<float> yaw_offsets = {0, deg_to_rad, -deg_to_rad}; for (int i = 0; i < offsets.size(); ++i) { for (int j = 0; j < point_sets.size(); ++j) { int sum = 0; for (const auto& point : point_sets[j]) { float px = point.x + offsets[i].x; float py = point.y + offsets[i].y; if (px >= 0 && px < map_temp.cols && py >= 0 && py < map_temp.rows) { sum += map_temp.at<uchar>(py, px); } } if (sum > max_sum) { max_sum = sum; best_dx = offsets[i].x; best_dy = offsets[i].y; best_dyaw = yaw_offsets[j]; } } } // 更新雷达位置和角度 lidar_x += best_dx; lidar_y += best_dy; lidar_yaw += best_dyaw; // 判断匹配循环是否可以终止 if(check(lidar_x , lidar_y , lidar_yaw)) { break; } } } if(clear_countdown > -1) clear_countdown --; if(clear_countdown == 0) { std_srvs::Empty srv; clear_costmaps_client.call(srv); } } std::deque<std::tuple<float, float, float>> data_queue; const size_t max_size = 10; bool check(float x, float y, float yaw) { if(x == 0 && y == 0 && yaw == 0) { data_queue.clear(); return true; } // 添加新数据 data_queue.push_back(std::make_tuple(x, y, yaw)); // 如果队列超过最大大小,移除最旧的数据 if (data_queue.size() > max_size) { data_queue.pop_front(); } // 如果队列已满,检查第一个和最后一个元素 if (data_queue.size() == max_size) { auto& first = data_queue.front(); auto& last = data_queue.back(); float dx = std::abs(std::get<0>(last) - std::get<0>(first)); float dy = std::abs(std::get<1>(last) - std::get<1>(first)); float dyaw = std::abs(std::get<2>(last) - std::get<2>(first)); // 如果所有差值的绝对值都小于5,清空队列退出循环 if (dx < 5 && dy < 5 && dyaw < 5*deg_to_rad) { data_queue.clear(); return true; } } return false; } cv::Mat createGradientMask(int size) { cv::Mat mask(size, size, CV_8UC1); int center = size / 2; for (int y = 0; y < size; y++) { for (int x = 0; x < size; x++) { double distance = std::hypot(x - center, y - center); int value = cv::saturate_cast<uchar>(255 * std::max(0.0, 1.0 - distance / center)); mask.at<uchar>(y, x) = value; } } return mask; } void processMap() { if (map_cropped.empty()) return; map_temp = cv::Mat::zeros(map_cropped.size(), CV_8UC1); cv::Mat gradient_mask = createGradientMask(101); // 创建一个101x101的渐变掩模 for (int y = 0; y < map_cropped.rows; y++) { for (int x = 0; x < map_cropped.cols; x++) { if (map_cropped.at<uchar>(y, x) == 100) // 障碍物 { int left = std::max(0, x - 50); int top = std::max(0, y - 50); int right = std::min(map_cropped.cols - 1, x + 50); int bottom = std::min(map_cropped.rows - 1, y + 50); cv::Rect roi(left, top, right - left + 1, bottom - top + 1); cv::Mat region = map_temp(roi); int mask_left = 50 - (x - left); int mask_top = 50 - (y - top); cv::Rect mask_roi(mask_left, mask_top, roi.width, roi.height); cv::Mat mask = gradient_mask(mask_roi); cv::max(region, mask, region); } } } } void pose_tf() { if (scan_count == 0) return; if (map_cropped.empty() || map_msg.data.empty()) return; // 一阶低通滤波状态 static bool s_init = false; static double s_x = 0.0, s_y = 0.0, s_yaw = 0.0; static tf2_ros::Buffer tfBuffer; static tf2_ros::TransformListener tfListener(tfBuffer); // 1. 计算原始位姿 double raw_x = (lidar_x + map_roi_info.x_offset) * map_msg.info.resolution + map_msg.info.origin.position.x; double raw_y = (lidar_y + map_roi_info.y_offset) * map_msg.info.resolution + map_msg.info.origin.position.y; double raw_yaw = -lidar_yaw; // 2. 低通滤波 if (!s_init) { s_x = raw_x; s_y = raw_y; s_yaw = raw_yaw; s_init = true; } else { double dy = raw_yaw - s_yaw; while (dy > M_PI) dy -= 2.0 * M_PI; while (dy < -M_PI) dy += 2.0 * M_PI; s_x = alpha * raw_x + (1.0 - alpha) * s_x; s_y = alpha * raw_y + (1.0 - alpha) * s_y; s_yaw = s_yaw + alpha * dy; } // 3. 构造 map -> base_link 变换 tf2::Transform map_to_base; tf2::Quaternion q; // ← 把 q 提到此处,使后续都能访问 q.setRPY(0, 0, s_yaw); map_to_base.setOrigin(tf2::Vector3(s_x, s_y, 0.0)); map_to_base.setRotation(q); // 发布 map -> base_link geometry_msgs::TransformStamped map_to_base_msg; map_to_base_msg.header.stamp = ros::Time::now(); map_to_base_msg.header.frame_id = "map"; map_to_base_msg.child_frame_id = base_frame; map_to_base_msg.transform.translation.x = s_x; map_to_base_msg.transform.translation.y = s_y; map_to_base_msg.transform.translation.z = 0.0; map_to_base_msg.transform.rotation = tf2::toMsg(q); // 现在 q 可见 static tf2_ros::TransformBroadcaster br; br.sendTransform(map_to_base_msg); // 发布机器人里程计 nav_msgs::Odometry robot_odom; robot_odom.header.stamp = ros::Time::now(); robot_odom.header.frame_id = "map"; robot_odom.child_frame_id = base_frame; robot_odom.pose.pose.position.x = s_x; robot_odom.pose.pose.position.y = s_y; robot_odom.pose.pose.position.z = 0.0; robot_odom.pose.pose.orientation = tf2::toMsg(q); robot_pose_pub.publish(robot_odom); // 7. 查询 odom -> laser_frame geometry_msgs::TransformStamped odom_to_laser; try { odom_to_laser = tfBuffer.lookupTransform(odom_frame, laser_frame, ros::Time(0)); } catch (tf2::TransformException &ex) { ROS_WARN("%s", ex.what()); return; } // 8. 计算 map -> odom tf2::Transform odom_to_laser_tf2, map_to_laser; tf2::fromMsg(odom_to_laser.transform, odom_to_laser_tf2); // 注意此处用 s_y 而不是 x_y map_to_laser.setOrigin(tf2::Vector3(s_x, s_y, 0.0)); map_to_laser.setRotation(q); tf2::Transform map_to_odom = map_to_laser * odom_to_laser_tf2.inverse(); // 9. 发布 map -> odom geometry_msgs::TransformStamped map_to_odom_msg; map_to_odom_msg.header.stamp = ros::Time::now(); map_to_odom_msg.header.frame_id = "map"; map_to_odom_msg.child_frame_id = odom_frame; map_to_odom_msg.transform = tf2::toMsg(map_to_odom); br.sendTransform(map_to_odom_msg); } int main(int argc, char** argv) { setlocale(LC_ALL,""); ros::init(argc, argv, "lidar_loc"); // 读取参数 ros::NodeHandle private_nh("~"); private_nh.param<std::string>("base_frame", base_frame, "base_footprint"); private_nh.param<std::string>("odom_frame", odom_frame, "odom"); private_nh.param<std::string>("laser_frame", laser_frame, "scan"); private_nh.param<std::string>("laser_topic", laser_topic, "base_scan"); private_nh.param("filter_alpha", alpha, 0.1); ros::NodeHandle nh; ros::Duration(1).sleep(); // 初始化发布器 - 用于机器人位姿 robot_pose_pub = nh.advertise<nav_msgs::Odometry>("lidar_odom", 10); ros::Subscriber map_sub = nh.subscribe("map", 1, mapCallback); ros::Subscriber scan_sub = nh.subscribe(laser_topic, 1, scanCallback); ros::Subscriber initial_pose_sub = nh.subscribe("initialpose", 1, initialPoseCallback); clear_costmaps_client = nh.serviceClient<std_srvs::Empty>("move_base/clear_costmaps"); ros::Rate rate(30); // tf发送频率 while (ros::ok()) { pose_tf(); ros::spinOnce(); rate.sleep(); } return 0; }为什么我使用这个激光定位算法并且加入滤波后,机器人轨迹还是会有跳变或者起伏,请你帮我全面分析一下
最新发布
08-03
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值