基于ORB-SLAM2的语义地图构建，分成服务端和客户端
注：视觉SLAM算法使用ORBSLAM2，语义分割网络模型使用deeplabv2。
源码分析
Github：Semantic_Mapping_on_ORBSLAM2
服务端
源码来自deeplabv2_server/server.py。
服务端接收客户端传送的图像（字节串），然后进行语义分割probs = inference.runSegModel(img)，再将语义分割的结果传送回客户端。
注：使用的语义分割模型可以根据需求自己选择，需要注意环境的配置。
#!/usr/bin/env python
# coding=utf-8
from socket import *
import cv2
import matplotlib.pyplot as plt
import numpy as np
import inference
HOST = '127.0.0.1'
PORT = 8080
ADDR = (HOST,PORT)
def recvall(sock, count):
buf = b'' # buf是一个bytes（字节串）
while count:
# 接受TCP套接字的数据。数据以字符串形式返回，count指定要接收的最大数据量.
newbuf = sock.recv(count)
if not newbuf:
return None
buf += newbuf
count -= len(newbuf)
print(count)
return buf
# 语义分割模型初始化
inference.init()
# 创建AF_INET地址族，TCP的套接字
with socket(AF_INET,SOCK_STREAM) as tcpSerSock:
# 绑定ip和端口
tcpSerSock.bind(ADDR)
# 监听端口，是否有请求
tcpSerSock.listen(5)
print("Listen on port",PORT)
while True:
# accept() 是阻塞的
tcpClientSock,addr = tcpSerSock.accept()
print("The client: ",addr,"is connecting.")
with tcpClientSock:
# 使用一个while循环，持续和客户端通信，直到客户端断开连接或者崩溃
while True:
# 接收图像大小
count = recvall(tcpClientSock,16)
if not count:
break
print("The file size: ",(int)(count))
# 接收图像
stringData = recvall(tcpClientSock,(int)(count))
if not stringData:
break
# 字节串转换成uint8类型数组
data = np.frombuffer(stringData, np.uint8)
# 将uint8类型数组解码成图像
img = cv2.imdecode(data, cv2.IMREAD_COLOR)
print(img.shape)
# 图像语义分割返回“概率图”
probs = inference.runSegModel(img)
print(type(probs)) # 客户端
源码来自src/ImageSeg.cc。
创建了ImageSeg类专门用于进行图像的语义分割。ImageSeg::connectServer()用于与服务端建立连接，ImageSeg::handleImage(const cv::Mat& imRGB)用于传送图像给服务端并接收返回的语义分割结果。
#include "ImageSeg.h"
namespace ORB_SLAM2
{
// public
ImageSeg::ImageSeg():mClientSockfd(-1), mIsFirstImage(true){}
ImageSeg::~ImageSeg(){
this->closeClientSockfd();
}
bool ImageSeg::connectServer()
{
int client_sockfd = socket(AF_INET, SOCK_STREAM, 0);
if (client_sockfd == -1) {
perror("Socket error");
return false;
}
struct sockaddr_in client_addr;
bzero(&client_addr, sizeof(client_addr));
client_addr.sin_family = AF_INET;
client_addr.sin_port = htons(Global::PORT);
inet_pton(AF_INET,Global::HOSTADDR.c_str(), &client_addr.sin_addr.s_addr);
int ret = connect(client_sockfd, (struct sockaddr*)&client_addr, sizeof(client_addr));
if (ret == -1) {
perror("Connect error");
return false;
}
char ipbuf[128];
printf("Connect successfully! client iP: %s, port: %d\n", inet_ntop(AF_INET, &client_addr.sin_addr.s_addr, ipbuf,
sizeof(ipbuf)), ntohs(client_addr.sin_port));
mClientSockfd = client_sockfd;
return true;
}
void ImageSeg::handleImage(const cv::Mat& imRGB)
{
char* buf = new char[BUFFERSIZE];
int bufLength = this->transMatToCharArray(buf, imRGB);
cout << bufLength << endl;
// int转char数组
char lenStrSend[16];
stringstream ss;
ss << bufLength;
ss >> lenStrSend;
for (int i = strlen(lenStrSend); i < sizeof(lenStrSend) ; i++) {
lenStrSend[i] = ' ';
}
send(mClientSockfd, lenStrSend, sizeof(lenStrSend), 0);
send(mClientSockfd, buf, bufLength, 0);
delete [] buf;
char lenStrRec[16];
// 阻塞读取
int len = recv(mClientSockfd, lenStrRec, sizeof(lenStrRec), 0);
if (!isRecvSuccess(len)) {
return;
}
int size;
stringstream stream(lenStrRec);
stream >> size;
char* recBuf = new char[BUFFERSIZE];
len = 0;
do {
int l = recv(mClientSockfd, recBuf+len, BUFFERSIZE-len, 0);
if (!isRecvSuccess(l)) {
return;
}
len += l;
}while(len < size);
for (int c = 0; c < Global::CHANNELS; c++) {
for (int h = 0; h < Global::HEIGHT; h++) {
for (int w = 0; w < Global::WIDTH; w++) {
int index = c*Global::HEIGHT*Global::WIDTH + h*Global::WIDTH + w;
char c_heading[4];
for (int i = 0; i < 4; i++) { // float四个字节
c_heading[i] = recBuf[index*4+i];
}
if (mIsFirstImage) {
mProbMap.push_back(*((float*)(c_heading)));
}else {
mProbMap[index] = *((float*)(c_heading));
}
}
}
}
mIsFirstImage = false;
delete [] recBuf;
}
void ImageSeg::closeClientSockfd()
{
if (mClientSockfd != -1) {
close(mClientSockfd);
mClientSockfd = -1;
}
}
const vector<float>& ImageSeg::getProbMap()
{
return mProbMap;
}
// private
bool ImageSeg::isRecvSuccess(int len)
{
if (len == -1) {
perror("Read error");
}else if (len == 0){
printf("Without receiving data's Length from server.");
}else{
return true;
}
this->closeClientSockfd();
return false;
}
int ImageSeg::transMatToCharArray(char* modelImage, const cv::Mat& imRGB)
{
// 将cv::Mat数据编码成数据流
std::vector<uchar> buff;
cv::imencode(".png", imRGB, buff);
memset(modelImage, 0, BUFFERSIZE);
memcpy(modelImage, reinterpret_cast<char*>(&buff[0]), buff.size());
return buff.size();
}
}
ORBSLAM2中使用ImageSeg类
- 语义建图初始化时，与服务端建立连接。
PointCloudMappingClient::PointCloudMappingClient(string savePCDPath, double thprob, double thdepth): mCx(0), mCy(0), mFx(0), mFy(0), mbShutdown(false), mbFinish(false), mSavePCDPath(savePCDPath), mThdepth(thdepth), mThprob(thprob)
{
this->initColorMap();
mImgSeg.connectServer(); // 连接服务端
mPointCloud = boost::make_shared<PointCloud>(); // 用boost::make_shared<> ?
viewerThread = std::make_shared<std::thread>(&PointCloudMappingClient::showPointCloud, this);
}
- 生成点云时，进行语义分割，并返回概率图用一维vector保存。
void PointCloudMappingClient::generatePointCloud(const cv::Mat& imRGB, cv::Mat& imD, const cv::Mat& pose, int nId)
{
std::cout << "Converting image: " << nId << std::endl;
std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
mImgSeg.handleImage(imRGB); // 将图像发送给服务端进行语义分割，并返回概率图
vector<float> probMap = mImgSeg.getProbMap(); // 获取从服务端返回的概率图
this->mergePointCloudFromImage(probMap, imRGB, imD, pose); // 合并每一帧图像生成的点云
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
double t = std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
std::cout << "Cost = " << t << std::endl;
}
- 语义地图构建原理参见：基于ORB-SLAM2的语义地图构建