AI 助理
备案控制台
开发者社区 IoT 文章 正文

使用ORBSLAM2进行kineticV2稠密建图，实时转octomap建图以及导航（下）

2022-01-13 2058
版权
版权声明：
本文内容由阿里云实名注册用户自发贡献，版权归原作者所有，阿里云开发者社区不拥有其著作权，亦不承担相应法律责任。具体规则请查看《 阿里云开发者社区用户服务协议》和 《阿里云开发者社区知识产权保护指引》。如果您发现本社区中有涉嫌抄袭的内容，填写 侵权投诉表单进行举报，一经查实，本社区将立刻删除涉嫌侵权内容。
本文涉及的产品
资源编排，不限时长
简介： 使用ORBSLAM2进行kineticV2稠密建图，实时转octomap建图以及导航（下）

之后存在几种方法去实现导航：


1、octomap_server是ROS中的一个基于octomap的功能包。


我在查阅资料的时候，发现所有的介绍、博客等资料都是在介绍其将点云地图转化为基于Octree的OctoMap的功能。由于之前一直在查找三维点云地图转化为二维地图的方法，所以之前试过这个包的三维转换功能后就没有在继续使用，由于之前使用其他方法将二维占据栅格地图生成了，然后准备回过头来再看一下octomap_server的三维概率地图，然后在不经意间就发现了它也有转化为二维地图的功能


首先简单介绍下octomap_server【设置】的安装。


打开一个终端.(ctrl+alt+T)输入下面指令安装octomap.
sudo apt-get install ros-kinetic-octomap-ros #安装octomap
sudo apt-get install ros-kinetic-octomap-msgs
sudo apt-get install ros-kinetic-octomap-server
安装octomap 在 rviz 中的插件
sudo apt-get install ros-kinetic-octomap-rviz-plugins


安装上这个插件以后你可以启动 rviz ,这时候点开Add选项，会多一个octomap_rviz_plugins模组.如下图所示:


20200402171457842.png


其中的OccupancyGrid是显示三维概率地图，也就是octomap地图。OccupancyMap是显示二维占据栅格地图。


可以通过一个launch文件启动octomap_server节点，如下：


20200402171524920.png


其中的param都是可以修改的，具体的修改细节见这里。/octotree_map修改为自己的PointCloud2点云即可。


运行此launch文件会有如下话题：


20200402171542505.png


打开rviz，在里面添加OccupancyGrid，OccupancyMap，Map显示选项,显示话题选择octomap_full或者octomap_binary。

注意：param中的frame_id要和rviz的Fixed Frame一致。


Map:


20200402171621624.png


OccupancyMap:


20200402171638317.png


OccupancyGrid:

20200402171653564.png


对应的参考代码：https://github.com/306327680/PointCloud-to-grid-map【参考注释】


或者北达科他大学( North Dakota State University)cloud_to_map学习代码：https://download.csdn.net/download/sru_alo/12277545


2、使用3D稠密点云图，并使用octomap进行压缩滤除地面信息。


然后通过2D投影生成占据格地图最后利用costmap进行全局和局部路径规划导航实时避障。【参考代码】


20200402173801804.png


然后参考OctoMap中3D-RRT路径规划


20200402173912766.png


#include "ros/ros.h"
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <octomap_ros/conversions.h>
#include <octomap/octomap.h>
#include <message_filters/subscriber.h>
#include "visualization_msgs/Marker.h"
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/base/OptimizationObjective.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
// #include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/planners/rrt/InformedRRTstar.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/config.h>
#include <iostream>
#include "fcl/config.h"
#include "fcl/octree.h"
#include "fcl/traversal/traversal_node_octree.h"
#include "fcl/collision.h"
#include "fcl/broadphase/broadphase.h"
#include "fcl/math/transform.h"
namespace ob = ompl::base;
namespace og = ompl::geometric;
// Declear some global variables
//ROS publishers
ros::Publisher vis_pub;
ros::Publisher traj_pub;
class planner {
public:
  void setStart(double x, double y, double z)
  {
    ob::ScopedState<ob::SE3StateSpace> start(space);
    start->setXYZ(x,y,z);
    start->as<ob::SO3StateSpace::StateType>(1)->setIdentity();
    pdef->clearStartStates();
    pdef->addStartState(start);
  }
  void setGoal(double x, double y, double z)
  {
    ob::ScopedState<ob::SE3StateSpace> goal(space);
    goal->setXYZ(x,y,z);
    goal->as<ob::SO3StateSpace::StateType>(1)->setIdentity();
    pdef->clearGoal();
    pdef->setGoalState(goal);
    std::cout << "goal set to: " << x << " " << y << " " << z << std::endl;
  }
  void updateMap(std::shared_ptr<fcl::CollisionGeometry> map)
  {
    tree_obj = map;
  }
  // Constructor
  planner(void)
  {
    //四旋翼的障碍物几何形状
    Quadcopter = std::shared_ptr<fcl::CollisionGeometry>(new fcl::Box(0.8, 0.8, 0.3));
    //分辨率参数设置
    fcl::OcTree* tree = new fcl::OcTree(std::shared_ptr<const octomap::OcTree>(new octomap::OcTree(0.15)));
    tree_obj = std::shared_ptr<fcl::CollisionGeometry>(tree);
    //解的状态空间
    space = ob::StateSpacePtr(new ob::SE3StateSpace());
    // create a start state
    ob::ScopedState<ob::SE3StateSpace> start(space);
    // create a goal state
    ob::ScopedState<ob::SE3StateSpace> goal(space);
    // set the bounds for the R^3 part of SE(3)
    // 搜索的三维范围设置
    ob::RealVectorBounds bounds(3);
    bounds.setLow(0,-5);
    bounds.setHigh(0,5);
    bounds.setLow(1,-5);
    bounds.setHigh(1,5);
    bounds.setLow(2,0);
    bounds.setHigh(2,3);
    space->as<ob::SE3StateSpace>()->setBounds(bounds);
    // construct an instance of  space information from this state space
    si = ob::SpaceInformationPtr(new ob::SpaceInformation(space));
    start->setXYZ(0,0,0);
    start->as<ob::SO3StateSpace::StateType>(1)->setIdentity();
    // start.random();
    goal->setXYZ(0,0,0);
    goal->as<ob::SO3StateSpace::StateType>(1)->setIdentity();
    // goal.random();
      // set state validity checking for this space
    si->setStateValidityChecker(std::bind(&planner::isStateValid, this, std::placeholders::_1 ));
    // create a problem instance
    pdef = ob::ProblemDefinitionPtr(new ob::ProblemDefinition(si));
    // set the start and goal states
    pdef->setStartAndGoalStates(start, goal);
      // set Optimizattion objective
    pdef->setOptimizationObjective(planner::getPathLengthObjWithCostToGo(si));
    std::cout << "Initialized: " << std::endl;
  }
  // Destructor
  ~planner()
  {
  }
  void replan(void)
  {
    std::cout << "Total Points:" << path_smooth->getStateCount () << std::endl;
    if(path_smooth->getStateCount () <= 2)
      plan();
    else
    {
      for (std::size_t idx = 0; idx < path_smooth->getStateCount (); idx++)
      {
        if(!replan_flag)
          replan_flag = !isStateValid(path_smooth->getState(idx));
        else
          break;
      }
      if(replan_flag)
        plan();
      else
        std::cout << "Replanning not required" << std::endl;
    }
  }
  void plan(void)
  {
      // create a planner for the defined space
    og::InformedRRTstar* rrt = new og::InformedRRTstar(si);
    //设置rrt的参数range
    rrt->setRange(0.2);
    ob::PlannerPtr plan(rrt);
      // set the problem we are trying to solve for the planner
    plan->setProblemDefinition(pdef);
      // perform setup steps for the planner
    plan->setup();
      // print the settings for this space
    si->printSettings(std::cout);
    std::cout << "problem setting\n";
      // print the problem settings
    pdef->print(std::cout);
      // attempt to solve the problem within one second of planning time
    ob::PlannerStatus solved = plan->solve(1);
    if (solved)
    {
          // get the goal representation from the problem definition (not the same as the goal state)
          // and inquire about the found path
      std::cout << "Found solution:" << std::endl;
      ob::PathPtr path = pdef->getSolutionPath();
      og::PathGeometric* pth = pdef->getSolutionPath()->as<og::PathGeometric>();
      pth->printAsMatrix(std::cout);
          // print the path to screen
          // path->print(std::cout);
      nav_msgs::Path msg;
      msg.header.stamp = ros::Time::now();
      msg.header.frame_id = "map";
      for (std::size_t path_idx = 0; path_idx < pth->getStateCount (); path_idx++)
      {
        const ob::SE3StateSpace::StateType *se3state = pth->getState(path_idx)->as<ob::SE3StateSpace::StateType>();
              // extract the first component of the state and cast it to what we expect
        const ob::RealVectorStateSpace::StateType *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
              // extract the second component of the state and cast it to what we expect
        const ob::SO3StateSpace::StateType *rot = se3state->as<ob::SO3StateSpace::StateType>(1);
        geometry_msgs::PoseStamped pose;
//        pose.header.frame_id = "/world"
        pose.pose.position.x = pos->values[0];
        pose.pose.position.y = pos->values[1];
        pose.pose.position.z = pos->values[2];
        pose.pose.orientation.x = rot->x;
        pose.pose.orientation.y = rot->y;
        pose.pose.orientation.z = rot->z;
        pose.pose.orientation.w = rot->w;
        msg.poses.push_back(pose);
      }
      traj_pub.publish(msg);
          //Path smoothing using bspline
      //B样条曲线优化
      og::PathSimplifier* pathBSpline = new og::PathSimplifier(si);
      path_smooth = new og::PathGeometric(dynamic_cast<const og::PathGeometric&>(*pdef->getSolutionPath()));
      pathBSpline->smoothBSpline(*path_smooth,3);
      // std::cout << "Smoothed Path" << std::endl;
      // path_smooth.print(std::cout);
      //Publish path as markers
      nav_msgs::Path smooth_msg;
      smooth_msg.header.stamp = ros::Time::now();
      smooth_msg.header.frame_id = "map";
      for (std::size_t idx = 0; idx < path_smooth->getStateCount (); idx++)
      {
                  // cast the abstract state type to the type we expect
        const ob::SE3StateSpace::StateType *se3state = path_smooth->getState(idx)->as<ob::SE3StateSpace::StateType>();
              // extract the first component of the state and cast it to what we expect
        const ob::RealVectorStateSpace::StateType *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
              // extract the second component of the state and cast it to what we expect
        const ob::SO3StateSpace::StateType *rot = se3state->as<ob::SO3StateSpace::StateType>(1);
        geometry_msgs::PoseStamped point;
//        pose.header.frame_id = "/world"
        point.pose.position.x = pos->values[0];
        point.pose.position.y = pos->values[1];
        point.pose.position.z = pos->values[2];
        point.pose.orientation.x = rot->x;
        point.pose.orientation.y = rot->y;
        point.pose.orientation.z = rot->z;
        point.pose.orientation.w = rot->w;
        smooth_msg.poses.push_back(point);
        std::cout << "Published marker: " << idx << std::endl;
      }
      vis_pub.publish(smooth_msg);
      // ros::Duration(0.1).sleep();
      // Clear memory
      pdef->clearSolutionPaths();
      replan_flag = false;
    }
    else
      std::cout << "No solution found" << std::endl;
  }
private:
  // construct the state space we are planning in
  ob::StateSpacePtr space;
  // construct an instance of  space information from this state space
  ob::SpaceInformationPtr si;
  // create a problem instance
  ob::ProblemDefinitionPtr pdef;
  og::PathGeometric* path_smooth;
  bool replan_flag = false;
  std::shared_ptr<fcl::CollisionGeometry> Quadcopter;
  std::shared_ptr<fcl::CollisionGeometry> tree_obj;
  bool isStateValid(const ob::State *state)
  {
      // cast the abstract state type to the type we expect
    const ob::SE3StateSpace::StateType *se3state = state->as<ob::SE3StateSpace::StateType>();
      // extract the first component of the state and cast it to what we expect
    const ob::RealVectorStateSpace::StateType *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
      // extract the second component of the state and cast it to what we expect
    const ob::SO3StateSpace::StateType *rot = se3state->as<ob::SO3StateSpace::StateType>(1);
    fcl::CollisionObject treeObj((tree_obj));
    fcl::CollisionObject aircraftObject(Quadcopter);
      // check validity of state defined by pos & rot
    fcl::Vec3f translation(pos->values[0],pos->values[1],pos->values[2]);
    fcl::Quaternion3f rotation(rot->w, rot->x, rot->y, rot->z);
    aircraftObject.setTransform(rotation, translation);
    fcl::CollisionRequest requestType(1,false,1,false);
    fcl::CollisionResult collisionResult;
    fcl::collide(&aircraftObject, &treeObj, requestType, collisionResult);
    return(!collisionResult.isCollision());
  }
  // Returns a structure representing the optimization objective to use
  // for optimal motion planning. This method returns an objective which
  // attempts to minimize the length in configuration space of computed
  // paths.
  ob::OptimizationObjectivePtr getThresholdPathLengthObj(const ob::SpaceInformationPtr& si)
  {
    ob::OptimizationObjectivePtr obj(new ob::PathLengthOptimizationObjective(si));
    // obj->setCostThreshold(ob::Cost(1.51));
    return obj;
  }
  ob::OptimizationObjectivePtr getPathLengthObjWithCostToGo(const ob::SpaceInformationPtr& si)
  {
    ob::OptimizationObjectivePtr obj(new ob::PathLengthOptimizationObjective(si));
    obj->setCostToGoHeuristic(&ob::goalRegionCostToGo);
    return obj;
  }
};
void octomapCallback(const octomap_msgs::Octomap::ConstPtr &msg, planner* planner_ptr)
{
    //loading octree from binary
  // const std::string filename = "/home/xiaopeng/dense.bt";
  // octomap::OcTree temp_tree(0.1);
  // temp_tree.readBinary(filename);
  // fcl::OcTree* tree = new fcl::OcTree(std::shared_ptr<const octomap::OcTree>(&temp_tree));
  // convert octree to collision object
  octomap::OcTree* tree_oct = dynamic_cast<octomap::OcTree*>(octomap_msgs::msgToMap(*msg));
  fcl::OcTree* tree = new fcl::OcTree(std::shared_ptr<const octomap::OcTree>(tree_oct));
  // Update the octree used for collision checking
  planner_ptr->updateMap(std::shared_ptr<fcl::CollisionGeometry>(tree));
  planner_ptr->replan();
}
void odomCb(const nav_msgs::Odometry::ConstPtr &msg, planner* planner_ptr)
{
  planner_ptr->setStart(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
}
void startCb(const geometry_msgs::PointStamped::ConstPtr &msg, planner* planner_ptr)
{
  planner_ptr->setStart(msg->point.x, msg->point.y, msg->point.z);
}
void goalCb(const geometry_msgs::PointStamped::ConstPtr &msg, planner* planner_ptr)
{
  planner_ptr->setGoal(msg->point.x, msg->point.y, msg->point.z);
  planner_ptr->plan();
}
int main(int argc, char **argv)
{
  ros::init(argc, argv, "octomap_planner");
  ros::NodeHandle n;
  planner planner_object;
  ros::Subscriber octree_sub = n.subscribe<octomap_msgs::Octomap>("/octomap_binary", 1, boost::bind(&octomapCallback, _1, &planner_object));
  // ros::Subscriber odom_sub = n.subscribe<nav_msgs::Odometry>("/rovio/odometry", 1, boost::bind(&odomCb, _1, &planner_object));
  ros::Subscriber goal_sub = n.subscribe<geometry_msgs::PointStamped>("/goal/clicked_point", 1, boost::bind(&goalCb, _1, &planner_object));
  ros::Subscriber start_sub = n.subscribe<geometry_msgs::PointStamped>("/start/clicked_point", 1, boost::bind(&startCb, _1, &planner_object));
//  vis_pub = n.advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
  vis_pub = n.advertise<nav_msgs::Path>( "visualization_marker", 0 );
//  traj_pub = n.advertise<trajectory_msgs::MultiDOFJointTrajectory>("waypoints",1);
  traj_pub = n.advertise<nav_msgs::Path>("waypoints",1);
  std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
  ros::spin();
  return 0;
}


3、利用OMPL实现的ROS turtlebot 路径规划(需要安装OMPL库，从而可以实现上述两种方式的导航)


20200402174025801.png


此外对应两个纯ORB的开源项目：

https://github.com/Ewenwan/Active-ORB-SLAM2(Ubuntu14.04)

https://github.com/abhineet123/ORB_SLAM2(Ubuntu16.04)


20200402163821498.png


一个RGBD进阶版建图

https://github.com/was48i/IndoorMapping


20200402174407431.png


文章标签：
资源编排
定位技术
相关实践学习
使用ROS创建VPC和VSwitch
本场景主要介绍如何利用阿里云资源编排服务，定义资源编排模板，实现自动化创建阿里云专有网络和交换机。
阿里云资源编排ROS使用教程
资源编排（Resource Orchestration）是一种简单易用的云计算资源管理和自动化运维服务。用户通过模板描述多个云计算资源的依赖关系、配置等，并自动完成所有资源的创建和配置，以达到自动化部署、运维等目的。编排模板同时也是一种标准化的资源和应用交付方式，并且可以随时编辑修改，使基础设施即代码（Infrastructure as Code）成为可能。 产品详情：https://www.aliyun.com/product/ros/
1886446284370443
目录
相关文章
蔡珏
|
8月前
|
算法
飞行员配对方案(Dinic求最大二分图匹配（对匈牙利算法的优化），以及二分图的建图方式)
飞行员配对方案(Dinic求最大二分图匹配（对匈牙利算法的优化），以及二分图的建图方式)
蔡珏
125 0
月照银海似蛟龙
|
存储 前端开发 数据可视化
3D激光SLAM：LeGO-LOAM---两步优化的帧间里程计及代码分析
**LeGO-LOAM**的全称是 Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain 其中LeGO就是轻量级和利用地面优化，轻量级的实现就是通过两步的优化方式，利用地面优化的部分也在两步优化的第一步中。 和原始LOAM一样，通过前后两帧点云来估计两帧之间的运动，从而累加得到前端里程计的输出，和上述方法使用线面约束同时优化六自由度帧间位姿不同，LeGO-LOAM的前端分成两个步骤，每个步骤估计三自由度的变量。 通过这种方式进行帧间里程计的运算，可以提供运算效率，使得可以在嵌入式平台
月照银海似蛟龙
791 0
3D激光SLAM：LeGO-LOAM---两步优化的帧间里程计及代码分析
荔枝科研社
|
计算机视觉
【状态估计】基于卡尔曼滤波器和扩展卡尔曼滤波器用于 INS/GNSS 导航、目标跟踪和地形参考导航研究（Matlab代码实现）
【状态估计】基于卡尔曼滤波器和扩展卡尔曼滤波器用于 INS/GNSS 导航、目标跟踪和地形参考导航研究（Matlab代码实现）
荔枝科研社
101 0
zuoben
|
算法
Threejs中使用astar(A*)算法寻路导航，Threejs寻路定位导航
Threejs中使用astar(A*)算法寻路导航，Threejs寻路定位导航
zuoben
665 0
LeapMay
|
传感器 算法 机器人
获取机器人turtlebot3 在gazebo 中仿真导航时的位姿真值
获取机器人turtlebot3 在gazebo 中仿真导航时的位姿真值
LeapMay
532 0
荔枝科研社
|
传感器 算法 机器人
【通过粒子滤波进行地形辅助导航】用于地形辅助导航的粒子滤波器和 PCRB研究（Matlab代码实现）
【通过粒子滤波进行地形辅助导航】用于地形辅助导航的粒子滤波器和 PCRB研究（Matlab代码实现）
荔枝科研社
152 0
珞瑜
|
传感器 存储 编解码
使用激光雷达数据构建地图并使用SLAM算法估计车辆轨迹
使用激光雷达数据构建地图并使用SLAM算法估计车辆轨迹。
珞瑜
248 0
珞瑜
|
传感器 存储 算法
使用车载激光雷达数据在惯性测量单元读数帮助下构建地图
处理来自安装在车辆上的传感器的 3-D 激光雷达数据，以便在惯性测量单元 （IMU） 读数的帮助下逐步构建地图。这样的地图可以促进车辆导航的路径规划，也可以用于定位。
珞瑜
117 0
Matlab科研工作室
|
传感器 机器学习/深度学习 算法
【滤波跟踪】基于卡尔曼滤波实现ins与gps松组合导航附matlab代码
【滤波跟踪】基于卡尔曼滤波实现ins与gps松组合导航附matlab代码
Matlab科研工作室
201 0
Matlab科研工作室
|
传感器 机器学习/深度学习 算法
【目标定位】基于拓展卡尔曼滤波实现GPS-INS组合导航系统附matlab代码
【目标定位】基于拓展卡尔曼滤波实现GPS-INS组合导航系统附matlab代码
Matlab科研工作室
167 0