freespace_evidence
根据视点计算点云的freespace_evidence
参考资料：
Bresenham's line algorithm：https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
Bresenham in 3D algorithm: http://members.chello.at/~easyfilter/bresenham.html
https://www.cnblogs.com/wlzy/p/8695226.html
//计算自由空间栅格，依赖视点
void qMIMSPlugin::doRasterFreeSpace()
{
//选择文件夹，设置平面点云提取参数
ccNDTFuisonDlg dlg;
dlg.cellRaidiusSpinBox->setValue(.5f);//注意此处设置计算法向量的参数
dlg.cellepsilonDoubleSpinBox->setValue(.);
dlg.epsilonDoubleSpinBox->setValue(.); // set distance threshold to 0.5% of bounding box width if (!dlg.exec())
return;
//获取参数
double leaf_size=dlg.cellRaidiusSpinBox->value();
QString mFolderPath=dlg.txtPath->text();
int startIdx=dlg.spinBox->value();
int endIdx=dlg.spinBox_2->value();
double ratio=1.0;
double isPlyformat=;
Eigen::Vector3d bbMin;
Eigen::Vector3d bbMax;
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_PointClouds;
//循环读取点云数据和相机（视点信息）
for (int idx=startIdx;idx<endIdx;idx++)
{
//读取点云数据
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
//读取每一帧点云
char a[],b[];
sprintf(a, "%03d",idx);
if (isPlyformat)
{
string filename=mFolderPath.toStdString() + "\\scan" + a + ".ply";
pcl::PLYReader reader;
if (reader.read(filename,*cloud) == -)
{ PCL_ERROR ("Couldn't read file *.pcd \n");
m_app->dispToConsole("Read PCD file failed!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
break;
}
}
else
{
string filename=mFolderPath.toStdString() + "\\scan" + a + ".pcd";
if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud) == -)
{
PCL_ERROR ("Couldn't read file *.pcd \n");
m_app->dispToConsole("Read PCD file failed!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
break;
}
}
//包围盒叠加,存储极值的两个点
pcl::PointXYZ minPt, maxPt;
//获取坐标极值
pcl::getMinMax3D(*cloud, minPt, maxPt); bbMin[]=std::min(bbMin[],(double)minPt.x);
bbMin[]=std::min(bbMin[],(double)minPt.y);
bbMin[]=std::min(bbMin[],(double)minPt.z);
bbMax[]=std::max(bbMax[],(double)maxPt.x);
bbMax[]=std::max(bbMax[],(double)maxPt.y);
bbMax[]=std::max(bbMax[],(double)maxPt.z);
//视点信息
Eigen::Vector4f origin_=cloud->sensor_origin_;
m_PointClouds.push_back(cloud);
}
//根据包围盒计算栅格数据的长宽高
Eigen::Vector3d diff = bbMax - bbMin;
double scale=std::max(std::max(diff[], diff[]), diff[]);
Eigen::Vector3d b0 =bbMin -0.05*diff;
double xmax=bbMax[]; double ymax=bbMax[]; double zmax=bbMax[];
double xmin=bbMin[]; double ymin=bbMin[]; double zmin=bbMin[];
unsigned gNumX = ceil(1.10*(xmax-xmin)/leaf_size);
unsigned gNumY = ceil(1.10*(ymax-ymin)/leaf_size);
unsigned gNumZ = ceil(1.10*(zmax-zmin)/leaf_size); double *freespace_evidence = new double[gNumX*gNumY*gNumZ];
for (int idt=;idt<gNumX*gNumY*gNumZ;idt++)
{
freespace_evidence[idt]=;
}
//遍历每一个点云
for (int idx=;idx<m_PointClouds.size();idx++)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud=m_PointClouds[idx];
Eigen::Vector4f origin_=cloud->sensor_origin_;
int pointCount=cloud->size();
//视点信息
Eigen::Vector4f viewPoint=cloud->sensor_origin_; Eigen::Vector3d p0=Eigen::Vector3d::Zero();
p0[] = viewPoint[];
p0[] = viewPoint[];
p0[] = viewPoint[]; //遍历每一个点
for (int jdx=;jdx<pointCount;jdx++)
{
Eigen::Vector3d p1=Eigen::Vector3d::Zero();
p1[] = cloud->points[jdx].x;
p1[] = cloud->points[jdx].y;
p1[] = cloud->points[jdx].z; Eigen::Vector3d X=Eigen::Vector3d::Zero();
X[] = floor((p0[]-b0[])/leaf_size);
X[] = floor((p0[]-b0[])/leaf_size);
X[] = floor((p0[]-b0[])/leaf_size);//相机在栅格中的位置 Eigen::Vector3d Y=Eigen::Vector3d::Zero();
Y[] = floor((p1[]-b0[])/leaf_size);
Y[] = floor((p1[]-b0[])/leaf_size);
Y[] = floor((p1[]-b0[])/leaf_size);//点在栅格中的位置 Eigen::Vector3d v=Eigen::Vector3d::Zero();
v[] = p1[] - p0[];
v[] = p1[] - p0[];
v[] = p1[] - p0[]; Eigen::Vector3d step=Eigen::Vector3d::Zero();
step[] = sign(v[]);
step[] = sign(v[]);
step[] = sign(v[]); Eigen::Vector3d tDelta=Eigen::Vector3d::Zero();
tDelta[] = abs(leaf_size/v[]);
tDelta[] = abs(leaf_size/v[]);
tDelta[] = abs(leaf_size/v[]); Eigen::Vector3d tMax=Eigen::Vector3d::Zero();
tMax[] = abs((0.5*(-step[])*leaf_size-(b0[] + leaf_size*(X[]+) - p0[]))/v[]);
tMax[] = abs((0.5*(-step[])*leaf_size-(b0[] + leaf_size*(X[]+) - p0[]))/v[]);
tMax[] = abs((0.5*(-step[])*leaf_size-(b0[] + leaf_size*(X[]+) - p0[]))/v[]); int count = ;
unsigned long long index;
vector<int> xlist;
vector<int> ylist;
vector<int> zlist;
while(true)
{
//计算视点和点之间的关系
if (X[] == Y[] && X[] == Y[] && X[] == Y[]) break; if (X[] < || X[] >= gNumX || X[] < || X[] >= gNumY || X[] < || X[] >= gNumZ) break; if (tMax[] < tMax[])
{
if (tMax[] < tMax[])
{
tMax[] = tMax[] + tDelta[];
X[] = X[] + step[];
index = X[]*gNumX*gNumY+X[]*gNumY+X[];
if(X[]>= && X[]<gNumX && X[]>= && X[]<gNumY && X[]>= && X[]<gNumZ)
{
xlist.push_back(X[]);
ylist.push_back(X[]);
zlist.push_back(X[]);
} }
else
{
tMax[] = tMax[] + tDelta[];
X[] = X[] + step[];
index = X[]*gNumX*gNumY+X[]*gNumY+X[];
if(X[]>= && X[]<gNumX && X[]>= && X[]<gNumY && X[]>= && X[]<gNumZ)
{
xlist.push_back(X[]);
ylist.push_back(X[]);
zlist.push_back(X[]);
} }
}
else
{
if (tMax[] < tMax[])
{
tMax[] = tMax[] + tDelta[];
X[] = X[] + step[];
index = X[]*gNumX*gNumY+X[]*gNumY+X[];
if(X[]>= && X[]<gNumX && X[]>= && X[]<gNumY && X[]>= && X[]<gNumZ)
{
xlist.push_back(X[]);
ylist.push_back(X[]);
zlist.push_back(X[]);
}
}
else
{
tMax[] = tMax[] + tDelta[];
X[] = X[] + step[];
index = X[]*gNumX*gNumY+X[]*gNumY+X[];
if(X[]>= && X[]<gNumX && X[]>= && X[]<gNumY && X[]>= && X[]<gNumZ)
{
xlist.push_back(X[]);
ylist.push_back(X[]);
zlist.push_back(X[]);
}
} }
} int size_list = xlist.size(); for(int j=;j<ratio*xlist.size();j++)
{
index = zlist[j]*gNumX*gNumY+xlist[j]*gNumY+ylist[j];
*(freespace_evidence+index) = *(freespace_evidence+index) + ;
}
}
}
//按照Z方向层数，计算累加的栅格概率 double *cellXOYs = new double[gNumX*gNumY];
for (int idt=;idt<gNumX*gNumY;idt++)
{
cellXOYs[idt]=;
}
for (int idx=;idx<gNumX;idx++)
{
for (int idy=;idy<gNumY;idy++)
{
for (int k=;k<gNumZ;k++)
{
//注意这个数组是以左下角点为原点的
unsigned long long idxImage = idx*gNumY+idy;
unsigned long long index = k*gNumX*gNumY+idx*gNumY+idy;
*(cellXOYs+idxImage)+=*(freespace_evidence+index);
}
}
} //完成投影2D栅格，保存
cv::Mat rgb1(gNumY,gNumX, CV_8UC1);
double dmax=;double dmin=;
for (int idx=;idx<gNumX;idx++)
{
for (int idy=;idy<gNumY;idy++)
{
unsigned long long idxImage = idx*gNumY+idy;//每个2D栅格中的值
double acc=*(cellXOYs+idxImage);
if (acc>dmax)
{
dmax=acc;
}
}
}
for (int row=;row<gNumY;row++)
{
for (int col=;col<gNumX;col++)
{
unsigned long long idxImage = col*gNumY+row;//每个2D栅格中的值，左下角点为坐标原点
double acc=*(cellXOYs+idxImage);
//rgb1.data[idx*gNumY+idy]=255*acc/(dmax-dmin);
if (acc>=)
{
//图像为左上角点为坐标原点,同时行列顺序也变量
rgb1.data[(gNumY-row-)*gNumX+col]=;
}
else
{
rgb1.data[(gNumY-row-)*gNumX+col]=acc;
} }
}
//cv::normalize(rgb1,rgb1,1.0,0.0,cv::NORM_MINMAX);
cv::Mat elementdilate = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(, ));
cv::Mat outdilate;
//进行膨胀操作
cv::dilate(rgb1, outdilate, elementdilate);
cv::imwrite( "D:\\freesapce.png", outdilate);
//计算左上角点坐标
double topleftX=b0[];
double topleftY=b0[]+gNumY*leaf_size;
FILE * coord=fopen("D:\\freesapce.pgw","w");
if (coord)
{
fprintf(coord,"%f\n",leaf_size);
fprintf(coord,"%f\n",0.000000);
fprintf(coord,"%f\n",0.000000);
fprintf(coord,"%f\n",-1.0*leaf_size);
fprintf(coord,"%f\n",topleftX);
fprintf(coord,"%f\n",topleftY);
fclose(coord);
}
delete[] freespace_evidence;
delete[] cellXOYs;
}
freespace_evidence的更多相关文章
随机推荐
- nohup 同时实现记录日志和屏幕输出
nohup nohup命令:如果你正在运行一个进程,而且你觉得在退出帐户时该进程还不会结束,那么可以使用nohup命令.该命令可以在你退出帐户/关闭终端之后继续运行相应的进程.nohup就是不挂断 ...
- eclipse搭建j2ee
Tomcat环境变量设置,分别添加3个系统变量 CATALINA_BASE E:/tomcat7 CATALINA_HOME E:/tomcat7 CATALINA_TMPDIR Etomcat7/t ...
- .NET Core 2.0 单元测试中初识 IOptionsMonitor<T>
在针对下面设置 CookieAuthenticationOptions 的扩展方法写单元测试时遇到了问题. public static IServiceCollection AddCnblogsAut ...
- Java元组Tuple介绍与使用
一.元组介绍 仅仅一次方法调用就可以返回多个对象,你应该经常需要这样的功能吧.可以return语句只允许返回单个对(可能有人说返回一个集合就可以了,请记住,一个集合也只是一个对象而已)因此,解决办法就 ...
- Some untracked working tree files would be overwritten by checkout. Please move or remove them before you can checkout. View them
Some untracked working tree files would be overwritten by checkout. Please move or remove them befor ...
- SQLServer 查询数据死锁语句
SQLServer 查询数据死锁语句 我们可以通过以下SQL 语句用来查询,当前数据中存在的死锁语句,定位数据库出现死锁的根本原因. --每秒死锁数量 SELECT * FROM sys.dm ...
- MySQL获取分组后的TOP 1和TOP N记录-转
有时会碰到一些需求,查询分组后的最大值,最小值所在的整行记录或者分组后的top n行的记录,在一些别的数据库可能有窗口函数可以方面的查出来,但是MySQL没有这些函数,没有直接的方法可以查出来,可通过 ...
- SQL行列转换6种方法
在进行报表开发时,很多时候会遇到行列转换操作,很对开发人员针对于SQL级别行列转换操作一直不甚理解,今天正好抽空对其进行了一些简单的总结.这里主要列举3种可以实现SQL行列转换的方法,包括通用SQL解 ...
- Linux 配置SFTP，配置用户访问权限
之前我服务器是使用的Windows Server 2003,这段时间由于访问量变大我还是机智的换成Linux了,在搭建FTP的时候看到网上都是推荐vsftpd,不过我不推荐这个家伙,看官且看下文. 我 ...
- zabbix设置中文并解决乱码问题
1.登录页面,设置中文 如下 2.解决乱码 进入本地PC的C:\Windows\Fonts,找到微软雅黑字体,复制粘贴,粘贴默认会生成两个文件 将msyh.ttf文件上传至zabbix服务器/usr/ ...