Permalink
Browse files

Merge branch 'master' of https://github.com/MichaelMathieu/depth-esti…

…mation

Conflicts:
	ardrone/depth_map.cpp
	ardrone/depth_map.h
	ardrone/test_simulator.cpp
	depth_estimation_api.lua
  • Loading branch information...
2 parents 9f326ed + 944fcfd commit 94f6ca32312690303c6f50b2a9770a56d8d3bb1f @MichaelMathieu committed May 18, 2012
View
@@ -1,7 +1,7 @@
LINK=`pkg-config --libs opencv` -lglut -lGLU -lGL -lluaT -ltorch-lua -lTH
FLAGS=-g -W -Wall -Wextra
CXX=g++
-CPP=simulator.cpp common.cpp ardrone_api.cpp radial_depth_map.cpp depth_map.cpp
+CPP=simulator.cpp common.cpp ardrone_api.cpp depth_map.cpp
OBJ=$(CPP:.cpp=.o)
all: simulator ardroneSDK
@@ -23,4 +23,4 @@ ardroneSDK:
clean:
rm -rf simulator test_simulator.o ${OBJ}
cd API/Examples/Linux/ && make clean && cd -
- rm -rf ardrone_sdk
+ rm -rf ardrone_sdk
View
@@ -42,48 +42,58 @@ void DepthMap::newPixel(float x, float y, float depth, float confidence,
ray[iBin].value() = 0.5f * (ray[iBin].value() + confidence);
}
-void DepthMap::newDisplacement(const matf & pos, const matf & sight) {
- // position
- Map new_map(nBinsTheta(), nBinsRho());
- double tx = pos(0,0), ty = pos(1,0);
- vector<SphericCoordinates> coords_tmp;
- for (int iTheta = 0; iTheta < nBinsTheta(); ++iTheta) {
- for (int iRho = 0; iRho < nBinsRho(); ++iRho) {
- BinIndex newBin = BinIndex(this, iRho, iTheta);
- newBin.getPointsInside(5,5, coords_tmp);
- float value = 0.0f;
- for (size_t iPt = 0; iPt < coords_tmp.size(); ++iPt) {
- CartesianCoordinates pt = coords_tmp[iPt].toCartesianCoordinates();
- pt.add(pos(0,0), pos(1,0));
- value += pt.toBinIndex().value();
+float DepthMap::getSafeTheta(size_t fov) {
+ assert(fov<nBinsTheta());
+ float safeTheta = 0;
+ int iniTheta = floor((nBinsTheta()-fov)/2);
+ int endTheta = iniTheta+fov;
+ size_t closestBin = nBinsRho()-1;
+ for (int iTheta=iniTheta; iTheta<endTheta; iTheta++) {
+ // printf("iTheta = %d\n", iTheta);
+ float maxConfidence = 1e-1;
+ size_t maxConfidenceBin = nBinsRho()-1;
+ for (int iRho=0; iRho<nBinsRho(); iRho++) {
+ // printf("iRho = %d\n", iRho);
+ float confidence = map(iTheta, iRho);
+ if (confidence>maxConfidence) {
+ maxConfidence = confidence;
+ maxConfidenceBin = iRho;
}
- new_map(newBin.iTheta, newBin.iRho) = value/(float)coords_tmp.size()*unseenDecay;
}
+ if (maxConfidenceBin < closestBin) {
+ closestBin = maxConfidenceBin;
+ safeTheta = -((float)(iTheta) / (float)(nBinsTheta()-1) - 0.5f) * 2.0f * PI;
+ }
+ // float theta = ((float)(iTheta) / (float)(nBinsTheta()-1) - 0.5f) * 2.0f * PI;
+ // printf("theta = %f, maxBin = %d\n", theta, maxConfidenceBin);
+ // safeTheta += theta*maxConfidenceBin/nBinsRho();
+
}
- map = new_map;
- // angle
- float theta = atan2(sight(1,0), sight(0,0));
- if (theta < 0.0f)
- theta = theta + 2.0f*PI;
- theta_sight = theta;
+ if (closestBin<8)
+ return safeTheta;
+ else
+ return 0;
}
-void DepthMap::newDisplacement2(const matf & pos, const matf & sight) {
+void DepthMap::newDisplacement(const matf & pos, const matf & sight) {
// position
Map new_map(nBinsTheta(), nBinsRho());
double tx = pos(0,0), ty = pos(1,0);
vector<SphericCoordinates> coords_tmp;
for (int iTheta = 0; iTheta < nBinsTheta(); ++iTheta) {
for (int iRho = 0; iRho < nBinsRho(); ++iRho) {
+
BinIndex newBin = BinIndex(this, iRho, iTheta);
- newBin.getPointsInside(5,5, coords_tmp);
+ newBin.getPointsInside(5, 5, coords_tmp);
float value = 0.0f;
+ // printf("iTheta = %d, iRho = %d\n", iTheta, iRho);
for (size_t iPt = 0; iPt < coords_tmp.size(); ++iPt) {
- CartesianCoordinates pt = coords_tmp[iPt].toCartesianCoordinates();
- pt.add(pos(0,0), pos(1,0));
- value += pt.toBinIndex().value();
+ CartesianCoordinates pt = coords_tmp[iPt].toCartesianCoordinates();
+ pt.add(pos(0,0), pos(1,0));
+ // printf("iPt = %d\n", iPt);
+ value += pt.toBinIndex().value();
}
- new_map(newBin.iTheta, newBin.iRho) = value/(float)coords_tmp.size()*unseenDecay;
+ new_map(newBin.iTheta, newBin.iRho) = value/(float)coords_tmp.size();//*unseenDecay;
}
}
map = new_map;
View
@@ -76,15 +76,23 @@ class DepthMap {
// if possible, do not use these functions outside BinIndex, SphericCoordinates and BinRay
// (so that it will be easier to add features)
inline size_t getIRhoFromRho(float rho) const;
+ inline size_t getIRhoFromRho2(float rho) const;
inline size_t getIThetaFromTheta(float theta) const;
+
inline float getRhoFromIRho(size_t iRho) const;
inline float getRho1FromIRho(size_t iRho) const;
inline float getRho2FromIRho(size_t iRho) const;
+
+ inline float getRhoFromIRho2(size_t iRho) const;
+ inline float getRho1FromIRho2(size_t iRho) const;
+ inline float getRho2FromIRho2(size_t iRho) const;
+
inline float getThetaFromITheta(size_t iTheta) const; //can be > PI (by small amount)
inline float getTheta1FromITheta(size_t iTheta) const; //never > PI
inline float getTheta2FromITheta(size_t iTheta) const; //can be > PI (by small amount)
BinRay getRayFromPixel(float x, float y, float wImg, float hImg);
+ float getSafeTheta(size_t fov);
inline size_t nBinsRho () const;
inline size_t nBinsTheta () const;
public:
View
@@ -1,25 +1,24 @@
-
// BinIndex
DepthMap::BinIndex::BinIndex(DepthMap* map, size_t iRho, size_t iTheta)
:map(map), iRho(iRho), iTheta(iTheta) {
}
DepthMap::SphericCoordinates DepthMap::BinIndex::toSphericCoordinates() const {
- return SphericCoordinates(map, map->getRhoFromIRho(iRho), map->getThetaFromITheta(iTheta));
+ return SphericCoordinates(map, map->getRhoFromIRho2(iRho), map->getThetaFromITheta(iTheta));
}
DepthMap::CartesianCoordinates DepthMap::BinIndex::toCartesianCoordinates() const {
return toSphericCoordinates().toCartesianCoordinates();
}
float DepthMap::BinIndex::getRho1() const {
- return map->getRho1FromIRho(iRho);
+ return map->getRho1FromIRho2(iRho);
}
float DepthMap::BinIndex::getRho2() const {
- return map->getRho2FromIRho(iRho);
+ return map->getRho2FromIRho2(iRho);
}
float DepthMap::BinIndex::getTheta1() const {
@@ -45,7 +44,7 @@ DepthMap::SphericCoordinates::SphericCoordinates(DepthMap* map, float rho, float
}
DepthMap::BinIndex DepthMap::SphericCoordinates::toBinIndex() const {
- return BinIndex(map, map->getIRhoFromRho(rho), map->getIThetaFromTheta(theta));
+ return BinIndex(map, map->getIRhoFromRho2(rho), map->getIThetaFromTheta(theta));
}
DepthMap::CartesianCoordinates DepthMap::SphericCoordinates::toCartesianCoordinates() const {
@@ -79,7 +78,7 @@ DepthMap::BinRay::BinRay(DepthMap* map, float theta)
}
size_t DepthMap::BinRay::getIBinFromDepth(float depth) const {
- return map->getIRhoFromRho(depth);
+ return map->getIRhoFromRho2(depth);
}
const DepthMap::BinIndex DepthMap::BinRay::operator[] (size_t i) const {
@@ -169,6 +168,14 @@ size_t DepthMap::getIRhoFromRho(float rho) const {
*/
}
+size_t DepthMap::getIRhoFromRho2(float rho) const {
+ assert(rho > 0);
+ if (rho > maxDepth)
+ return nBinsRho()-1;
+ else
+ return floor(rho / maxDepth * (float)nBinsRho());
+}
+
size_t DepthMap::getIThetaFromTheta(float theta) const {
float theta_rectified = theta + theta_sight;
if (theta_rectified > PI)
@@ -181,6 +188,11 @@ float DepthMap::getRhoFromIRho(size_t iRho) const {
//return ((float)iRho + 0.5f) * maxDepth/(float)nBinsRho();
}
+float DepthMap::getRhoFromIRho2(size_t iRho) const {
+ return 0.5f*(getRho1FromIRho2(iRho) + getRho2FromIRho2(iRho));
+ //return ((float)iRho + 0.5f) * maxDepth/(float)nBinsRho();
+}
+
float DepthMap::getRho1FromIRho(size_t iRho) const {
if (iRho == 0)
return 0.0f;
@@ -197,6 +209,14 @@ float DepthMap::getRho2FromIRho(size_t iRho) const {
//return ((float)iRho + 1.0f) * maxDepth/(float)nBinsRho();
}
+float DepthMap::getRho1FromIRho2(size_t iRho) const {
+ return maxDepth/((float)nBinsRho()) * (float)iRho;
+}
+
+float DepthMap::getRho2FromIRho2(size_t iRho) const {
+ return maxDepth/((float)nBinsRho()) * ((float)iRho+1);
+}
+
float DepthMap::getThetaFromITheta(size_t iTheta) const {
return getTheta1FromITheta(iTheta) + 1.0f*PI/(float)nBinsTheta();
}
@@ -86,15 +86,15 @@ mat3b RadialDepthMap::to2DMap() {
float k = 2.0f*maxDepth;
mat3b ret(size, size, cv::Vec3b(0.0f, 0.0f, 0.0f));
for (int i = 1; i < 5; ++i)
- ret(150+i, 150)[1] = 255;
- ret(150, 150)[2] = 255;
+ ret(size/2+i, size/2)[1] = 255;
+ ret(size/2, size/2)[2] = 255;
for (int iTheta = 0; iTheta < nBinsTheta(); ++iTheta) {
float theta = getThetaFromITheta(iTheta);
float rho = map(iTheta);
- float x = rho*cos(theta)+150;
- float y = rho*sin(theta)+150;
+ float x = rho*cos(theta) + size/2;
+ float y = rho*sin(theta) + size/2;
if (x<size && y<size && x>0 && y>0) {
ret(x, y)[0] = 255*(1-map.getVariance(iTheta)/MAX_VARIANCE);
View
@@ -14,9 +14,9 @@ SimulatedAPI::SimulatedAPI(int depthMapWidth, int depthMapHeight)
dmH(depthMapHeight), dmW(depthMapWidth),
alpha_friction(0.5f), focal_length(depthMapWidth),
obstacles() {
- for (int i=0; i< 50; ++i) {
- obstacles.push_back(Obstacle(10*(i+1), 5, 0, 1.0f));
- obstacles.push_back(Obstacle(10*(i+1), -5, 0, 1.0f));
+ for (int i=0; i< 100; ++i) {
+ obstacles.push_back(Obstacle(5*(i+1), -10+sin(i*0.5)*2*i, 0, 1.0f));
+ obstacles.push_back(Obstacle(5*(i+1), +10+sin(i*0.5)*2*i, 0, 1.0f));
}
}
@@ -75,9 +75,9 @@ matf SimulatedAPI::getIMUTranslation() const {
matf up = getUp();
matf v = dx * delta_t;
matf ret(3,1);
- ret(0,0) = v.dot(pray);// + randn(0, 0.2);
- ret(1,0) = v.dot(npray);// + randn(0, 0.2);
- ret(2,0) = v.dot(up);// + randn(0, 0.2);
+ ret(0,0) = v.dot(pray) + randn(0, 0.2);
+ ret(1,0) = v.dot(npray) + randn(0, 0.2);
+ ret(2,0) = v.dot(up) + randn(0, 0.2);
return ret;
}
@@ -87,9 +87,9 @@ matf SimulatedAPI::getVisualOdometryTranslation() const {
matf up = getUp();
matf v = dx * delta_t;
matf ret(3,1);
- ret(0,0) = v.dot(pray);// + randn(0, 0.1);
- ret(1,0) = v.dot(npray);// + randn(0, 0.1);
- ret(2,0) = v.dot(up);// + randn(0, 0.1);
+ ret(0,0) = v.dot(pray) + randn(0, 0.1);
+ ret(1,0) = v.dot(npray) + randn(0, 0.1);
+ ret(2,0) = v.dot(up) + randn(0, 0.1);
return ret;
}
View
@@ -1,7 +1,10 @@
+#include "depth_map.h"
+// #include "radial_map.h"
+// #include "bin_map.h"
+// #include "point_map.h"
+
#include "simulator.h"
#include "ardrone_api.h"
-#include "radial_depth_map.h"
-#include "depth_map.h"
#include <GL/glut.h>
#include <iostream>
#include <ctime>
@@ -13,19 +16,20 @@ float dyaw = 0.0f, pitch =0.0f, roll = 0.0f, gaz = 0.0f;
bool flying = false;
DroneAPI* pApi = NULL;
GLuint map_texture;
-//RadialDepthMap* pMap = NULL;
DepthMap* pMap = NULL;
+// BinMap* pMap = NULL;
+// PointMap* pMap = NULL;
void keyboard(int key, bool special, bool down) {
if (special) {
switch(key) {
case GLUT_KEY_LEFT:
//turn left
- if (down) dyaw = -1.0f; else dyaw = 0.0f;
+ if (down) dyaw = -0.3f; else dyaw = 0.0f;
break;
case GLUT_KEY_RIGHT:
//turn right
- if (down) dyaw = 1.0f; else dyaw = 0.0f;
+ if (down) dyaw = 0.3f; else dyaw = 0.0f;
break;
case GLUT_KEY_UP:
//move up
@@ -40,19 +44,19 @@ void keyboard(int key, bool special, bool down) {
switch (key) {
case 'a':
//move left
- if (down) roll = -100.0f; else roll = 0.0f;
+ if (down) roll = -0.3f; else roll = 0.0f;
break;
case 'd':
//move right
- if (down) roll = 100.0f; else roll = 0.0f;
+ if (down) roll = 0.3f; else roll = 0.0f;
break;
case 'w':
//move forward
- if (down) pitch = -100.0f; else pitch = 0.0f;
+ if (down) pitch = 0.5f; else pitch = 0.0f;
break;
case 's':
//move backward
- if (down) pitch = 100.0f; else pitch = 0.0f;
+ if (down) pitch = -0.3f; else pitch = 0.0f;
break;
case ' ':
//takeoff/land
@@ -80,6 +84,27 @@ void keyboardUp2(int key, int, int) {
#include "opencv/highgui.h"
void idle() {
+
+ float safeTheta = pMap->getSafeTheta(16);
+ //printf("safeTheta = %f\n", safeTheta);
+
+ keyboard('w', false, true);
+ //roll = safeTheta*0.1f;
+ if (safeTheta>0) {
+ keyboard('d', false, true);
+ keyboard(GLUT_KEY_RIGHT, true, true);
+ }
+ if (safeTheta<0) {
+ keyboard('a', false, true);
+ keyboard(GLUT_KEY_LEFT, true, true);
+ }
+ if (safeTheta == 0) {
+ keyboard('a', false, false);
+ keyboard('d', false, false);
+ keyboard(GLUT_KEY_RIGHT, true, false);
+ keyboard(GLUT_KEY_LEFT, true, false);
+ }
+
pApi->next();
cout << pitch << " " << roll << " " << dyaw << " " << gaz << endl;
cout << pApi->toString() << endl;
@@ -92,6 +117,7 @@ void idle() {
printf("%d %d\n", win_h, win_w);
glutReshapeWindow(win_w, win_h);
}
+
//glDrawPixels(win_w, win_h, GL_LUMINANCE, GL_FLOAT, (float*)((matf)(0.01f*frameDMap)).data);
//glutSwapBuffers();
@@ -120,11 +146,12 @@ void render() {
}
int main(int argc, char* argv[]) {
- //SimulatedAPI api(320, 240);
- ARdroneAPI api("control_pipe", "navdata_pipe");
+ SimulatedAPI api(320, 240);
+ // ARdroneAPI api("control_pipe", "navdata_pipe");
pApi = &api;
//RadialDepthMap map(512, 100, 1.0f, 320);
- DepthMap map(64, 64, 60, 0.9f, 320);
+ DepthMap map(64, 128, 100, 0.9f, 320);
+ //PointMap map(16, 16, 100, 0.9f, 320);
pMap = &map;
glutInit(&argc, argv);
glutInitWindowPosition(0,0);
Oops, something went wrong.

0 comments on commit 94f6ca3

Please sign in to comment.