Skip to content

Commit

Permalink
second commit
Browse files Browse the repository at this point in the history
  • Loading branch information
@zoumaguanxin
zoumaguanxin committed Jul 21, 2019
1 parent d36ecf1 commit af7cb1b
Show file tree
Hide file tree
Showing 308 changed files with 572 additions and 20,667 deletions.
3 changes: 2 additions & 1 deletion .gitignore
100644 → 100755
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,3 @@
/build/
/build
*.kdev4
build/
9 changes: 7 additions & 2 deletions .kdev4/rCSM.kdev4
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,5 +14,10 @@ Environment Profile=
Extra Arguments=
Install Directory=file:///usr/local

[Project]
VersionControlSupport=kdevgit
[Defines And Includes][Compiler]
Name=GCC
Path=gcc
Type=GCC

[SourceFileTemplates]
LastUsedTemplate=/home/shaoan/.kde/share/apps/kdevfiletemplates/template_descriptions/cpp_basic.desktop
18 changes: 17 additions & 1 deletion CMakeLists.txt
100644 → 100755
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,18 @@ cmake_minimum_required(VERSION 2.8)

project(rcsm)

set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native")

include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
endif()

find_package(catkin REQUIRED COMPONENTS
roscpp
tf
Expand All @@ -17,7 +29,11 @@ find_package(PCL 1.7 REQUIRED)
find_package(Eigen3 REQUIRED)

include_directories(${catkin_INCLUDE_DIRS})
include_directories(${PCL_INCLUDE_DIRS})
include_directories(${PCL_INCLUDE_DIRS} src/)

SET(SRCS src/point_cloud.cpp src/real_time_correlative_scan_matcher.cpp)

add_library(scanMatcher ${SRCS})

add_executable(rcsm main.cpp)

Expand Down
134 changes: 72 additions & 62 deletions CorrelativeMatch.hpp
100644 → 100755
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ class likelihoodFiled{
}

/*
* TODO 复制构造函数
* TODO 复制构造函数
*
*/

Expand Down Expand Up @@ -86,40 +86,40 @@ Eigen::MatrixXf generateGuassKernal(const int &sizes)
{
if((sizes%2)!=1)
{
cout<<"sizes必须是一个奇数"<<endl;
cout<<"sizes必须是一个奇数"<<endl;
assert((sizes%2)==1);
}
Eigen::MatrixXf K;
K.resize(sizes,sizes);
//只计算1/8的内容，其他全部复制
//只计算1/8的内容，其他全部复制
for(int i=0;i<(sizes/2+1);i++)
{
for(int j=i;j<(sizes/2+1);j++)
{
Eigen::Vector2f temp;
temp<<(sizes/2-i)*map_resolution,(sizes/2-j)*map_resolution;
K(i,j)=1/((2*3.141692f)*pow(sigma,2))*exp(-temp.squaredNorm()/(2*pow(sigma,2)));
//沿着以矩阵列方向中心线复制
K(i,j)=log(1/((2*3.141692f)*pow(sigma,2))*exp(-temp.squaredNorm()/(2*pow(sigma,2))));
//沿着以矩阵列方向中心线复制
K(i,sizes-j-1)=K(i,j);
if(i!=j)
{
//因为是对角矩阵,复制对角
//因为是对角矩阵,复制对角
K(j,i)=K(i,j);
K(j,sizes-i-1)=K(j,i);
}
if((sizes-i-1)!=i)
{
//以矩阵的行方向为中心线进行复制
//以矩阵的行方向为中心线进行复制
K.block(sizes-i-1,0,1,sizes)=K.block(i,0,1,sizes);
}
}
}
//除以最大数，因为我们希望中心是最亮的
//除以最大数，因为我们希望中心是最亮的
return K/K.maxCoeff();
}

/**
* \brief 对每一个点都创建一个似然场区域
* \brief 对每一个点都创建一个似然场区域
*/
bool smear()
{
Expand Down Expand Up @@ -147,7 +147,7 @@ Eigen::MatrixXf generateGuassKernal(const int &sizes)
{
if(m>=Windowsizes||n>=Windowsizes)
{
cout<<"矩阵索引超出边界"<<endl;
cout<<"矩阵索引超出边界"<<endl;
assert(m<Windowsizes&&n<Windowsizes);
}
likehoodFiledMap(i,j)+=guassK(m,n);
Expand All @@ -169,7 +169,7 @@ Eigen::MatrixXf generateGuassKernal(const int &sizes)
{
if(m>=Windowsizes||n>=Windowsizes)
{
cout<<"矩阵索引超出边界"<<endl;
cout<<"矩阵索引超出边界"<<endl;
assert(m<Windowsizes&&n<Windowsizes);
}
likehoodFiledMap(i,j)=guassK(m,n);
Expand All @@ -184,7 +184,7 @@ Eigen::MatrixXf generateGuassKernal(const int &sizes)


/**
* \brief 是用点云更新，点云必须是已经为地图坐标系下
* \brief 是用点云更新，点云必须是已经为地图坐标系下
*/
Eigen::MatrixXf update(const sensor_msgs::PointCloud &mapPC)
{
Expand All @@ -201,7 +201,7 @@ Eigen::MatrixXf generateGuassKernal(const int &sizes)


/**
* 初始化为零，重新创建
* 初始化为零，重新创建
*/
Eigen::MatrixXf create(const sensor_msgs::PointCloud &mapPC)
{
Expand Down Expand Up @@ -241,21 +241,21 @@ Eigen::MatrixXf generateGuassKernal(const int &sizes)
private:

float map_resolution;
//区域的中心
//区域的中心
Eigen::Vector2f centriod;
//更新的区域大小，Windowsizes * Windowsizes的区域，为栅格个数，必须设置为奇数，表示区域
//更新的区域大小，Windowsizes * Windowsizes的区域，为栅格个数，必须设置为奇数，表示区域
int Windowsizes;
//用来计算概率
//用来计算概率
float sigma;
//存放点云，用来更新似然场
//存放点云，用来更新似然场
sensor_msgs::PointCloud point_cloud;
//似然场
//似然场
Eigen::MatrixXf likehoodFiledMap;
//决定似然场的大小
//决定似然场的大小
int mapSizes;
//地图坐标系原点在矩阵坐标系下的位置，一般设置为在矩阵的中心
//地图坐标系原点在矩阵坐标系下的位置，一般设置为在矩阵的中心
Eigen::Vector2i map_origin;
//存放高斯核，用于更新似然场
//存放高斯核，用于更新似然场
Eigen::MatrixXf guassK;
};

Expand All @@ -264,6 +264,12 @@ Eigen::MatrixXf generateGuassKernal(const int &sizes)
class CorrelativeMatch{

public:
enum lookMethod
{
bruteForce,
slice2D,
MutiResolution
};
CorrelativeMatch() {
searchMethod="brute force";
favor_threshold=1.0;
Expand All @@ -275,7 +281,7 @@ class CorrelativeMatch{
}

/*
* 复制构造函数
* 复制构造函数
* TODO
*
*/
Expand Down Expand Up @@ -304,7 +310,7 @@ class CorrelativeMatch{


/**
* 使用暴力查找的方法
* 使用暴力查找的方法
*/
Eigen::Vector3f BruteForceLookup() const
{
Expand Down Expand Up @@ -335,7 +341,7 @@ class CorrelativeMatch{
float scoreValue=score(state);
if(scoreValue>score_Max&&scoreValue<=bound)
{
cout<<"发现一个更好的pose"<<endl;
cout<<"发现一个更好的pose"<<endl;
score_Max=scoreValue;
beset_state=state;
}
Expand All @@ -352,29 +358,22 @@ class CorrelativeMatch{



//分片的目的是为了节约运行时间，如我们在旋转这一层先计算旋转，那么内部就可以少计算N*N次旋转，
//分片的目的是为了节约运行时间，如我们在旋转这一层先计算旋转，那么内部就可以少计算N*N次旋转，
Eigen::Vector3f SilceLookup() const
{
float bound=pc_base.points.size()*favor_threshold*llf.getMaxCoffsGuassianK();
float score_Max=score(poseWindowCentriod);
if(score_Max>=bound)
{
return poseWindowCentriod;
}

}
int theta_sizes=floor(poseWindowSizes(2)/orientation_resolution);
int x_sizes=floor(poseWindowSizes(0)/x_resolution);
int y_sizes=floor(poseWindowSizes(1)/y_resolution);


Eigen::Vector3f best_state=poseWindowCentriod;




int y_sizes=floor(poseWindowSizes(1)/y_resolution);
Eigen::Vector3f best_state=poseWindowCentriod;
for(int i=0;i<theta_sizes;i++)
{
sensor_msgs::PointCloud TransformedPC;
sensor_msgs::PointCloud RotatededPC;
float theta=poseWindowCentriod(2)-poseWindowSizes(2)/2+i*orientation_resolution;
Eigen::Quaternion<float> q(cos(theta)/2,0,0,sin(theta)/2);
for(int i_=0;i_<pc_base.points.size();i_++)
Expand All @@ -384,7 +383,7 @@ class CorrelativeMatch{
tempoint=q.toRotationMatrix()*tempoint;
geometry_msgs::Point32 tempointx;
tempointx.x=tempoint(0);tempointx.y=tempoint(1);tempointx.z=0;
TransformedPC.points.push_back(tempointx);
RotatededPC.points.push_back(tempointx);
}

for(int j=0;j<x_sizes;j++)
Expand All @@ -395,20 +394,20 @@ class CorrelativeMatch{
{
float pose_y=poseWindowCentriod(1)-poseWindowSizes(1)/2+k*y_resolution;
float sum=1;
for(int ii=0;ii<TransformedPC.points.size();ii++)
for(int ii=0;ii<RotatededPC.points.size();ii++)
{
Eigen::Vector2i temIndex;
Eigen::Vector2f point2d;
point2d(0)=TransformedPC.points[ii].x+pose_x;
point2d(1)=TransformedPC.points[ii].y+pose_y;
point2d(0)=RotatededPC.points[ii].x+pose_x;
point2d(1)=RotatededPC.points[ii].y+pose_y;
temIndex=preDeal::world2grid(llf.getMapResolution(),llf.getMapOrigin(),point2d);
if(temIndex(0)>=0&&temIndex(0)<llfmap.rows() &&temIndex(1)>=0&&temIndex(1)<llfmap.cols())
sum*=llfmap(temIndex(0),temIndex(1));
sum+=llfmap(temIndex(0),temIndex(1));
}
Eigen:: Vector3f state(pose_x,pose_y,theta);
if(sum>score_Max&&sum<=bound)
{
cout<<"发现一个更好的pose"<<endl;
cout<<"发现一个更好的pose"<<endl;
score_Max=sum;
best_state=state;
}
Expand All @@ -425,13 +424,12 @@ class CorrelativeMatch{




//在点云常用的缩写是pc
//需要验证，ros中所有的stampTransform代表的意思都是得到转换关系都是把child_frame_id数据处理到frame_id下，而且设置transform时，也是必须按照这个设置
//在点云常用的缩写是pc
//需要验证，ros中所有的stampTransform代表的意思都是得到转换关系都是把child_frame_id数据处理到frame_id下，而且设置transform时，也是必须按照这个设置

/**
* \brief 对转换关系进行打分，分值等于所有的激光hits的点被转换到map坐标系下，在似然场中的分数的乘积，比用求和的效果要好
* \param[in] tf_ laser到map的转换关系
* \brief 对转换关系进行打分，分值等于所有的激光hits的点被转换到map坐标系下，在似然场中的分数的乘积，比用求和的效果要好
* \param[in] tf_ laser到map的转换关系
*/
float score(const tf::Transform &tf_) const
{
Expand All @@ -448,7 +446,7 @@ class CorrelativeMatch{
point2d<<tempoint(0),tempoint(1);
temIndex=preDeal::world2grid(llf.getMapResolution(),llf.getMapOrigin(),point2d);
if(temIndex(0)>=0&&temIndex(0)<llfmap.rows() &&temIndex(1)>=0&&temIndex(1)<llfmap.cols())
tf_score*=llfmap(temIndex(0),temIndex(1));
tf_score+=llfmap(temIndex(0),temIndex(1));
}
}

Expand All @@ -475,7 +473,7 @@ class CorrelativeMatch{

/**
* \brief update likehood field
* \param[in] llf_ 用于计算的似然场
* \param[in] llf_ 用于计算的似然场
*/
void updataikehoodField(const likelihoodFiled &llf_)
{
Expand All @@ -496,7 +494,7 @@ class CorrelativeMatch{
}
}

/*错误的做法
/*错误的做法
Eigen::Vector3f getCorrelativePose(const sensor_msgs::LaserScan & scan_,const Eigen::Vector3f &poseWindowCentriod_)
{
preDeal::scan2pc(scan_,currentPCinLaserFrame);
Expand All @@ -520,7 +518,7 @@ class CorrelativeMatch{
}
else
{
cout<<"没有相应的搜索方法可以匹配，请重新输入 "<<endl;
cout<<"没有相应的搜索方法可以匹配，请重新输入 "<<endl;
exit(0);
}
// relativePose=lookup();
Expand All @@ -529,31 +527,43 @@ class CorrelativeMatch{
}

private:
//搜索时x的分辨率
//搜索时x的分辨率
float x_resolution;
//搜索时的分辨率
//搜索时的分辨率
float y_resolution;
//搜索时角度的分辨虚
//搜索时角度的分辨虚
float orientation_resolution;
//窗的位置，由中心决定
//窗的位置，由中心决定
Eigen::Vector3f poseWindowCentriod;
//窗的尺寸，单位是米，认为传感器的误差越大，则窗应该设置的越大
//窗的尺寸，单位是米，认为传感器的误差越大，则窗应该设置的越大
Eigen::Vector3f poseWindowSizes;
//目前不是相对的，得到的直接就是位姿
//目前不是相对的，得到的直接就是位姿
Eigen::Vector3f relativePose;
//在base坐标系下的点云数据
//在base坐标系下的点云数据
sensor_msgs::PointCloud pc_base;
//似然场
//似然场
likelihoodFiled llf;
//存放似然场的地图信息
//存放似然场的地图信息
Eigen::MatrixXf llfmap;
//搜索位姿的方法
//搜索位姿的方法
string searchMethod;
//阈值
//阈值
float favor_threshold;
};


class RealTimeCorrelativeScanMatch: public CorrelativeMatch
{
public:
RealTimeCorrelativeScanMatch(){}

private:
Eigen::MatrixXf generateMultiResoltionMap()
{}

};


}


Expand Down
Empty file modified LICENSE
100644 → 100755
View file
Empty file.
Empty file modified README.md
100644 → 100755
View file
Empty file.
5 changes: 5 additions & 0 deletions alonetest.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
#include <boost/graph/graph_concepts.hpp>
int main()
{

}
Empty file removed build/CATKIN_IGNORE
View file
Empty file.
Loading

0 comments on commit af7cb1b

Please sign in to comment.