扫描匹配算法的使用（三）

（续前）

七、地图概述

将带有相应转换的扫描加在一起，可以创建一个整体的扫描地图

在接下来的练习中，转换信息来自于模拟器的真实值，因此地图将是完美的(除非扫描记录和转换之间有高延迟)

SLAM（同时定位与测绘)可以利用扫描匹配转换的输出来替代模拟器的真实值转换

在模拟器中移动汽车

在本练习将学习如何制作点云地图，熟悉运行和控制Carla模拟器，同时与地图代码并行。首先运行./run_carla.sh，以后台模式单独启动Carla模拟器；然后运行 ./cloud_mapper启动地图代码，它将可视化模拟汽车的位置，并对环境进行3D扫描。对于第一个任务，启动cloud_mapper程序，并使用下面列出的键来控制模拟的汽车。汽车有一条绿色的线延伸出来，代表汽车的设置转向角度。

控制汽车

手动控制模拟汽车。PCL在其查看器窗口中有一个侦听器。首先单击视图窗口来激活键盘监听器，然后可以使用以下键来激活汽车。注意，只是轻按键。

油门(UP 键)

每一次轻击将增加油门功率。三次按压就有很大的油门功率。

反转/停止(Down 键)

只要轻轻一击，就会停止汽车；如果汽车在行驶，就会将油门重置为零。如果汽车不是在移动状态，它将应用油门在反方向。

转向(Left/Right 键)

点击这些键可以逐渐改变转向角度。在上图中，在红框前面挤出的绿线代表转向值，它的终止点将依据当前值左右移动。

中央摄影机(a)

按此键可使相机对焦，以获得汽车的自上而下视图。

旋转相机(按鼠标左键拖动)

镜头平移(按鼠标中键并拖动)

变焦(按鼠标滚动)

红色的盒子是车，转向角度是绿色的线延伸到车的前面。扫描结果是蓝点云。 

绘制地图

该模拟器使用虚拟激光雷达不断扫描环境，可以选择自定义其设置。每次汽车移动多少米，经过多少时间，扫描结果就会被添加到整体收集的点云地图中。目前，扫描结果与汽车在同一局部参考系，意味着它们也将以原点(0,0)为中心，汽车的角度从x轴的0度开始。当驾驶模拟器时，注意因为不正确的旋转，扫描结果会堆叠在一起。本练习制作地图部分的任务就是纠正扫描转换：使用输入位姿信息传入lidar->Listen()函数。位姿变量包含由模拟器提供的汽车定位和方向的真实值。使用transform3D将输入姿态转换为4 x 4矩阵。然后将变换矩阵与扫描点的位置向量相乘，完成校正。修正之后，就可以创建如下图所示的精确点云图了。作为最后一步，使用体素过滤器Voxel filter 过滤整个点云，和之前的练习中所做的那样。这确保了整个地图的分辨率是一致的，有助于防止在以后使用它进行扫描匹配时再次过滤它。通过按键盘上的s键保存地图为my_map.pcd。

驾驶在一个闭环创建的点云地图。点云图约28MB，由895018个点组成。 

八、绘制地图---代码

#include <carla/client/Client.h>
#include <carla/client/ActorBlueprint.h>
#include <carla/client/BlueprintLibrary.h>
#include <carla/client/Map.h>
#include <carla/geom/Location.h>
#include <carla/geom/Transform.h>
#include <carla/client/Sensor.h>
#include <carla/sensor/data/LidarMeasurement.h>
#include <thread>

#include <carla/client/Vehicle.h>

//pcl code
//#include "render/render.h"

namespace cc = carla::client;
namespace cg = carla::geom;
namespace csd = carla::sensor::data;

using namespace std::chrono_literals;
using namespace std::string_literals;

using namespace std;

#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include "helper.h"
#include <sstream>
#include <chrono> 
#include <ctime> 

PointCloudT pclCloud;
cc::Vehicle::Control control;
std::chrono::time_point<std::chrono::system_clock> currentTime;
vector<ControlState> cs;

bool refresh_view = false;
bool save_map = false;
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void* viewer)
{

      //boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptr<pcl::visualization::PCLVisualizer> *>(viewer_void);
    if (event.getKeySym() == "Right" && event.keyDown()){
        cs.push_back(ControlState(0, -0.02, 0));
      }
    else if (event.getKeySym() == "Left" && event.keyDown()){
        cs.push_back(ControlState(0, 0.02, 0)); 
      }
      if (event.getKeySym() == "Up" && event.keyDown()){
        cs.push_back(ControlState(0.1, 0, 0));
      }
    else if (event.getKeySym() == "Down" && event.keyDown()){
        cs.push_back(ControlState(-0.1, 0, 0)); 
      }

    if(event.getKeySym() == "a" && event.keyDown()){
        refresh_view = true;
    }
    if(event.getKeySym() == "s" && event.keyDown()){
        save_map = true;
    }
}

void Accuate(ControlState response, cc::Vehicle::Control& state){

    if(response.t > 0){
        if(!state.reverse){
            state.throttle = min(state.throttle+response.t, 1.0f);
        }
        else{
            state.reverse = false;
            state.throttle = min(response.t, 1.0f);
        }
    }
    else if(response.t < 0){
        response.t = -response.t;
        if(state.reverse){
            state.throttle = min(state.throttle+response.t, 1.0f);
        }
        else{
            state.reverse = true;
            state.throttle = min(response.t, 1.0f);

        }
    }
    state.steer = min( max(state.steer+response.s, -1.0f), 1.0f);
    state.brake = response.b;
}

void drawCar(Pose pose, int num, Color color, double alpha, pcl::visualization::PCLVisualizer::Ptr& viewer){

    BoxQ box;
    box.bboxTransform = Eigen::Vector3f(pose.position.x, pose.position.y, 0);
    box.bboxQuaternion = getQuaternion(pose.rotation.yaw);
    box.cube_length = 4;
    box.cube_width = 2;
    box.cube_height = 2;
    renderBox(viewer, box, num, color, alpha);
}

int main(){


    auto client = cc::Client("localhost", 2000);
    client.SetTimeout(2s);
    auto world = client.GetWorld();

    auto blueprint_library = world.GetBlueprintLibrary();
    auto vehicles = blueprint_library->Filter("vehicle");

    auto map = world.GetMap();
    auto transform = map->GetRecommendedSpawnPoints()[1];
    auto ego_actor = world.SpawnActor((*vehicles)[12], transform);

    //Create lidar
    auto lidar_bp = *(blueprint_library->Find("sensor.lidar.ray_cast"));
    // CANDO: Can modify lidar values to get different scan resolutions
    lidar_bp.SetAttribute("upper_fov", "15");
    lidar_bp.SetAttribute("lower_fov", "-25");
    lidar_bp.SetAttribute("channels", "32");
    lidar_bp.SetAttribute("range", "30");
    lidar_bp.SetAttribute("rotation_frequency", "30");
    lidar_bp.SetAttribute("points_per_second", "500000");

    auto user_offset = cg::Location(0, 0, 0);
    auto lidar_transform = cg::Transform(cg::Location(-0.5, 0, 1.8) + user_offset);
    auto lidar_actor = world.SpawnActor(lidar_bp, lidar_transform, ego_actor.get());
    auto lidar = boost::static_pointer_cast<cc::Sensor>(lidar_actor);
    bool new_scan = true;
    std::chrono::time_point<std::chrono::system_clock> lastScanTime;

    pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
      viewer->setBackgroundColor (0, 0, 0);
    viewer->registerKeyboardCallback(keyboardEventOccurred, (void*)&viewer);

    auto vehicle = boost::static_pointer_cast<cc::Vehicle>(ego_actor);
    Pose pose(Point(0,0,0), Rotate(0,0,0));
    Pose poseRef(Point(vehicle->GetTransform().location.x, vehicle->GetTransform().location.y, vehicle->GetTransform().location.z), Rotate(vehicle->GetTransform().rotation.yaw * pi/180, vehicle->GetTransform().rotation.pitch * pi/180, vehicle->GetTransform().rotation.roll * pi/180));

    lidar->Listen([&new_scan, &lastScanTime, &pose, &viewer](auto data){

        if(new_scan){
            auto scan = boost::static_pointer_cast<csd::LidarMeasurement>(data);

            Eigen::Matrix4d transform = transform3D(pose.rotation.yaw, pose.rotation.pitch, pose.rotation.roll, pose.position.x, pose.position.y, pose.position.z);
            for (auto detection : *scan){
                if((detection.point.x*detection.point.x + detection.point.y*detection.point.y + detection.point.z*detection.point.z) > 8.0){ // Don't include points touching ego
                    Eigen::Vector4d local_point(detection.point.x, detection.point.y, detection.point.z, 1);
                    Eigen::Vector4d transform_point = transform * local_point;
                    pclCloud.points.push_back(PointT(transform_point[0], transform_point[1], transform_point[2]));
                }

            }
            if(pclCloud.points.size() > 5000) // CANDO: Can modify this value to get different scan resolutions
                new_scan = false;

            lastScanTime = std::chrono::system_clock::now();

            PointCloudT::Ptr scanCloud(new PointCloudT);
            *scanCloud = pclCloud;
            viewer->removeAllPointClouds();
            renderPointCloud(viewer, scanCloud, "map", Color(0,0,1)); 

        }

    });

    vector<Point> scanPoses = {Point(poseRef.position.x, poseRef.position.y, poseRef.position.z)};

    while (!viewer->wasStopped () && !save_map)
      {
        while(new_scan){
            std::this_thread::sleep_for(0.1s);
            world.Tick(1s);
        }

        if(refresh_view){
            viewer->setCameraPosition(pose.position.x, pose.position.y, 60, pose.position.x+1, pose.position.y+1, 0, 0, 0, 1);
            refresh_view = false;
        }

        viewer->removeAllShapes();
        pose = Pose(Point(vehicle->GetTransform().location.x, vehicle->GetTransform().location.y, vehicle->GetTransform().location.z), Rotate(vehicle->GetTransform().rotation.yaw * pi/180, vehicle->GetTransform().rotation.pitch * pi/180, vehicle->GetTransform().rotation.roll * pi/180)) - poseRef;

        drawCar(pose, 0,  Color(1,0,0), 0.7, viewer);
        double theta = pose.rotation.yaw;
        double stheta = control.steer * pi/4 + theta;
        viewer->removeShape("steer");
        renderRay(viewer, Point(pose.position.x+2*cos(theta), pose.position.y+2*sin(theta),pose.position.z),  Point(pose.position.x+4*cos(stheta), pose.position.y+4*sin(stheta),pose.position.z), "steer", Color(0,1,0));

        ControlState accuate(0, 0, 1);
        if(cs.size() > 0){
            accuate = cs.back();
            cs.clear();

            Accuate(accuate, control);
            vehicle->ApplyControl(control);
        }

        viewer->spinOnce ();

        currentTime = std::chrono::system_clock::now();
        if(!new_scan){
            std::chrono::duration<double> last_scan_seconds = currentTime - lastScanTime; 
            if(last_scan_seconds.count() > 1.0 && minDistance(Point(pose.position.x, pose.position.y, pose.position.z), scanPoses) > 5.0){
                scanPoses.push_back(Point(pose.position.x, pose.position.y, pose.position.z));
                new_scan = true;

            }
        }
      }

    // save the point cloud map
    PointCloudT::Ptr scanCloud(new PointCloudT);
    *scanCloud = pclCloud;
    scanCloud->width = scanCloud->points.size();
    scanCloud->height = 1;

    // TODO: Downsample the map point cloud using a voxel filter

    pcl::io::savePCDFileASCII ("my_map.pcd", *scanCloud);
    cout << "saved pcd map" << endl;

    return 0;
}

附：

CARLA Simulator:一个开源模拟器，CARLA Simulator，用于自动驾驶汽车技术堆栈的各个方面的培训和测试，包括计算机视觉、传感器融合、定位、运动规划等。在本课程中，我们利用它来帮助创建模拟的3D点云图。

点云库(PCL):用于处理点云的强大库。参见：Point Cloud Library (PCL): PCL API Documentation

其它参考资源

• Matrix Calculator---Matrix Calculator
• PCL Voxel Grid Tutorial---Downsampling a PointCloud using a VoxelGrid filter — Point Cloud Library 1.12.1-dev documentation
• ICP in PCL Tutorial---How to use iterative closest point — Point Cloud Library 1.12.1-dev documentation
• Full list of ICP Parameters---pcl: pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar > Class Template Reference
• NDT in PCL Tutorial---How to use Normal Distributions Transform — Point Cloud Library 1.12.1-dev documentation