josch/3dpcp
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity
3dpcp/.svn/pristine/3a/3a4111eecc9f99493ea3cadb922880e59d7bf765.svn-base

1500 lines
58 KiB
Text
Raw Normal View History

initial commit
2012-09-16 12:33:11 +00:00
/**
* @file scene.cc
*
* @auhtor Remus Claudiu Dumitru <r.dumitru@jacobs-university.de>
* @date 13 Feb 2012
*
*/
//==============================================================================
// Includes
//==============================================================================
#include "model/scene.h"
#include "model/graphicsAlg.h"
#include "model/util.h"
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/ml/ml.hpp>
#include <boost/algorithm/string.hpp>
#include <sys/stat.h>
#include <errno.h>
#include <limits>
#include <iomanip>
#include <fstream>
#include <sstream>
using namespace std;
//==============================================================================
// Static fields initialization
//==============================================================================
const double model::Scene::PRECISION = 0.0;
const double model::Scene::RAY_DIST = 2.5;
const double model::Scene::PATCH_DIST = 1.0;
const double model::Scene::WALL_DIST = 10.0; // must be bigger than RAY_DIST
const double model::Scene::MIN_EDGE_COV = 0.60;
//==============================================================================
// Implementation
//==============================================================================
model::Scene::Scene(const IOType& type,
const int& start, const int& end,
std::string dir, const bool& scanserver,
const int& maxDist, const int& minDist,
const PlaneAlgorithm& alg, const int& octree, const double& red,
const vector<Pose6d>& poses)
{
if (!quiet) cout << endl << "== Creating scene..." << endl;
if (!quiet) cout << endl << "== Reading scans..." << endl;
// create an output directory for planes and for images
if (makeDir(dir + "img/")== false) {
throw runtime_error("failed to create directory " + dir + "img/");
}
// copy class fields here
this->poses = poses;
// identity matrix
double id[16];
M4identity(id);
// begin the plane detection
// Scan::dir = dir;
// Scan::readScans(type, start, end, dir, maxDist, minDist, false);
Scan::openDirectory(scanserver, dir, type, start, end);
if(Scan::allScans.size() == 0) {
cerr << "No scans found. Did you use the correct format?" << endl;
exit(-1);
}
int nrPlanes = 0 , currScan = start;
// for(vector <Scan*>::iterator scan = Scan::allScans.begin(); scan != Scan::allScans.end(); ++scan) {
for(ScanVector::iterator scan = Scan::allScans.begin(); scan != Scan::allScans.end(); ++scan) {
// prepare for plane detection
(*scan)->setRangeFilter(maxDist, minDist);
(*scan)->setReductionParameter(red, octree);
// scan->setSearchTreeParameter(nns_method, cuda_enabled);
(*scan)->toGlobal();
(*scan)->transform(id, Scan::ICP, 0);
// read the pose of the current scan
pair<Vector3d, Rotation3d> pose;
const double* pt = (*scan)->get_rPos();
const double* rot = (*scan)->get_rPosTheta();
pose.first.x = pt[0];
pose.first.y = pt[1];
pose.first.z = pt[2];
pose.second.x = rot[0];
pose.second.y = rot[1];
pose.second.z = rot[2];
if (!quiet) cout << "\n== Starting plane detection for scan " << currScan << " @ pose ("
<< pose.first.x << " " << pose.first.y << " " << pose.first.z << " "
<< pose.second.x << " " << pose.second.y << " " << pose.second.z << ")..." << endl;
// apply hough transform
Hough hough((*scan), true);
nrPlanes += hough.RHT(); // TODO call proper algorithm
if (!quiet) cout << "** Detected " << nrPlanes << " planes so far" << endl;
// loop over all planes and fill in the data
for (vector<ConvexPlane*>::iterator convexPlane = hough.planes.begin();
convexPlane != hough.planes.end(); convexPlane++)
{
// get the nomrla and the point of application of the normal
double tempNormal[3], tempOrigin[3];
(*convexPlane)->getNormal(tempNormal, tempOrigin);
// the normal and application points using our datastructures
Vector3d normal(tempNormal[0], tempNormal[1], tempNormal[2]);
Point3d origin(tempOrigin[0], tempOrigin[1], tempOrigin[2]);
// fill in the convex hulls
vector<double> doubleHull = (*convexPlane)->getConvexHull();
vector<Point3d> pointHull;
for (vector<double>::iterator point = doubleHull.begin(); point != doubleHull.end(); point += 3) {
pointHull.push_back(Point3d(*(point+0), *(point+1), *(point+2)));
}
Plane3d planeToAdd(origin, normal, pointHull);
// add the planes to the vector
if (planeToAdd.isHorizontal() || planeToAdd.isVertical()) {
this->planes.push_back(planeToAdd);
}
else if (!quiet) {
cout << "** Discarding oblique plane centered at "
<< planeToAdd.pt.x << " " << planeToAdd.pt.y << " " << planeToAdd.pt.z
<< endl;
}
}
// keep the pointer to the points
DataXYZ points = (*scan)->get("xyz reduced");
unsigned int points_size = points.size();
// const vector<Point>* points = (*scan)->get_points();
for (unsigned int i = 0; i < points_size; ++i) {
// XXX get_points returns a pointer
Point3d toPush(points[i][0], points[i][1], points[i][2]);
toPush.translate(pose.first);
toPush.rotate(Point3d(0.0, 0.0, 0.0), pose.second);
this->points.push_back(toPush);
}
// contains the current scan number
currScan++;
}
// to be used when constructing the octree
this->octTreePoints = new double*[this->points.size()];
// go over all points from all the scans
for (unsigned int i = 0; i < this->points.size(); ++i) {
double x = this->points[i].x;
double y = this->points[i].y;
double z = this->points[i].z;
// add the points to the array to be added to the octree
this->octTreePoints[i] = new double[3];
this->octTreePoints[i][0] = x;
this->octTreePoints[i][1] = y;
this->octTreePoints[i][2] = z;
}
// Create the octree and fill it in.
if (!quiet) cout << endl << "== Building OctTree with " << this->points.size() << " points ..." << endl;
this->octTree = new BOctTree<double>(this->octTreePoints, this->points.size(), octree);
//this->_octTree->init();
this->nrPoints = this->points.size();
Scan::allScans.clear();
}
model::Scene::Scene(const Scene& other) {
this->points = other.points;
this->planes = other.planes;
this->walls = other.walls;
this->ceiling = other.ceiling;
this->floor = other.floor;
}
model::Scene::~Scene() {
// delete the octree
if (this->octTree != NULL) {
delete this->octTree;
}
if (this->octTreePoints != NULL) {
for (unsigned int i = 0; i < this->nrPoints; ++i) {
if (this->octTreePoints[i] != NULL) {
delete[] this->octTreePoints[i];
}
}
delete[] this->octTreePoints;
}
}
vector<model::Plane3d> model::Scene::getConvexHull(vector<Plane3d> planes) {
if (planes.size() <= 3) {
return planes;
}
vector<Plane3d> hull;
vector<Plane3d>::iterator leftmostIt = planes.begin();
// compute the lefmost point
for (vector<Plane3d>::iterator it = planes.begin() + 1;
it != planes.end(); ++it)
{
// add only vertical planes
if (it->isVertical() && it->pt.x < leftmostIt->pt.x) {
leftmostIt = it;
}
}
Plane3d planeOnHull = *leftmostIt;
Plane3d endPlane;
do {
// add the candidate plane to the hull
hull.push_back(planeOnHull);
endPlane = planes[0];
for (unsigned int j = 1; j < planes.size(); ++j) {
// consider only vertical planes
if (planes[j].isVertical() == false) {
continue;
}
// define dummy planes in the XOZ plane
Point3d fakeCurPt(planes[j].pt.x, 0.0, planes[j].pt.z);
Point3d fakePointOnHull(planeOnHull.pt.x, 0.0, planeOnHull.pt.z);
Point3d fakeEndPoint(endPlane.pt.x, 0.0, endPlane.pt.z);
// define a point to the left of the line
Point3d farLeft = fakeEndPoint;
farLeft.rotate(planeOnHull.pt, Rotation3d(0.0, -M_PI/6, 0.0));
// if ... or planes[j] on the left side of line (planeOnHull, endPlane)
if (planeOnHull.pt != planes[j].pt &&
(endPlane.pt == planeOnHull.pt || sameSide(fakeCurPt, farLeft, fakePointOnHull, fakeEndPoint)))
{
endPlane = planes[j];
}
}
planeOnHull = endPlane;
} while (endPlane.pt != hull[0].pt);
return hull;
}
vector<model::Plane3d> model::Scene::getSignificantPlanes(vector<Plane3d> planes) {
vector<model::Plane3d> result;
while (!planes.empty()) {
// add the first plane as viable
result.push_back(planes.front());
planes.erase(planes.begin());;
for (vector<Plane3d>::iterator it = planes.begin(); it < planes.end(); ++it) {
if (result.back().isSamePlane(*it)) {
result.back().normal += it->normal;
planes.erase(it);
}
}
// normalize this normal after we have normalized
result.back().normal.normalize();
}
return result;
}
bool model::Scene::getCeiling(vector<Plane3d> planes, Plane3d& result) {
vector<Plane3d> horizontalPlanes;
for (vector<Plane3d>::iterator it = planes.begin(); it != planes.end(); ++it) {
if (it->isHorizontal()) {
horizontalPlanes.push_back(*it);
}
}
if (horizontalPlanes.size() <= 0) {
return false;
}
// start with the first horizontal plane
result = planes.front();
for (vector<Plane3d>::iterator it = planes.begin()+1;
it != planes.end(); ++it)
{
if (it->pt.y > result.pt.y) {
result = *it;
}
}
return true;
}
bool model::Scene::getFloor(vector<Plane3d> planes, Plane3d& result) {
vector<Plane3d> horizontalPlanes;
for (vector<Plane3d>::iterator it = planes.begin();
it != planes.end(); ++it)
{
if (it->isHorizontal()) {
horizontalPlanes.push_back(*it);
}
}
if (horizontalPlanes.size() <= 0) {
return false;
}
// start with the first horizontal plane
result = planes.front();
for (vector<Plane3d>::iterator it = planes.begin()+1;
it != planes.end(); ++it)
{
if (it->pt.y < result.pt.y) {
result = *it;
}
}
return true;
}
void model::Scene::detectWalls() {
if (!quiet) cout << endl << "== Building model..." << endl;
// temporary wall storage
vector<Plane3d> tempWalls = this->planes;
// extract the convex hull of the room
tempWalls = GraphicsAlg::getConcaveHull(tempWalls);
// throw away redundant tempWalls
tempWalls = this->getSignificantPlanes(tempWalls);
// get other bounds, ceiling and floor
Plane3d tempCeiling, tempFloor;
if (!this->getCeiling(this->planes, tempCeiling) || !this->getFloor(this->planes, tempFloor)) {
throw runtime_error("error while determining ceiling or floor");
}
if (tempWalls.size() < 4) {
throw runtime_error("room has less than 4 walls");
}
if (!quiet) cout << "** " << tempWalls.size() << " walls have been detected" << endl;
// XXX planes in the _walls vector should be in order
vector< pair<Point3d, Point3d> > corners;
Point3d upperCorner, lowerCorner;
if(!quiet) cout << endl << "== Computing corners..." << endl;
for (vector<Plane3d>::iterator it = tempWalls.begin(); it != tempWalls.end(); ++it) {
// be careful at the last element
if (it < tempWalls.end()-1) {
upperCorner = it->intersect(*(it+1), tempCeiling);
lowerCorner = it->intersect(*(it+1), tempFloor);
}
else if (it == tempWalls.end()-1) {
upperCorner = it->intersect(tempWalls.front(), tempCeiling);
lowerCorner = it->intersect(tempWalls.front(), tempFloor);
}
if (!quiet) cout << showpos << "** Computed upper corner " << upperCorner << endl;
if (!quiet) cout << showpos << "** Computed lower corner " << lowerCorner << endl;
corners.push_back(make_pair(upperCorner, lowerCorner));
}
if (!quiet) cout << endl << "== Building walls, ceiling and floor..." << endl;
// used to create the walls
Point3d upperLeft, upperRight;
Point3d lowerLeft, lowerRight;
// used to create the floor and the ceiling
vector<Point3d> ceilingHull;
vector<Point3d> floorHull;
Point3d ceilingCenter(0.0, 0.0, 0.0);
Point3d floorCenter(0.0, 0.0, 0.0);
Vector3d ceilingNormal;
Vector3d floorNormal;
for (vector< pair<Point3d, Point3d> >::iterator it = corners.begin();
it != corners.end(); ++it)
{
upperLeft = it->first;
lowerLeft = it->second;
if (it < corners.end()-1) {
upperRight = (it+1)->first;
lowerRight = (it+1)->second;
}
else if (it == corners.end()-1) {
upperRight = corners.front().first;
lowerRight = corners.front().second;
}
Point3d center((upperLeft.x + lowerLeft.x + upperRight.x + lowerRight.x) / 4.0,
(upperLeft.y + lowerLeft.y + upperRight.y + lowerRight.y) / 4.0,
(upperLeft.z + lowerLeft.z + upperRight.z + lowerRight.z) / 4.0);
vector<Point3d> hull;
hull.push_back(upperLeft);
hull.push_back(lowerLeft);
hull.push_back(lowerRight);
hull.push_back(upperRight);
if (!quiet) cout << "** Adding wall centered at " << center << endl;
this->walls.push_back(LabeledPlane3d(center, Vector3d(), hull));
this->walls.back().normal = this->walls.back().computeAverageNormal();
// compute the floor and the ceiling
ceilingHull.push_back(it->first);
floorHull.push_back(it->second);
ceilingCenter.x += it->first.x;
ceilingCenter.y += it->first.y;
ceilingCenter.z += it->first.z;
floorCenter.x += it->second.x;
floorCenter.y += it->second.y;
floorCenter.z += it->second.z;
}
ceilingCenter /= ceilingHull.size();
floorCenter /= floorHull.size();
// dummy normal
Vector3d dummy;
if (!quiet) cout << "** Adding ceiling centered at " << ceilingCenter << endl;
this->ceiling = LabeledPlane3d(ceilingCenter, dummy, ceilingHull);
this->ceiling.normal = this->ceiling.computeAverageNormal();
if (!quiet) cout << "** Adding floor centered at " << floorCenter << endl;
this->floor = LabeledPlane3d(floorCenter, dummy, floorHull);
this->floor.normal = this->floor.computeAverageNormal();
}
bool model::Scene::castRay(const Point3d& src, const Point3d& dest, const double& extraDist,
Point3d& ptHit)
{
// put the Bresenham line in here, we need some extra distance in case the points are close
// but after the detected wall, therefore we need to go a bit further than the wall to check
vector<Point3d> line;
GraphicsAlg::getDiscreteLine(src, dest, PRECISION, extraDist, line);
// loop through all the points in the Bresenham line and decide if it hits something
for (vector<Point3d>::iterator it = line.begin(); it != line.end(); ++it) {
if (this->isOccupied(*it, RAY_DIST)) {
ptHit = *it;
return true;
}
}
// no occlusion took place
return false;
}
void model::Scene::applyLabels(LabeledPlane3d& surf) {
if (this->poses.empty()) {
throw runtime_error("no poses have been provided to the scene");
}
// the maximum distance from which we subtract to create a depth image
double maxDist = 2.0 * WALL_DIST + RAY_DIST;
// determine the points on the plane for which to apply labels
vector<vector<Point3d> > discretePoints = surf.getDiscretePoints(PATCH_DIST);
surf.patches.resize(discretePoints.size());
surf.depthMap.resize(discretePoints.size());
// we need these values for later
surf.depthMapDistances.first = WALL_DIST;
surf.depthMapDistances.second = maxDist;
for (unsigned int i = 0; i < discretePoints.size(); ++i) {
for (unsigned int j = 0; j < discretePoints.front().size(); ++j) {
// consider everything is occluded initially
surf.patches[i].push_back(make_pair(discretePoints[i][j], OCCLUDED));
surf.depthMap[i].push_back(WALL_DIST);
}
}
if (!quiet) cout << endl << "== Performing ray casting for surface centered at " << surf.pt << endl;
for (vector<Pose6d>::iterator srcPose = this->poses.begin(); srcPose != this->poses.end(); ++srcPose) {
for (unsigned int i = 0; i < surf.patches.size(); ++i) {
for (unsigned int j = 0; j < surf.patches[i].size(); ++j) {
// prepare two points for ray casting through wall
Point3d ptOnWall = surf.patches[i][j].first;
Point3d src(surf.normal.x + ptOnWall.x,
surf.normal.y + ptOnWall.y,
surf.normal.z + ptOnWall.z);
double len = ptOnWall.distance(src);
double temp = (len + WALL_DIST) / len;
src.x = ptOnWall.x + (src.x - ptOnWall.x) * temp;
src.y = ptOnWall.y + (src.y - ptOnWall.y) * temp;
src.z = ptOnWall.z + (src.z - ptOnWall.z) * temp;
// remember the point we hit when ray casting
Point3d ptHit;
if (insideHull(surf.patches[i][j].first, surf.hull) && castRay(src, ptOnWall, WALL_DIST, ptHit)) {
surf.patches[i][j].second = OCCUPIED;
surf.depthMap[i][j] = maxDist - src.distance(ptHit);
} else if (!castRay(srcPose->first, surf.patches[i][j].first, 0, ptHit)) {
surf.patches[i][j].second = EMPTY;
surf.depthMap[i][j] = 0.0;
}
}
}
}
// create the OpenCV depth image
int imgHeight = static_cast<int>(surf.depthMap.size());
int imgWidth = static_cast<int>(surf.depthMap.front().size());
// crate an OpenCV image and allocate memory for it
cv::Mat img(imgHeight, imgWidth, CV_8UC1);
// compute the maximum from the image
double max = numeric_limits<double>::min();
for (int i = 0; i < imgHeight; ++i) {
for (int j = 0; j < imgWidth; ++j) {
if (max < surf.depthMap[i][j]) {
max = surf.depthMap[i][j];
}
}
}
// write the current image to a file
cv::Mat labels(imgHeight, imgWidth, CV_8UC3);
// adjust the image
for (int i = 0; i < imgHeight; ++i) {
for (int j = 0; j < imgWidth; ++j) {
img.data[(i * img.cols) + j] = static_cast<char>(MAX_IMG_VAL * surf.depthMap[i][j] / max);
// put the color into the image according to the label
int curr = (i * 3 * img.cols) + 3 * j;
switch (surf.patches[i][j].second) {
case EMPTY:
labels.data[curr + 0] = 0;
labels.data[curr + 1] = 200;
labels.data[curr + 2] = 0;
break;
case OCCLUDED:
labels.data[curr + 0] = 200;
labels.data[curr + 1] = 0;
labels.data[curr + 2] = 0;
break;
case OCCUPIED:
labels.data[curr + 0] = 0;
labels.data[curr + 1] = 0;
labels.data[curr + 2] = 200;
break;
default:
throw runtime_error("invalid default branch taken");
break;
}
}
}
cv::imwrite("./img/labels.png", labels);
// copy the image
surf.depthImg = img.clone();
}
void model::Scene::applyAllLabels() {
if (!quiet) cout << endl << "== Performing ray casting for all surfaces..." << endl;
applyLabels(this->ceiling);
applyLabels(this->floor);
for (vector<LabeledPlane3d>::iterator it = this->walls.begin(); it != this->walls.end(); ++it) {
applyLabels(*it);
}
}
void model::Scene::detectPotentialOpenings(const LabeledPlane3d& surf,
std::vector<CandidateOpening>& openings)
{
if (!quiet) cout << endl << "== Determining openings for surface centered at " << surf.pt << endl;
// get the candidate openings for current plane
vector<CandidateOpening> candidates;
surf.computeOpeningCandidates(candidates);
// TODO train the SVM using some outside variables, not hard coded in here, play around a lot with these numbers
// prepare to train the SVM
const unsigned int nrSamples = 68;
const unsigned int nrMainClass = 14;
const unsigned int nrFeatures = model::CandidateOpening::NR_FEATURES;
// make a set of excluded features
int excludedFeaturesInit[] = {6, 7, 8, 12, 13};
set<int> excludedFeatures(excludedFeaturesInit, excludedFeaturesInit + 5);
// set up the training data using some real life examples, needs to be flaot
float trainingData[nrSamples][14] = {
// TODO fix the RMS fit residual
// 1 area | 2 w/h | 3 w/W | 4 h/H | 5-8 dist to edges | 9 RMS | 10-12 E Occup Occl | 13 int rect | 14 int U-shapes
{ 3120, 0.42, 0.05, 0.36, 23, 138, 550, 126, 0, 0.93, 0.01, 0.06, 15, 0},
{ 3090, 0.42, 0.05, 0.36, 23, 138, 126, 550, 0, 0.93, 0.01, 0.06, 15, 0},
{ 6810, 0.94, 0.11, 0.34, 23, 140, 453, 176, 0, 0.91, 0.02, 0.07, 15, 0},
{ 6815, 0.94, 0.11, 0.34, 23, 140, 176, 453, 0, 0.91, 0.02, 0.07, 15, 0},
{11780, 1.52, 0.19, 0.35, 23, 139, 450, 128, 0, 0.89, 0.01, 0.10, 15, 0},
{11190, 1.52, 0.18, 0.35, 23, 139, 128, 450, 0, 0.88, 0.02, 0.10, 15, 0},
{10374, 1.70, 0.18, 0.31, 31, 141, 453, 124, 0, 0.87, 0.02, 0.11, 15, 0},
{10374, 1.70, 0.18, 0.31, 31, 141, 124, 453, 0, 0.87, 0.02, 0.11, 15, 0},
{27600, 0.82, 0.22, 0.62, 18, 97, 403, 148, 0, 0.95, 0.03, 0.02, 12, 0},
{27600, 0.82, 0.22, 0.62, 18, 97, 148, 403, 0, 0.94, 0.03, 0.03, 12, 0},
{25516, 0.87, 0.22, 0.59, 28, 100, 49, 396, 0, 0.84, 0.11, 0.05, 20, 0},
{26316, 0.87, 0.22, 0.59, 28, 100, 396, 49, 0, 0.84, 0.13, 0.03, 22, 0},
{25516, 0.87, 0.22, 0.59, 28, 100, 70, 396, 0, 0.84, 0.11, 0.05, 20, 0},
{26316, 0.87, 0.22, 0.59, 28, 100, 396, 70, 0, 0.84, 0.13, 0.03, 22, 0},
//========================================================================
{+143289, +2.10345, +0.786533, +0.887755, +31, +1, +1, +147, +0, +0.371222, +0.524681, +0.104097, +34300, +0},
{+180612, +2.65134, +0.991404, +0.887755, +31, +1, +1, +4, +0, +0.294571, +0.620845, +0.0845846, +44100, +0},
{+180873, +2.65517, +0.992837, +0.887755, +31, +1, +1, +3, +0, +0.294146, +0.621392, +0.0844626, +55125, +0},
{+181395, +2.66284, +0.995702, +0.887755, +31, +1, +1, +1, +0, +0.293299, +0.622481, +0.0842195, +67375, +0},
{+181656, +2.66667, +0.997135, +0.887755, +31, +1, +1, +0, +0, +0.292878, +0.621846, +0.0852766, +80850, +0},
{+181828, +2.64885, +0.994269, +0.891156, +31, +0, +0, +3, +0, +0.292634, +0.621114, +0.0862518, +70125, +2750},
{+143838, +2.09542, +0.786533, +0.891156, +31, +0, +1, +147, +0, +0.369805, +0.525466, +0.104729, +35700, +1400},
{+181304, +2.64122, +0.991404, +0.891156, +31, +0, +1, +4, +0, +0.293446, +0.621465, +0.0850891, +45900, +1800},
{+181566, +2.64504, +0.992837, +0.891156, +31, +0, +1, +3, +0, +0.293023, +0.622011, +0.0849663, +57375, +2250},
{+182090, +2.65267, +0.995702, +0.891156, +31, +0, +1, +1, +0, +0.29218, +0.623093, +0.0847273, +70125, +2750},
{+182352, +2.65649, +0.997135, +0.891156, +31, +0, +1, +0, +0, +0.29176, +0.622455, +0.0857846, +84150, +3300},
{+120270, +2.70142, +0.792768, +0.717687, +31, +51, +1, +147, +0, +0.427181, +0.486813, +0.0860065, +8400, +0},
{+150654, +3.38389, +0.993046, +0.717687, +31, +51, +1, +3, +0, +0.341299, +0.588308, +0.0703931, +10800, +0},
{+150865, +3.38863, +0.994437, +0.717687, +31, +51, +1, +2, +0, +0.340821, +0.588884, +0.0702946, +13500, +0},
{+39680, +0.605469, +0.222063, +0.870748, +32, +5, +396, +146, +0, +0.654738, +0.294254, +0.0510081, +3243, +0},
{+38656, +0.589844, +0.216332, +0.870748, +32, +5, +400, +146, +0, +0.672082, +0.27931, +0.0486082, +1081, +0},
{+39835, +0.603113, +0.222063, +0.87415, +32, +4, +396, +146, +0, +0.655153, +0.293787, +0.0510606, +3384, +0},
{+38807, +0.587549, +0.216332, +0.87415, +32, +4, +400, +146, +0, +0.672508, +0.278893, +0.0485995, +1128, +0},
{+39990, +0.600775, +0.222063, +0.877551, +32, +3, +396, +146, +0, +0.655539, +0.293373, +0.0510878, +3528, +0},
{+38958, +0.585271, +0.216332, +0.877551, +32, +3, +400, +146, +0, +0.672904, +0.278505, +0.0485908, +1176, +0},
{+40145, +0.598456, +0.222063, +0.880952, +32, +2, +396, +146, +0, +0.655374, +0.293586, +0.05104, +3675, +0},
{+39109, +0.583012, +0.216332, +0.880952, +32, +2, +400, +146, +0, +0.672735, +0.278759, +0.0485055, +1225, +0},
{+40300, +0.596154, +0.222063, +0.884354, +32, +1, +396, +146, +0, +0.652854, +0.296154, +0.0509926, +3825, +0},
{+39260, +0.580769, +0.216332, +0.884354, +32, +1, +400, +146, +0, +0.670148, +0.281457, +0.0483953, +1275, +0},
{+40455, +0.59387, +0.222063, +0.887755, +32, +0, +396, +146, +0, +0.650352, +0.298702, +0.0509455, +3978, +153},
{+39411, +0.578544, +0.216332, +0.887755, +32, +0, +400, +146, +0, +0.66758, +0.284134, +0.048286, +1326, +51},
{+115564, +4.14371, +0.991404, +0.568027, +33, +93, +1, +4, +0, +0.429745, +0.554031, +0.0162248, +990, +0},
{+115731, +4.1497, +0.992837, +0.568027, +33, +93, +1, +3, +0, +0.429124, +0.554674, +0.0162014, +1170, +0},
{+116065, +4.16168, +0.995702, +0.568027, +33, +93, +1, +1, +0, +0.42789, +0.555956, +0.0161547, +1365, +0},
{+116232, +4.16766, +0.997135, +0.568027, +33, +93, +1, +0, +0, +0.427275, +0.555544, +0.0171812, +1575, +0},
{+118674, +4.05848, +0.994269, +0.581633, +33, +89, +0, +3, +0, +0.419292, +0.556575, +0.0241333, +1911, +0},
{+119016, +4.07018, +0.997135, +0.581633, +33, +89, +0, +1, +0, +0.418087, +0.557849, +0.024064, +2205, +0},
{+118332, +4.04678, +0.991404, +0.581633, +33, +89, +1, +4, +0, +0.420453, +0.556739, +0.0228087, +1386, +0},
{+33790, +0.711009, +0.222063, +0.741497, +33, +42, +396, +146, +0, +0.736372, +0.215715, +0.0479136, +828, +0},
{+33136, +0.697248, +0.217765, +0.741497, +33, +42, +399, +146, +0, +0.750905, +0.201231, +0.0478634, +276, +0},
{+65880, +4.575, +0.786533, +0.408163, +33, +140, +1, +147, +0, +0.541166, +0.455055, +0.0037796, +45, +0},
{+66360, +4.60833, +0.792264, +0.408163, +33, +140, +1, +143, +0, +0.537251, +0.458996, +0.00375226, +55, +0},
{+135218, +3.59278, +0.998567, +0.659864, +33, +66, +0, +0, +0, +0.375216, +0.557995, +0.0667884, +11550, +0},
{+135135, +3.55385, +0.992837, +0.663265, +33, +65, +0, +4, +0, +0.375484, +0.557457, +0.0670589, +6468, +0},
{+135330, +3.55897, +0.994269, +0.663265, +33, +65, +0, +3, +0, +0.374943, +0.558095, +0.0669622, +8316, +0},
{+135720, +3.56923, +0.997135, +0.663265, +33, +65, +0, +1, +0, +0.373865, +0.559365, +0.0667698, +10395, +0},
{+135915, +3.57436, +0.998567, +0.663265, +33, +65, +0, +0, +0, +0.373329, +0.558893, +0.0677777, +12705, +0},
{+142065, +3.38049, +0.992837, +0.697279, +33, +55, +0, +4, +0, +0.357998, +0.572006, +0.0699961, +7728, +0},
{+117117, +4.10059, +0.992837, +0.57483, +30, +94, +0, +4, +0, +0.428213, +0.555837, +0.0159499, +280, +0},
{+117286, +4.10651, +0.994269, +0.57483, +30, +94, +0, +3, +0, +0.427596, +0.556477, +0.0159269, +360, +0},
{+117624, +4.11834, +0.997135, +0.57483, +30, +94, +0, +1, +0, +0.426367, +0.557752, +0.0158811, +450, +0},
{+117793, +4.12426, +0.998567, +0.57483, +30, +94, +0, +0, +0, +0.425755, +0.557359, +0.0168856, +550, +0},
{+43176, +1.52976, +0.368195, +0.571429, +30, +95, +396, +44, +0, +0.571822, +0.428178, +0, +36, +0},
{+43344, +1.53571, +0.369628, +0.571429, +30, +95, +396, +43, +0, +0.569606, +0.430394, +0, +60, +0},
{+42672, +1.5119, +0.363897, +0.571429, +30, +95, +399, +44, +0, +0.578576, +0.421424, +0, +18, +0},
{+42840, +1.51786, +0.36533, +0.571429, +30, +95, +399, +43, +0, +0.576307, +0.423693, +0, +36, +0},
{+42504, +1.50595, +0.362464, +0.571429, +30, +95, +400, +44, +0, +0.580863, +0.419137, +0, +6, +0},
{+42672, +1.5119, +0.363897, +0.571429, +30, +95, +400, +43, +0, +0.578576, +0.421424, +0, +18, +0},
{+33338, +0.748815, +0.226361, +0.717687, +29, +53, +396, +143, +0, +0.760244, +0.195453, +0.0443038, +1134, +0},
};
// match the labels to the training data
float mainLabel = 1.0;
float labels[nrSamples];
// create the labels
for (unsigned int i = 0; i < nrSamples; ++i) {
if (i < nrMainClass) {
labels[i] = mainLabel;
} else {
labels[i] = -mainLabel;
}
}
// normalize the values above
float maxFeatures[nrFeatures];
for (unsigned int i = 0; i < nrFeatures; ++i) {
maxFeatures[i] = numeric_limits<float>::min();
}
for (unsigned int i = 0; i < nrSamples; ++i) {
for (unsigned int j = 0; j < nrFeatures; ++j) {
if (maxFeatures[j] < trainingData[i][j]) {
maxFeatures[j] = trainingData[i][j];
}
}
}
for (unsigned int i = 0; i < nrSamples; ++i) {
for (unsigned int j = 0; j < nrFeatures; ++j) {
if (excludedFeatures.find(j) != excludedFeatures.end()) {
trainingData[i][j] = 0.0;
} else {
trainingData[i][j] /= maxFeatures[j];
}
}
}
// compute the max of the values of the candidates, but normalize later
for (unsigned int i = 0; i < nrFeatures; ++i) {
maxFeatures[i] = numeric_limits<float>::min();
}
for (vector<CandidateOpening>::iterator it = candidates.begin(); it != candidates.end(); ++it) {
for (unsigned int feat = 0; feat < nrFeatures; ++feat) {
if (maxFeatures[feat] < it->features[feat]) {
maxFeatures[feat] = it->features[feat];
}
}
}
// create two OpenCV matrices
cv::Mat trainingMat(nrSamples, nrFeatures, CV_32FC1, trainingData);
cv::Mat labelsMat(nrSamples, 1, CV_32FC1, labels);
// set up the SVM parameters
cv::SVMParams params;
params.svm_type = cv::SVM::NU_SVC;
params.kernel_type = cv::SVM::RBF;
params.term_crit = cv::TermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 1000, 1e-6);
params.nu = 0.8;
params.C = 8.0;
params.gamma = 20;
// create SVM and train it
cv::SVM svm;
svm.train_auto(trainingMat, labelsMat, cv::Mat(), cv::Mat(), params); // XXX with or without auto
// store these openings, and filter some more later on
std::vector<CandidateOpening> tempOpenings;
for (vector<CandidateOpening>::iterator it = candidates.begin(); it != candidates.end(); ++it) {
if (it->features.size() != nrFeatures) {
throw runtime_error("number of features in candidate in does not equal the expected one");
}
float candidateData[nrFeatures];
for (size_t jt = 0; jt < it->features.size(); ++jt) {
if (excludedFeatures.find(jt) != excludedFeatures.end()) {
candidateData[jt] = 0.0;
} else {
// also normalize the data here, and not in the candidateOpening object
candidateData[jt] = static_cast<float>(it->features[jt]) / maxFeatures[jt];
}
}
cv::Mat candidateMat(1, nrFeatures, CV_32FC1, candidateData);
// decide of which type the current opening is
float response = svm.predict(candidateMat);
if (response == mainLabel) {
tempOpenings.push_back(*it);
}
}
// filter openings with respect to how much they overlap with the Canny edge
cv::Mat canny, hSobel, vSobel, combined;
surf.detectEdges(canny, hSobel, vSobel, combined);
double maxEdgeCoverage = numeric_limits<double>::min();
double averageArea = 0.0;
for (vector<CandidateOpening>::iterator it = tempOpenings.begin(); it != tempOpenings.end(); ++it) {
int y1 = it->edges[0];
int y2 = it->edges[1];
int x1 = it->edges[2];
int x2 = it->edges[3];
// how many pixels overlap?
int total = 2 * (y2 - y1 + x2 - x1) + 1;
int count = total;
if (y2 < y1 || x2 < x1) {
throw runtime_error("candidate edges must be in proper order");
}
for (int i = y1; i <= y2; ++i) {
if (combined.data[i * combined.cols + x1] == 0) {
count--;
}
if (combined.data[i * combined.cols + x2] == 0) {
count--;
}
}
for (int i = x1; i <= x2; ++i) {
if (combined.data[y1 * combined.cols + i] == 0) {
count--;
}
if (combined.data[y2 * combined.cols + i] == 0) {
count--;
}
}
// remember the edge coverage inside the object
it->edgeCoverage = static_cast<double>(count) / total;
if (maxEdgeCoverage < it->edgeCoverage) {
maxEdgeCoverage = it->edgeCoverage;
}
// increment the average area
averageArea += it->features[0];
}
averageArea /= tempOpenings.size();
unsigned int discarded = 0;
for (vector<CandidateOpening>::iterator it = tempOpenings.begin(); it != tempOpenings.end(); ++it) {
// TODO put coef in class
if ((it->features[0] > 0.3 * averageArea) &&
(it->edgeCoverage > MIN_EDGE_COV * maxEdgeCoverage))
{
openings.push_back(*it);
} else {
discarded++;
}
}
if (!quiet) cout << "** Discarded openings due to relative size to average candidate: " << discarded << endl;
// draw the chosen rectangles, and also the occupancy map
cv::Mat imgWithOpenings = surf.depthImg;
cv::cvtColor(imgWithOpenings, imgWithOpenings, CV_GRAY2BGR, 3);
for (size_t i = 0; i < openings.size(); ++i) {
int y1 = openings[i].edges[0];
int y2 = openings[i].edges[1];
int x1 = openings[i].edges[2];
int x2 = openings[i].edges[3];
cv::rectangle(imgWithOpenings, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 0, MAX_IMG_VAL), 1);
}
cv::imwrite("./img/depthCandidates.png", imgWithOpenings);
if (!quiet) cout << "** Detected " << openings.size() << " potential openings" << endl;
}
void model::Scene::clusterOpenings(const LabeledPlane3d& surf, const vector<CandidateOpening>& openings,
std::vector<CandidateOpening>& result) const
{
// clear the result just in case
result.clear();
if (openings.size() == 1) {
if (!quiet) cerr << "WARNING: only one candidate received for clustering!" << endl;
result.push_back(openings.front());
return;
} else if (openings.size() < 1) {
if (!quiet) cerr << "WARNING: no candidates received for clustering!" << endl;
return;
}
if (!quiet) cout << endl << "== Clustering openings for surface centered at " << surf.pt << endl;
size_t nrSamples = openings.size();
unsigned int dim = 7;
// a few matrix initializations
CvMat* points = cvCreateMat(nrSamples, dim, CV_32FC1);
CvMat* clusters = cvCreateMat(nrSamples, 1, CV_32SC1);
// fill in the matrices with data
for (size_t i = 0; i < nrSamples; ++i) {
if (openings[i].features.size() != model::CandidateOpening::NR_FEATURES) {
throw runtime_error("invalid feature vector size");
}
// compute the center of each opening
float x = static_cast<float>(openings[i].edges[2] + openings[i].edges[3]) / 2.0;
float y = static_cast<float>(openings[i].edges[0] + openings[i].edges[1]) / 2.0;
points->data.fl[i * dim + 0] = x; // window center
points->data.fl[i * dim + 1] = y; // window center
points->data.fl[i * dim + 2] = openings[i].edges[0]; // upper edge
points->data.fl[i * dim + 3] = openings[i].edges[1]; // lower edge
points->data.fl[i * dim + 4] = openings[i].edges[2]; // left edge
points->data.fl[i * dim + 5] = openings[i].edges[3]; // right edge
points->data.fl[i * dim + 6] = openings[i].features[1];// edge ratio
}
// termination criteria for the kMeans algorithm
CvTermCriteria term;
term.type = CV_TERMCRIT_ITER | CV_TERMCRIT_EPS;
term.max_iter = 10;
term.epsilon = 1.0;
int maxNrClusters = 256;
int nrClusters = 2; // from how many clusters to start
double prev, compactness; // previous compactness, and current compactness
CvMat *centers = cvCreateMat(nrClusters - 1, dim, CV_32FC1); // place where we compute the centers of each cluster
cvKMeans2(points, nrClusters - 1, clusters, term, 1, 0, 0, centers, &prev);
while (1) {
cvReleaseMat(&centers);
centers = cvCreateMat(nrClusters, dim, CV_32FC1);
cvKMeans2(points, nrClusters, clusters, term, 1, 0, 0, centers, &compactness);
// TODO put somewhere everything in class
double deltaCompactness = compactness - prev;
if (!quiet) cout << "** Delta compactness: " << deltaCompactness << endl;
if (fabs(deltaCompactness) < 1.0 || nrClusters >= maxNrClusters) {
break;
}
prev = compactness;
nrClusters++;
}
if (!quiet) cout << "** Detected " << static_cast<unsigned int>(nrClusters) << " clusters of potential openings" << endl;
// generate colors for each cluster
int r, g, b;
cv::Scalar colors[nrClusters];
for (int i = 0; i < nrClusters; ++i) {
randomColor(MAX_IMG_VAL, r, g, b);
colors[i] = (cv::Scalar(b, g, r));
}
// create an image with the clusters in separate colors
cv::Mat clusterImg = surf.depthImg;
cv::Mat finalImg = surf.depthImg;
cv::cvtColor(clusterImg, clusterImg, CV_GRAY2BGR, 3);
cv::cvtColor(finalImg, finalImg, CV_GRAY2BGR, 3);
// count how many elements we have in each cluster
vector<int> clusterCount;
clusterCount.resize(nrClusters);
fill(clusterCount.begin(), clusterCount.end(), 0);
// compute the candidate for each cluster with the best coverage of empty area
vector<CandidateOpening> bestCandidates;
bestCandidates.resize(nrClusters);
vector<double> bestEmptyAreas;
bestEmptyAreas.resize(nrClusters);
fill(bestEmptyAreas.begin(), bestEmptyAreas.end(), numeric_limits<double>::min());
// fill in the result separating each cluster
for (size_t i = 0; i < nrSamples; ++i) {
// the cluster of the current opening
int currCluster = clusters->data.i[i];
// draw each cluster
int y1 = openings[i].edges[0];
int y2 = openings[i].edges[1];
int x1 = openings[i].edges[2];
int x2 = openings[i].edges[3];
cv::rectangle(clusterImg, cv::Point(x1, y1), cv::Point(x2, y2), colors[currCluster], 1);
// TODO avoid hard coding
// find which is the best choice from the cluster
double currEmptyArea = openings[i].features[0] *
(openings[i].features[9] - openings[i].features[10] - openings[i].features[11]);
if (bestEmptyAreas[currCluster] < currEmptyArea) {
bestEmptyAreas[currCluster] = currEmptyArea;
bestCandidates[currCluster] = openings[i];
}
clusterCount[currCluster]++;
}
// divide by the cluster count to get the average of the cluster
for (size_t i = 0; i < bestEmptyAreas.size(); ++i) {
if (clusterCount[i] <= 0) {
continue;
}
int y1 = bestCandidates[i].edges[0];
int y2 = bestCandidates[i].edges[1];
int x1 = bestCandidates[i].edges[2];
int x2 = bestCandidates[i].edges[3];
cv::rectangle(finalImg, cv::Point(x1, y1), cv::Point(x2, y2), colors[i], 2);
result.push_back(bestCandidates[i]);
}
if (!quiet) {
cout << "** The count for each cluster is [";
for (unsigned int i = 0; i < clusterCount.size() - 1; ++i) {
cout << clusterCount[i] << ", ";
}
cout << clusterCount.back() << "]" << endl;
}
// display the center of the cluster only for the clusters that have candidates
for (int i = 0; i < nrClusters; ++i) {
if (clusterCount[i] > 0) {
int x = static_cast<int>(centers->data.fl[i * dim + 0]);
int y = static_cast<int>(centers->data.fl[i * dim + 1]);
cv::circle(clusterImg, cv::Point(x, y), 2, colors[i], CV_FILLED);
cv::circle(finalImg, cv::Point(x, y), 2, colors[i], CV_FILLED);
}
}
cv::imwrite("./img/depthClusters.png", clusterImg);
cv::imwrite("./img/depthOpenings.png", finalImg);
cvReleaseMat(&points);
cvReleaseMat(&centers);
cvReleaseMat(&clusters);
}
void model::Scene::addFinalOpenings(const LabeledPlane3d& surf,
std::vector<CandidateOpening>& result)
{
int imgHeight = static_cast<int>(surf.depthMap.size());
int imgWidth = static_cast<int>(surf.depthMap.front().size());
vector<CandidateOpening> openings, candidates;
this->detectPotentialOpenings(surf, openings);
this->clusterOpenings(surf, openings, candidates);
// sweep across the wall and see which candidates overlap
for (int i = 0; i < imgHeight; ++i) {
for (int j = 0; j < imgWidth; ++j) {
// put currently overlapping candidates here
vector<pair<vector<CandidateOpening>::iterator, double> > temp;
// the maximum opening area in the current overlapping windows
double bestEmptyArea = numeric_limits<double>::min();
for (vector<CandidateOpening>::iterator it = candidates.begin(); it < candidates.end(); ++it) {
int y1 = it->edges[0];
int y2 = it->edges[1];
int x1 = it->edges[2];
int x2 = it->edges[3];
if (i >= y1 && i < y2 && j >= x1 && j <= x2) {
// TODO avoid hard coding
double freeArea = it->features[0] *
(it->features[9] - it->features[10] - it->features[11]);
temp.push_back(make_pair(it, freeArea));
if (bestEmptyArea < freeArea) {
bestEmptyArea = freeArea;
}
}
}
for (vector<pair<vector<CandidateOpening>::iterator, double> >::iterator it = temp.begin(); it < temp.end(); ++it) {
// if the area is smaller than the maximum discard this plane
if (it->second < bestEmptyArea) {
candidates.erase(it->first);
}
}
}
}
// display an image with the final openings
int r, g, b;
cv::Mat finalOpeningsImg = surf.depthImg.clone();
cv::cvtColor(finalOpeningsImg, finalOpeningsImg, CV_GRAY2BGR, 3);
for (vector<CandidateOpening>::iterator it = candidates.begin(); it < candidates.end(); ++it) {
int y1 = it->edges[0];
int y2 = it->edges[1];
int x1 = it->edges[2];
int x2 = it->edges[3];
randomColor(MAX_IMG_VAL, r, g, b);
cv::rectangle(finalOpeningsImg, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(b, g, r), 3);
// add the new openings as windows
Point3d pt(0.0, 0.0, 0.0);
pt += surf.patches[y1].front().first;
pt += surf.patches[y2].front().first;
pt += surf.patches.front()[x1].first;
pt += surf.patches.front()[x2].first;
pt /= 4.0;
vector<Point3d> hull;
hull.push_back(surf.patches[y1][x1].first);
hull.push_back(surf.patches[y1][x2].first);
hull.push_back(surf.patches[y2][x2].first);
hull.push_back(surf.patches[y2][x1].first);
Plane3d toPush(pt, surf.normal, hull);
this->finalOpenings.push_back(toPush);
}
cv::imwrite("./img/depthOpeningsFinal.png", finalOpeningsImg);
if (!quiet) cout << "** Final openings found on current surface: " << candidates.size() << endl;
if (!quiet) cout << "** Final openings found so far: " << this->finalOpenings.size() << endl;
// copy the result
result = candidates;
}
void model::Scene::correct(LabeledPlane3d& surf, const vector<CandidateOpening>& openings) {
if (!quiet) cout << endl << "== Correcting surface centered at " << surf.pt << endl;
int imgHeight = static_cast<int>(surf.depthMap.size());
int imgWidth = static_cast<int>(surf.depthMap.front().size());
// some images we are interested in
cv::Mat depthImg = surf.depthImg.clone();
cv::Mat depthImgMask(imgHeight, imgWidth, CV_8UC1);
cv::Mat labelsImg(imgHeight, imgWidth, CV_8UC3);
// compute the maximum from the image
double max = numeric_limits<double>::min();
double mean = 0.0, stdDev = 0.0;
int count = 0;
for (int i = 0; i < imgHeight; ++i) {
for (int j = 0; j < imgWidth; ++j) {
// set all empty and occluded to unknown
if (surf.patches[i][j].second == EMPTY || surf.patches[i][j].second == OCCLUDED) {
surf.patches[i][j].second = UNKOWN;
// compute the mean of the occupied patches
} else if (surf.patches[i][j].second == OCCUPIED) {
mean += surf.depthImg.data[i * surf.depthImg.cols + j];
count++;
}
// compute the maximum from the image
if (max < surf.depthMap[i][j]) {
max = surf.depthMap[i][j];
}
}
}
// adjust the mean
mean /= count;
count = 0;
// compute the standard deviation
for (int i = 0; i < imgHeight; ++i) {
for (int j = 0; j < imgWidth; ++j) {
if (surf.patches[i][j].second == OCCUPIED) {
stdDev += pow(surf.depthImg.data[i * surf.depthImg.cols + j] - mean, 2.0);
count++;
}
}
}
// adjust the standard deviation
stdDev = sqrt(stdDev / count);
// mark all patches contained in openings
for (vector<CandidateOpening>::const_iterator it = openings.begin(); it != openings.end(); ++it) {
int y1 = it->edges[0];
int y2 = it->edges[1];
int x1 = it->edges[2];
int x2 = it->edges[3];
for (int i = 0; i < imgHeight; ++i) {
for (int j = 0; j < imgWidth; ++j) {
if (i >= y1 && i <= y2 && j >= x1 && j <= x2) {
surf.patches[i][j].second = OPENING;
}
}
}
}
// adjust the image
for (int i = 0; i < imgHeight; ++i) {
for (int j = 0; j < imgWidth; ++j) {
// initially mark as not part of the mask
depthImgMask.data[i * depthImgMask.cols + j] = 0;
// put the color into the image according to the label
int curr = (i * 3 * labelsImg.cols) + 3 * j;
switch (surf.patches[i][j].second) {
case OPENING:
labelsImg.data[curr + 0] = 200;
labelsImg.data[curr + 1] = 200;
labelsImg.data[curr + 2] = 200;
break;
case UNKOWN:
labelsImg.data[curr + 0] = 50;
labelsImg.data[curr + 1] = 50;
labelsImg.data[curr + 2] = 50;
// add pixel to mask for inpainting
depthImgMask.data[i * depthImgMask.cols + j] = 200;
break;
case OCCUPIED:
labelsImg.data[curr + 0] = 0;
labelsImg.data[curr + 1] = 0;
labelsImg.data[curr + 2] = 200;
break;
default:
throw runtime_error("invalid default branch taken, possibly EMPTY or OCCLUDED patches");
break;
}
}
}
cv::imwrite("./img/labelsWithOpenings.png", labelsImg);
cv::imwrite("./img/depthMask.png", depthImgMask);
cv::imwrite("./img/depthSimple.png", depthImg);
// TODO add values as part of class
if (!quiet) cout << "** Filling in missing data using inpaint algorithm" << endl;
cv::inpaint(depthImg, depthImgMask, depthImg, 0.05 * (imgWidth + imgHeight), cv::INPAINT_NS);
cv::imwrite("./img/depthInpaint.png", depthImg);
// add some gaussian noise to make the image look more real
if (!quiet) cout << "** Adding gaussian noise: mean = " << mean << ", stdDev = " << stdDev << endl;
cv::Mat noise(imgHeight, imgWidth, CV_8UC1);
cv::randn(noise, mean, stdDev);
cv::addWeighted(depthImg, 0.9, noise, 0.1, 0.0, depthImg);
cv::GaussianBlur(depthImg, depthImg, cv::Size(3, 3), 0.5, 0.5);
cv::imwrite("./img/depthInpaintNoise.png", depthImg);
surf.correctedDepthImg = depthImg.clone();
// TODO apply the inverse transform from 2D to 3D and create a new point cloud using UOS RGB
}
void model::Scene::writeModel(string dir) {
if (!fileIsDir(dir)) {
throw runtime_error(dir + " is not a directory");
}
string outputDir;
if (dir[dir.length()-1] == '/') {
outputDir = dir + "model";
}
else {
outputDir = dir + "/model";
}
int ret = mkdir(outputDir.c_str(), S_IRWXU | S_IRWXG | S_IRWXO);
if (ret != 0 && errno != EEXIST) {
throw runtime_error("unable to create directory " + outputDir);
}
if (!quiet) cout << endl << "== Writing planes to " << outputDir << " directory..." << endl;
string planesListFile = outputDir + "/planes.list";
ofstream list(planesListFile.c_str(), ios::out);
for (unsigned int counter = 0; counter < this->walls.size() + this->finalOpenings.size() + 2; ++counter) {
Plane3d currPlane; // the plane to be printed
string type; // the objet's type, floor, ceiling, wall or opening
if (counter < this->walls.size()) {
currPlane = this->walls[counter];
type = "Wall";
}
else if (counter == this->walls.size()) {
currPlane = this->ceiling;
type = "Ceiling";
}
else if (counter == this->walls.size() + 1) {
currPlane = this->floor;
type = "Floor";
} else {
// TODO find a better solution
currPlane = this->finalOpenings[counter - (this->walls.size() + 2)];
currPlane.normal.normalize();
for (vector<Point3d>::iterator it = currPlane.hull.begin(); it != currPlane.hull.end(); ++it) {
it->x += currPlane.normal.x;
it->y += currPlane.normal.y;
it->z += currPlane.normal.z;
}
type = "Opening";
}
stringstream ss;
ss << outputDir << "/scan" << setfill('0') << setw(3) << counter << ".3d";
ofstream file(ss.str().c_str(), ios::out);
// add the plane to the list of planes
list << type << " " << ss.str() << endl;
boost::algorithm::to_lower(type);
if (!quiet) cout << "** Writing " << type << ss.str() << endl;
// add the points to the scan file
for (vector<Point3d>::iterator it = currPlane.hull.begin();
it != currPlane.hull.end(); ++it)
{
file << *it << endl;
}
file.close();
}
list.close();
}
void model::Scene::writeCorrectedWalls(std::string dir) {
if (!fileIsDir(dir)) {
throw runtime_error(dir + " is not a directory");
}
string outputDir;
if (dir[dir.length()-1] == '/') {
outputDir = dir + "cloud";
}
else {
outputDir = dir + "/cloud";
}
int ret = mkdir(outputDir.c_str(), S_IRWXU | S_IRWXG | S_IRWXO);
if (ret != 0 && errno != EEXIST) {
throw runtime_error("unable to create directory " + outputDir);
}
if (!quiet) cout << endl << "== Writing corrected points to " << outputDir << " directory..." << endl;
// write the pose file
string poseFileName = outputDir + "/scan000.pose";
ofstream out(poseFileName.c_str(), ios::out);
out << "0 0 0" << endl;
out << "0 0 0" << endl;
out.close();
string pointsFileName = outputDir + "/scan000.3d";
out.open(pointsFileName.c_str(), ios::out);
// fetch each points and its color
vector<pair<Point3d, cv::Vec3i> > points;
getCorrectedWall(this->ceiling, points);
for (vector<pair<Point3d, cv::Vec3i> >::iterator it = points.begin(); it != points.end(); ++it) {
out << it->first << " " << static_cast<int>(it->second[0]) << " " << static_cast<int>(it->second[1]) << " " << static_cast<int>(it->second[2]) << endl;
}
getCorrectedWall(this->floor, points);
for (vector<pair<Point3d, cv::Vec3i> >::iterator it = points.begin(); it != points.end(); ++it) {
out << it->first << " " << static_cast<int>(it->second[0]) << " " << static_cast<int>(it->second[1]) << " " << static_cast<int>(it->second[2]) << endl;
}
for (vector<LabeledPlane3d>::iterator wall = this->walls.begin(); wall != this->walls.end(); ++wall) {
getCorrectedWall(*wall, points);
for (vector<pair<Point3d, cv::Vec3i> >::iterator it = points.begin(); it != points.end(); ++it) {
out << it->first << " " << static_cast<int>(it->second[0]) << " " << static_cast<int>(it->second[1]) << " " << static_cast<int>(it->second[2]) << endl;
}
}
out.close();
}
void model::Scene::getCorrectedWall(const LabeledPlane3d& surf, vector<pair<Point3d, cv::Vec3i> >& points) {
if (!quiet) cout << "** Computing point cloud for surface centered at " << surf.pt << endl;
// clear the output
points.clear();
if (surf.correctedDepthImg.empty()) {
if (!quiet) cout << "WARNING: the surface centered at " << surf.pt << " has not been corrected yet" << endl;
return;
}
if ((size_t) surf.correctedDepthImg.rows != surf.patches.size() ||
(size_t) surf.correctedDepthImg.cols != surf.patches.front().size())
{
throw runtime_error("corrected image must be the same size as depth map");
}
// compute the maximum of the depth image
int depthImgMax = 0;
for (size_t i = 0; i < surf.depthMap.size(); ++i) {
for (size_t j = 0; j < surf.depthMap.front().size(); ++j) {
if (depthImgMax < surf.depthMap[i][j]) {
depthImgMax = surf.depthMap[i][j];
}
}
}
// make the original points darker than the corrected ones
int r = 255, g = 50, b = 50;
int wr = 255, wg = 150, wb = 150;
for (int i = 0; i < surf.correctedDepthImg.rows; ++i) {
for (int j = 0; j < surf.correctedDepthImg.cols; ++j) {
cv::Vec3i color;
// only consider occupied and unknown pixels
if (surf.patches[i][j].second == OCCUPIED) {
color = cv::Vec3i(r, g, b);
} else if (surf.patches[i][j].second == UNKOWN) {
color = cv::Vec3i(wr, wg, wb);
} else {
continue;
}
// the value from the depth map
double depth = static_cast<double>(depthImgMax * surf.correctedDepthImg.data[i * surf.correctedDepthImg.cols + j]) / MAX_IMG_VAL;
// the distance from where the raytracing took place to the point it hit
double dist = depth + surf.depthMapDistances.first - surf.depthMapDistances.second;
Vector3d normal = surf.normal;
normal.normalize();
Point3d pt = surf.patches[i][j].first;
pt.x = pt.x + normal.x * dist;
pt.y = pt.y + normal.y * dist;
pt.z = pt.z + normal.z * dist;
points.push_back(make_pair(pt, color));
}
}
}
void model::Scene::flood(LabeledPlane3d& surf,
const int& i, const int& j,
const Label& target, const Label& replacement)
{
// no point of carrying on
if (target == replacement) {
return;
}
pair<int, int> north = make_pair(i-1, j);
pair<int, int> south = make_pair(i+1, j);
pair<int, int> east = make_pair(i, j+1);
pair<int, int> west = make_pair(i, j-1);
vector<pair<int, int> > neigh;
neigh.push_back(north);
neigh.push_back(south);
neigh.push_back(east);
neigh.push_back(west);
if (surf.patches[i][j].second == target) {
surf.patches[i][j].second = replacement;
}
for (vector<pair<int, int> >::iterator it = neigh.begin(); it != neigh.end(); ++it) {
if ((it->first > 0 && it->second > 0) &&
surf.patches[it->first][it->second].second == target)
{
flood(surf, it->first, it->second, target, replacement);
}
}
}
bool model::Scene::isOccupied(const Point3d& center, const double& width) {
// look for a close point
double pt[] = {center.x, center.y, center.z};
double* closest = this->octTree->FindClosest(pt, pow(sqrt(3) * (width / 2.0), 2.0), 0);
// bounding box limits
double xUppLim = center.x + width / 2.0;
double xLowLim = center.x - width / 2.0;
double yUppLim = center.y + width / 2.0;
double yLowLim = center.y - width / 2.0;
double zUppLim = center.z + width / 2.0;
double zLowLim = center.z - width / 2.0;
if (closest != NULL &&
(closest[0] > xLowLim && closest[0] < xUppLim) &&
(closest[1] > yLowLim && closest[1] < yUppLim) &&
(closest[2] > zLowLim && closest[2] < zUppLim))
{
return true;
}
return false;
}
Reference in a new issue Copy permalink