/********************************************************************
** Image Component Library (ICL) **
** **
** Copyright (C) 2006-2013 CITEC, University of Bielefeld **
** Neuroinformatics Group **
** Website: www.iclcv.org and **
** http://opensource.cit-ec.de/projects/icl **
** **
** File : ICLGeom/src/ICLGeom/CoPlanarityFeatureExtractor.cpp **
** Module : ICLGeom **
** Authors: Andre Ueckermann **
** **
** **
** GNU LESSER GENERAL PUBLIC LICENSE **
** This file may be used under the terms of the GNU Lesser General **
** Public License version 3.0 as published by the **
** **
** Free Software Foundation and appearing in the file LICENSE.LGPL **
** included in the packaging of this file. Please review the **
** following information to ensure the license requirements will **
** be met: http://www.gnu.org/licenses/lgpl-3.0.txt **
** **
** The development of this software was supported by the **
** Excellence Cluster EXC 277 Cognitive Interaction Technology. **
** The Excellence Cluster EXC 277 is a grant of the Deutsche **
** Forschungsgemeinschaft (DFG) in the context of the German **
** Excellence Initiative. **
** **
********************************************************************/
#include <ICLGeom/CoPlanarityFeatureExtractor.h>
#include <ICLGeom/SegmenterUtils.h>
namespace icl{
namespace geom{
math::DynMatrix<bool> CoPlanarityFeatureExtractor::apply(math::DynMatrix<bool> &initialMatrix, std::vector<SurfaceFeatureExtractor::SurfaceFeature> features,
const core::Img32f &depthImage, std::vector<std::vector<int> > &surfaces, float maxAngle,
float distanceTolerance, float outlierTolerance, int triangles, int scanlines){
math::DynMatrix<bool> coplanar = math::DynMatrix<bool>(initialMatrix.rows(),initialMatrix.rows(), true);//result matrix
//initialize
for(size_t i=0; i<initialMatrix.rows(); i++){
for(size_t j=0; j<initialMatrix.rows(); j++){
//only test pairs of non-adjacent planar surfaces
if(initialMatrix(i,j)==true || features[i].curvatureFactor!=SurfaceFeatureExtractor::PLANAR || features[j].curvatureFactor!=SurfaceFeatureExtractor::PLANAR){
coplanar(i,j)=false;
}
}
}
for(size_t i=0; i<coplanar.rows(); i++){
for(size_t j=i+1; j<coplanar.cols(); j++){//dont check pairs twice
if(coplanar(i,j)==true){//candidate
bool proceed=true;
//criterion 1: both surfaces have similar mean normal (same orientation)
proceed=criterion1(features[i].meanNormal, features[j].meanNormal, maxAngle);
//criterion 2: both surfaces have the same level (combined surface has similar normal)
if(proceed){
proceed = criterion2(depthImage, surfaces[i], surfaces[j], features[i].meanNormal, features[j].meanNormal, maxAngle, triangles);
}
//criterion3: both surfaces separated by occlusion
if(proceed){
proceed=criterion3(depthImage, surfaces[i], surfaces[j], distanceTolerance, outlierTolerance, scanlines);
}
if(!proceed){//remove if one of the criterions failed
coplanar(i,j)=false;
coplanar(j,i)=false;
}
}
}
}
return coplanar;
}
float CoPlanarityFeatureExtractor::getAngle(Vec n1, Vec n2){
float a1=(n1[0]*n2[0]+n1[1]*n2[1]+n1[2]*n2[2]);
float angle=acos(a1)*180.0/M_PI;//angle between the surface normals
return angle;
}
utils::Point CoPlanarityFeatureExtractor::getRandomPoint(std::vector<int> surface, int imgWidth){
int id=surface[rand()%surface.size()];
int y = (int)floor((float)id/(float)imgWidth);
int x = id-y*imgWidth;
utils::Point p(x,y);
return p;
}
Vec CoPlanarityFeatureExtractor::getNormal(Vec p0, Vec p1, Vec p2){
Vec fa=p1-p0;
Vec fb=p2-p0;
Vec n1(fa[1]*fb[2]-fa[2]*fb[1],//normal
fa[2]*fb[0]-fa[0]*fb[2],
fa[0]*fb[1]-fa[1]*fb[0],
0);
Vec n01=n1/norm3(n1);//normalized normal
return n01;
}
bool CoPlanarityFeatureExtractor::criterion1(Vec n1, Vec n2, float maxAngle){
float angle=getAngle(n1,n2);
if(angle>90) angle=180.-angle;//flip
if(angle>maxAngle){
return false;
}
return true;
}
bool CoPlanarityFeatureExtractor::criterion2(const core::Img32f &depthImage, std::vector<int> &surface1, std::vector<int> &surface2,
Vec n1, Vec n2, float maxAngle, int triangles){
std::vector<int> a,b;
int w = depthImage.getSize().width;
if(surface1.size()>surface2.size()){//find bigger surface
b=surface2;
a=surface1;
}else{
a=surface2;
b=surface1;
}
Vec meanNormal((n1[0]+n2[0])/2.,
(n1[1]+n2[1])/2.,
(n1[2]+n2[2])/2.,
0);//mean of both surface normals
Vec meanComb(0,0,0,0);//mean of combined surface normals
core::Channel32f depthImageC = depthImage[0];
for(int p=0; p<triangles; p++){
//random combined plane normals
utils::Point p0=getRandomPoint(b, w);
utils::Point p1=getRandomPoint(a, w);
utils::Point p2=getRandomPoint(a, w);
while (p1.x==p2.x && p1.y==p2.y){//not the same point
p2=getRandomPoint(a, w);
}
Vec a(p0.x,p0.y,depthImageC(p0.x,p0.y),0);
Vec b(p1.x,p1.y,depthImageC(p1.x,p1.y),0);
Vec c(p2.x,p2.y,depthImageC(p2.x,p2.y),0);
Vec n01 = getNormal(a,b,c);
float ang = getAngle(n01,meanNormal);
if(ang>90.){//flip
n01*=-1;
ang=180.-ang;
}
meanComb+=n01;
}
meanComb/=triangles;
float ang=getAngle(meanNormal,meanComb);
if(ang>maxAngle){
return false;
}
return true;
}
bool CoPlanarityFeatureExtractor::criterion3(const core::Img32f &depthImage, std::vector<int> &surface1, std::vector<int> &surface2,
float distanceTolerance, float outlierTolerance, int scanlines){
int w = depthImage.getSize().width;
int occlusions=0;
for(int l=0; l<scanlines; l++){
utils::Point p1=getRandomPoint(surface1, w);
utils::Point p2=getRandomPoint(surface2, w);
if(SegmenterUtils::occlusionCheck((core::Img32f&)depthImage, p1, p2, distanceTolerance, outlierTolerance)){
occlusions++;
}
}
if(occlusions<0.8*scanlines){
return false;
}
return true;
}
} // namespace geom
}