自宅をSLAMったソースコード

前々回くらいで自宅をSLAMったソースコードを載せます。
githubに載せたかったのですが、使い方が分からなかったのでこちらに直接載せます。

SLAMツール

mrpt-sample-appsのkinect-3d-slamを修正しました。
三次元座標のみを出力するだけだったのを、RGBも付けて出力するようにしました。
kinect-3d-slam_main.cppの518行目を以下のように修正するだけです。
※ mrpt-sample-appsのソースコードはサイトから直接落とせます。

	//globalPtsMap.save3D_to_text_file(s);
	globalPtsMap.save3D_and_colour_to_text_file(s);

点群ビューア

PCLをつかって上記のkinect-3d-slamが吐いた点群ファイルを読み込むソースコードを載せておきます。
@otlさんのソースコードを流用させた頂きしました。
大部分はSSVファイルの文字列操作の処理ですので公開する意味が乏しいですが.. そのまま動きますよ。

/**
 * @file View.cpp
 */

#include 
#include 
#include 
#include 
#include 
#include 

#define LOG printf

using namespace pcl;

// 文字列リスト.
typedef std::vector StringList;
// ファイル名.
static const std::string SAMPLE_FILE = "point_cloud.txt";

typedef PointCloud PointCloudXYZRGB;

// テキストからPointCloudXYZRGBを読み込む.
int LoadCloudText(PointCloudXYZRGB& ptCloud, std::string sFileName);
// 行から座標・RGBを読み込む.
int LintToXYZRGB(std::string sLine, 
	float& fX, float& fY, float& fZ, unsigned char& ucR, unsigned char& ucG, unsigned char& ucB);
// テキストファイルから行ごとに文字列を読み込む.
long ReadFileLine(StringList &strList, std::string sFileName);

int main(int argc, char *argv[])
{
	// ファイル名を引数から取得.
	std::string sFileName;
	if(argc > 1){
		LOG(" ARG: %s...", argv[1]);
		sFileName.append( argv[1] );
	}
	if(sFileName.length() < 2){
		LOG(" TARGET FILE: %s...", SAMPLE_FILE.c_str());
		sFileName.append( SAMPLE_FILE );
	}
	// テキストからロード.
	PointCloudXYZRGB ptCloud;
	int res = LoadCloudText(ptCloud, sFileName);
	if(res != 0){
		return res;
	}
	// 表示.
	visualization::CloudViewer viewer("Simple Viewer");
	viewer.showCloud(ptCloud.makeShared());
	while(1){
		if(viewer.wasStopped() == true){
			break;
		}
	}
	
	return 0;
}

int LoadCloudText(PointCloudXYZRGB& ptCloud, std::string sFileName)
{
	StringList sLineList;
	long res = ReadFileLine(sLineList, sFileName);
	LOG(" テキスト読み込み. res=%ld. file=%s.\n", res, sFileName.c_str());
	if(res != 0){
		LOG(" ERR: テキスト読み込み失敗.\n");
		return res;
	}
	const unsigned int numLine = sLineList.size();
	ptCloud.reserve(numLine);
	LOG(" 座標値読み込み. NUM=%d.\n", numLine);
	for(unsigned int nl=0; nl*1;
	fX = atof( sX.c_str() );
	//--------------------------
	// Yの取り出し.
	std::string sY = sLine;
	sY.erase(sY.begin(), sY.begin()+idxSP0+1);
	size_t idxSP1 = sY.find_first_of(sSP);
	if(idxSP1 == std::string::npos){
		return 2;
	}
	sY.erase(sY.begin()+idxSP1, sY.end());
	fY = atof( sY.c_str() );
	//--------------------------
	// Zの取り出し.
	std::string sZ = sLine;
	sZ.erase(sZ.begin(), sZ.begin()+idxSP0+1+idxSP1+1);
	size_t idxSP2 = sZ.find_first_of(sSP);
	if(idxSP2 == std::string::npos){
		return 3;
	}
	sZ.erase(sZ.begin()+idxSP2, sZ.end());
	fZ = atof( sZ.c_str() );
	//--------------------------
	// Rの取り出し.
	std::string sR = sLine;
	sR.erase(sR.begin(), sR.begin()+idxSP0+1+idxSP1+1+idxSP2+1);
	size_t idxSP3 = sR.find_first_of(sSP);
	if(idxSP3 == std::string::npos){
		return 4;
	}
	sR.erase(sR.begin()+idxSP3, sR.end());
	int iR = atoi(sR.c_str());
	ucR = (unsigned char)iR;
	//--------------------------
	// Gの取り出し.
	std::string sG = sLine;
	sG.erase(sG.begin(), sG.begin()+idxSP0+1+idxSP1+1+idxSP2+1+idxSP3+1);
	size_t idxSP4 = sG.find_first_of(sSP);
	if(idxSP4 == std::string::npos){
		return 5;
	}
	sG.erase(sG.begin()+idxSP4, sG.end());
	int iG = atoi(sG.c_str());
	ucG = (unsigned char)iG;
	//--------------------------
	// Bの取り出し.
	std::string sB =sLine;
	sB.erase(sB.begin(), sB.begin()+idxSP0+1+idxSP1+1+idxSP2+1+idxSP3+1+idxSP4+1);
	int iB = atoi(sB.c_str());
	ucB = (unsigned char)iB;
		
	return 0;
}

long ReadFileLine(StringList &strList, std::string sFileName)
{
	if(sFileName.length() < 3){
		return -1;
	}
	std::ifstream fs;
	fs.open(sFileName.c_str());
	if(fs == false){
		return -1;
	}
	const int STRLEN = 128;
	char buf[STRLEN];
	for(int n=0; ; n++){
		memset(buf, 0, sizeof(char)*STRLEN);
		fs.getline(buf, sizeof(char)*STRLEN);
		if(fs.eof() == true){
			break; // 終端.
		}
		std::string sLine( buf );
		if(sLine.length() < 3){
			break; // 無効な文字列.
		}
		strList.push_back( sLine );
	}
	fs.close();

	return 0;
}

## Makefile ##
SRC = View.cpp
DST = ViewTest

prefix  = /usr
INCPATH = $(prefix)/include
LIBPATH = $(prefix)/lib 

OPT = \
	-lcv -lcvaux -lcxcore -lhighgui \
	-lpcl_common -lpcl_io -lpcl_visualization \
	-lstdc++
CC  = g++ -O

CFLAGS  = \
	-I$(INCPATH)/pcl-1.1 \
	-I$(INCPATH)/eigen3 \
	-I$(INCPATH)/vtk-5.4 
LDFLAGS = -L. -L$(LIBPATH) 

all:
	$(CC) $(SRC)  -o $(DST) $(CFLAGS)  $(LDFLAGS) $(OPT)

clean:
	rm -fr $(DST)

まとめ

点群をポリゴンにするコードはテクスチャが貼れたら公開しようと思っています。

*1:uint32_t)iR << 16 | (uint32_t)iG << 8 | (uint32_t)iB); pt.rgb = *reinterpret_cast(&uiRGB); //pt.rgb = (iR << 16 | iG << 8 | iB); ptCloud.push_back(pt); } return 0; } int LintToXYZRGB(std::string sLine, float& fX, float& fY, float& fZ, unsigned char& ucR, unsigned char& ucG, unsigned char& ucB) { static const std::string sSP = " "; // スペース. //-------------------------- // Xの取り出し. std::string sX = sLine; size_t idxSP0 = sX.find_first_of(sSP); if(idxSP0 == std::string::npos){ return 1; } sX.erase(sX.begin()+idxSP0, sX.end(