Skip to content
Snippets Groups Projects
Commit 56fb6a5d authored by simon.fanetti's avatar simon.fanetti
Browse files

started refactoring

parents
No related branches found
No related tags found
No related merge requests found
Showing with 1003 additions and 0 deletions
Makefile 0 → 100644
include ./lib/common.mk
SRCS=$(wildcard ./*.cpp)
OBJS=$(SRCS:%.cpp=%.o)
OBJSALL=$(shell find . -name '*.o')
all:
$(MAKE) -C lib
$(MAKE) build -C .
$(MAKE) pack -C .
build: $(OBJS)
%.o: %.cpp
g++ -c $(CFLAGS) $(OPENCVFLAG) $< -o $@
# manque tout les autres fichiers .o
pack:
gcc -shared $(OBJSALL) $(CFLAGS) $(OPENCVFLAG) -lrealsense2 -o ./libsandbox.so
clean:
-rm -f *.o *.so
$(MAKE) clean -C lib
SCREEN=DVI-D-0
SCN_RES=1920x1080
BEAMER=HDMI-0
BM_RES=1400x1050
xrandr --output $SCREEN --rate 60 --mode $SCN_RES --fb $SCN_RES --panning $SCN_RES* \
--output $BEAMER --mode $BM_RES --same-as $SCREEN > /dev/null
0.643826 0.269363 0.329644
984.25
3 2
0 0 0
0 0 0
160 120
160 360
480 360
480 120
include common.mk
SRCS:=$(wildcard *.cpp)
OBJS:=$(SRCS:%.cpp=%.o)
all:
$(MAKE) -C components
#$(MAKE) -C tools
$(MAKE) build -C .
build: $(OBJS)
%.o: %.cpp
g++ $(CFLAGS) $(OPENCVFLAG) -c $< -o $@
clean:
-rm -f *.o
$(MAKE) clean -C components
#$(MAKE) clean -C tools
CFLAGS=-std=c++11 -Wall -Wextra -g -Ilib -fPIC
OPENCVFLAG=`pkg-config --libs --cflags opencv`
include ../common.mk
SRCS=$(wildcard *.cpp)
OBJS=$(SRCS:%.cpp=%.o)
CAMERAFLAG=-lrealsense2
build: $(OBJS)
camera.o: camera.cpp camera.h
g++ -c $(CFLAGS) $(OPENCVFLAG) $(CAMERAFLAG) $< -o $@
%.o: %.cpp
g++ -c $(CFLAGS) $(OPENCVFLAG) $< -o $@
clean:
-rm -f *.o
#include "beamer.h"
using namespace cv;
using namespace std;
#define ESCAPE_CHAR 27
/*
Calculate the line segment PaPb that is the shortest route between
two lines P1P2 and P3P4. Calculate also the values of mua and mub where
Pa = P1 + mua (P2 - P1)
Pb = P3 + mub (P4 - P3)
Return FALSE if no solution exists.
http://paulbourke.net/geometry/pointlineplane/
*/
typedef struct
{
double x, y, z;
} XYZ;
int LineLineIntersect(
Point3d p1, Point3d p2, Point3d p3, Point3d p4, Point3d *pa, Point3d *pb,
double *mua, double *mub)
{
float EPS = 0.0001;
XYZ p13, p43, p21;
double d1343, d4321, d1321, d4343, d2121;
double numer, denom;
p13.x = p1.x - p3.x;
p13.y = p1.y - p3.y;
p13.z = p1.z - p3.z;
p43.x = p4.x - p3.x;
p43.y = p4.y - p3.y;
p43.z = p4.z - p3.z;
if (abs(p43.x) < EPS && abs(p43.y) < EPS && abs(p43.z) < EPS)
return (false);
p21.x = p2.x - p1.x;
p21.y = p2.y - p1.y;
p21.z = p2.z - p1.z;
if (abs(p21.x) < EPS && abs(p21.y) < EPS && abs(p21.z) < EPS)
return (true);
d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;
denom = d2121 * d4343 - d4321 * d4321;
if (abs(denom) < EPS)
return (false);
numer = d1343 * d4321 - d1321 * d4343;
*mua = numer / denom;
*mub = (d1343 + d4321 * (*mua)) / d4343;
pa->x = p1.x + *mua * p21.x;
pa->y = p1.y + *mua * p21.y;
pa->z = p1.z + *mua * p21.z;
pb->x = p3.x + *mub * p43.x;
pb->y = p3.y + *mub * p43.y;
pb->z = p3.z + *mub * p43.z;
return (true);
}
Beamer::Beamer(){
//position par défaut
beamerPosition = Point3f(0.0f, 0.265f, -0.205f);
}
vector<int> Beamer::findCercleZ(Mat &rgb)
{
Mat src_gray;
cvtColor(rgb, src_gray, CV_BGR2GRAY);
/// Reduce the noise so we avoid false circle detection
GaussianBlur(src_gray, src_gray, Size(9, 9), 2, 2);
vector<Vec3f> circles;
circles.clear();
/// Apply the Hough Transform to find the circles
//source, output, method, inverse ratio of resolution, Minimum distance between detected centers, threeshold canny, threeshold center, min radius, max radius
HoughCircles(src_gray, circles, CV_HOUGH_GRADIENT, 1, src_gray.rows / 4, 75, 50, 0, 0);
//doit tester si le cercle est bon (rayon);
vector<int> result;
if (!circles.empty())
{
for (int i = 0; i < 3; i++)
{
result.push_back(round(circles[0][i]));
}
}
return result;
}
void Beamer::findBeamer(Camera camera)
{
char wname[] = "FindBeamer";
namedWindow(wname, CV_WINDOW_NORMAL);
setWindowProperty(wname, CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
Mat depth;
Mat rgb;
Mat frameImage(Size(1400, 1050), CV_8UC3, Scalar(0, 0, 0));
vector<Point> points;
points.push_back(Point(500, 500));
points.push_back(Point(1000, 300));
points.push_back(Point(300, 800));
unsigned int nbPoint = 3; //number of point to calculate 1 vector
vector<Point3d> points1; //vectors calculate for each point
vector<Point3d> points2; //1 point for each vector (to calculate constante d)
double fact = -20.0;
for (int i = 0; i < (int)points.size(); i++)
{
vector<Point3f> capturedPoints;
Point p = points[i];
while (1)
{
camera.captureFramesAlign();
depth = camera.getDepthFrameAlign();
rgb = camera.getRGBFrameAlign();
Scalar color;
vector<int> crc = findCercleZ(rgb);
if (!crc.empty())
color = Scalar(0, 255, 0);
else
color = Scalar(0, 0, 255);
line(frameImage, Point(p.x, 0), Point(p.x, frameImage.rows - 1), color, 4);
line(frameImage, Point(0, p.y), Point(frameImage.cols - 1, p.y), color, 4);
putText(frameImage, to_string(capturedPoints.size() + 1) + "/" + to_string(nbPoint), Point(400, 400), FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 255, 255));
cv::imshow(wname, frameImage);
char keyCode = waitKey(500);
if (keyCode == ESCAPE_CHAR)
exit(0);
else if (keyCode == ' ' && !crc.empty())
{
float coord[2] = {(float)crc[0], (float)crc[1]};
float z = static_cast<float>(depth.at<uint16_t>(crc[1], crc[0]));
Point3f p = camera.deprojectPixelToPoint(coord, z / 1000.0);
capturedPoints.push_back(p);
}
if (capturedPoints.size() == nbPoint)
{
Vec3f dir(capturedPoints[0].x - capturedPoints[1].x, capturedPoints[0].y - capturedPoints[1].y, capturedPoints[0].z - capturedPoints[1].z);
Vec6f line;
fitLine(capturedPoints, line, CV_DIST_L2, 0, 0.01, 0.01);
points1.push_back(Point3d(line[3] * fact, line[4] * fact, line[5] * fact));
points2.push_back(Point3d(line[0], line[1], line[2]));
frameImage.setTo(Scalar(0, 0, 0));
break;
}
frameImage.setTo(Scalar(0, 0, 0));
}
}
Point3d pa, pb;
double mua;
double mub;
vector<Point3d> beamerPoints;
LineLineIntersect(points1[0], points2[0], points1[1], points2[1], &pa, &pb, &mua, &mub);
beamerPoints.push_back(pa);
beamerPoints.push_back(pb);
LineLineIntersect(points1[0], points2[0], points1[2], points2[2], &pa, &pb, &mua, &mub);
beamerPoints.push_back(pa);
beamerPoints.push_back(pb);
LineLineIntersect(points1[1], points2[1], points1[2], points2[2], &pa, &pb, &mua, &mub);
beamerPoints.push_back(pa);
beamerPoints.push_back(pb);
Point3d beamerPoint(0.0, 0.0, 0.0);
for (unsigned int i = 0; i < beamerPoints.size(); i++)
{
beamerPoint += beamerPoints[i];
}
beamerPoint /= 6.0;
cout << "Beamer position: " << beamerPoint.x << " " << beamerPoint.y << " " << beamerPoint.z << "\n";
//set beamer position
beamerPosition.x = (float)beamerPoint.x;
beamerPosition.y = (float)beamerPoint.y;
beamerPosition.z = (float)beamerPoint.z;
/*
TODO : save beamerPosition in config_file
*/
cout << "Saving beamer position in file..." << endl;
std::ofstream myfile;
myfile.open(BEAMER_POSITION_FILE);
myfile << beamerPosition.x << " " << beamerPosition.y << " " << beamerPosition.z << endl;
myfile.close();
cout << "Done!" << endl;
destroyAllWindows();
}
#ifndef BEAMER_H
#define BEAMER_H
#include <opencv2/opencv.hpp>
#include "camera.h"
class Beamer
{
const char *BEAMER_POSITION_FILE = "./beamer.dat";
float solveD(cv::Vec3f v, cv::Point3f p);
cv::Point3f intersection(cv::Vec3f v1, cv::Point3f p1, cv::Vec3f v2, cv::Point3f p2, cv::Vec3f v3, cv::Point3f p3, bool &isFound);
std::vector<int> findCercleZ(cv::Mat &rgb);
cv::Point3f beamerPosition;
public:
Beamer();
static const int width = 1400;
static const int height = 1050;
cv::Point3f getPosition()
{
return beamerPosition;
};
void findBeamer(Camera camera);
};
#endif
#include "calibrate.h"
#include "camera.h"
using namespace cv;
using namespace std;
Calibrate::Calibrate()
{
}
Point2i Calibrate::rotatePixel(Point2i pixel)
{
Mat tmp = (Mat_<Vec2f>(1, 1) << Vec2f(pixel.x, pixel.y));
transform(tmp, tmp, matRotation);
return Point2i(tmp.at<Vec2f>(0, 0));
}
Point2i Calibrate::transformationPixel(int i, int j, float z, Camera camera, Point3f beamer)
{
//pixel to point 3d
float coord[2] = {static_cast<float>(j), static_cast<float>(i)};
Point3f p = camera.deprojectPixelToPoint(coord, z / 1000.0);
//calcul vector point to beamer
Vec3f dir(beamer.x - p.x, beamer.y - p.y, beamer.z - p.z);
float prop = (0.99 - p.z) / dir[2];
p.x += dir[0] * prop;
p.y += dir[1] * prop;
p.z += dir[2] * prop;
//point 3d to pixel
return camera.projectPointToPixel(p);
}
void Calibrate::transformationFrame(cv::Mat &src, cv::Mat &dst, Camera camera, Point3f beamer)
{
int64_t t1 = getTickCount();
//transformation on all pixel
for (int i = 0; i < src.rows; i++)
{
for (int j = 0; j < src.cols; j++)
{
Point pixelIJ(j, i);
Point pixel = transformationPixel(i, j, static_cast<float>(src.at<uint16_t>(pixelIJ)), camera, beamer);
if (pixel.x < dst.cols && pixel.y < dst.rows && pixel.x >= 0 && pixel.y >= 0)
dst.at<uint16_t>(pixel) = src.at<uint16_t>(pixelIJ);
}
}
cout << "temps de calcul: " << (getTickCount() - t1) / getTickFrequency() << endl;
warpAffine(dst, dst, matRotation, dst.size());
// medianBlur(dst, dst, 3);
}
void Calibrate::transformationFrame(cv::Mat &depth, cv::Mat &src, cv::Mat &dst, Camera camera, Point3f beamer)
{
int nbChannel = src.channels();
//transformation on all pixel
int64_t t1 = getTickCount();
for (int i = 0; i < src.rows; i++)
{
for (int j = 0; j < src.cols; j++)
{
Point pixelIJ(j, i);
Point pixel = transformationPixel(i, j, static_cast<float>(depth.at<uint16_t>(pixelIJ)), camera, beamer);
if (pixel.x < dst.cols && pixel.y < dst.rows && pixel.x >= 0 && pixel.y >= 0)
{
if (nbChannel == 1)
dst.at<char>(pixel) = src.at<char>(pixelIJ);
else if (nbChannel == 3)
dst.at<Vec3b>(pixel) = src.at<Vec3b>(pixelIJ);
}
}
}
cout << "temps de calcul: " << (getTickCount() - t1) / getTickFrequency() << endl;
warpAffine(dst, dst, matRotation, dst.size());
dilate(dst, dst, Mat(), Point(-1, -1), 2, 1, 1);
erode(dst, dst, Mat(), Point(-1, -1), 2, 1, 1);
}
#ifndef CALIBRATE_H
#define CALIBRATE_H
#include <opencv2/opencv.hpp>
#include "beamer.h"
#include "camera.h"
class Calibrate
{
private:
const char *wndname = (char *)"Sandbox";
cv::Mat matRotation;
float distancePlan;
public:
Calibrate();
cv::Point2i transformationPixel(int i, int j, float z, Camera camera, cv::Point3f beamer);
cv::Point2i rotatePixel(cv::Point2i pixel);
void transformationFrame(cv::Mat &src, cv::Mat &dst, Camera camera, cv::Point3f beamer);
void transformationFrame(cv::Mat &depth, cv::Mat &src, cv::Mat &dst, Camera camera, cv::Point3f beamer);
void setMatrixRotation(cv::Mat matrix)
{
matRotation = matrix.clone();
}
void setDistancePlan(float distance)
{
distancePlan = distance;
}
};
#endif
\ No newline at end of file
#include "camera.h"
using namespace cv;
Camera::Camera() {}
// Capture 30 frames to give autoexposure, etc. a chance to settle
void Camera::warmingUp()
{
for (int i = 0; i < 30; ++i)
{
rs2::frameset data = pipe.wait_for_frames();
auto frameDepth = data.get_depth_frame();
//frameDepth = decFilter.process(frameDepth);
frameDepth = spatFilter.process(frameDepth);
frameDepth = tempFilter.process(frameDepth);
}
}
Mat Camera::coloredFrame(Mat frameDepth)
{
Mat depthFrameColored(frameDepth.size(), CV_8U);
int width = frameDepth.cols, height = frameDepth.rows;
static uint32_t histogram[0x10000];
memset(histogram, 0, sizeof(histogram));
for (int i = 0; i < height; ++i)
{
for (int j = 0; j < width; ++j)
{
++histogram[frameDepth.at<ushort>(i, j)];
}
}
for (int i = 2; i < 0x10000; ++i)
histogram[i] += histogram[i - 1]; // Build a cumulative histogram for the indices in [1,0xFFFF]
for (int i = 0; i < height; ++i)
{
for (int j = 0; j < width; ++j)
{
if (uint16_t d = frameDepth.at<ushort>(i, j))
{
int f = histogram[d] * 255 / histogram[0xFFFF]; // 0-255 based on histogram location
depthFrameColored.at<uchar>(i, j) = static_cast<uchar>(f);
}
else
{
depthFrameColored.at<uchar>(i, j) = 0;
}
}
}
bitwise_not(depthFrameColored, depthFrameColored); //reverse colormap
applyColorMap(depthFrameColored, depthFrameColored, cv::COLORMAP_JET);
depthFrameColored.setTo(cv::Scalar(0, 0, 0), (frameDepth == 0));
return depthFrameColored;
}
Mat Camera::captureFrame()
{
auto frame = pipe.wait_for_frames();
auto frameDepth = frame.get_depth_frame();
filterDepthFrame(frameDepth);
Mat matFrame(Size(frameDepth.get_width(), frameDepth.get_height()), CV_16UC1, (void *)frameDepth.get_data(), Mat::AUTO_STEP);
return matFrame;
}
void Camera::captureFramesAlign()
{
rs2::align align(RS2_STREAM_DEPTH);
auto frameset = pipe.wait_for_frames();
//Get processed aligned frame
frameset = align.process(frameset);
// Trying to get color and aligned depth frames
rs2::video_frame color_frame = frameset.get_color_frame();
rs2::depth_frame depth_frame = frameset.get_depth_frame();
//filterDepthFrame(depth_frame);
matDepth = Mat(Size(depth_frame.get_width(), depth_frame.get_height()), CV_16UC1, (void *)depth_frame.get_data(), Mat::AUTO_STEP);
Mat(Size(color_frame.get_width(), color_frame.get_height()), CV_8UC3, (void *)color_frame.get_data(), Mat::AUTO_STEP).copyTo(matRGB);
}
void Camera::filterDepthFrame(rs2::depth_frame &frameDepth)
{
// frameDepth = decFilter.process(frameDepth);
frameDepth = spatFilter.process(frameDepth);
frameDepth = tempFilter.process(frameDepth);
intrinsics = frameDepth.get_profile().as<rs2::video_stream_profile>().get_intrinsics();
}
Mat Camera::getAverageFrame(int numberFrame)
{
Mat averageFrame;
Camera::captureFrame().copyTo(averageFrame);
for (int i = 1; i <= numberFrame; i++)
{
averageFrame *= i;
add(averageFrame, Camera::captureFrame(), averageFrame);
averageFrame /= i + 1;
}
return averageFrame;
}
Point3f Camera::deprojectPixelToPoint(float coord[], float z1)
{
float p[3];
rs2_deproject_pixel_to_point(p, &intrinsics, coord, z1);
return Point3f(p[0], p[1], p[2]);
}
Point2i Camera::projectPointToPixel(Point3f point3D)
{
float point[3] = {point3D.x, point3D.y, point3D.z};
float pixel[2];
rs2_project_point_to_pixel(pixel, &intrinsics, point);
return Point2i(pixel[0], pixel[1]);
}
void Camera::start()
{
rs2::log_to_console(RS2_LOG_SEVERITY_ERROR);
//spatFilter.set_option(RS2_OPTION_HOLES_FILL, 5);
profile = pipe.start(cfg);
auto sensor = profile.get_device().first<rs2::depth_sensor>();
// TODO: At the moment the SDK does not offer a closed enum for D400 visual presets
// We do this to reduce the number of black pixels
// The hardware can perform hole-filling much better and much more power efficient then our software
auto range = sensor.get_option_range(RS2_OPTION_VISUAL_PRESET);
for (auto i = range.min; i < range.max; i += range.step)
if (std::string(sensor.get_option_value_description(RS2_OPTION_VISUAL_PRESET, i)) == "High Density")
sensor.set_option(RS2_OPTION_VISUAL_PRESET, i);
warmingUp();
}
void Camera::startAlign()
{
rs2::log_to_console(RS2_LOG_SEVERITY_ERROR);
spatFilter.set_option(RS2_OPTION_HOLES_FILL, 5);
//cfg.enable_device_from_file("../input/flux/flux5.bag");
profile = pipe.start(cfg);
auto sensor = profile.get_device().first<rs2::depth_sensor>();
// TODO: At the moment the SDK does not offer a closed enum for D400 visual presets
// We do this to reduce the number of black pixels
// The hardware can perform hole-filling much better and much more power efficient then our software
auto range = sensor.get_option_range(RS2_OPTION_VISUAL_PRESET);
for (auto i = range.min; i < range.max; i += range.step)
if (std::string(sensor.get_option_value_description(RS2_OPTION_VISUAL_PRESET, i)) == "High Density")
sensor.set_option(RS2_OPTION_VISUAL_PRESET, i);
warmingUp();
}
void Camera::stop()
{
pipe.stop();
}
\ No newline at end of file
#ifndef CAMERA_H
#define CAMERA_H
#include <librealsense2/rs.hpp>
#include <librealsense2/rsutil.h>
#include <opencv2/opencv.hpp>
class Camera
{
private:
//constants in mm
//const float scale = rs2_get_depth_scale(sensor, NULL);
static const int maxHeightSand = 400;
static const int maxZ = 1120;
static const int minZ = maxZ - maxHeightSand;
rs2::spatial_filter spatFilter;
rs2::temporal_filter tempFilter;
rs2::decimation_filter decFilter;
rs2::config cfg;
rs2::pipeline pipe;
rs2::pipeline_profile profile;
rs2_intrinsics intrinsics;
void flipMat(cv::Mat &m);
void filterDepthFrame(rs2::depth_frame &frameDepth);
cv::Mat matDepth;
cv::Mat matRGB;
public:
Camera();
void start();
void stop();
// Capture 30 frames to give autoexposure, etc. a chance to settle
void warmingUp();
cv::Mat captureFrame();
cv::Mat getAverageFrame(int numberFrame);
static cv::Mat coloredFrame(cv::Mat frameDepth);
cv::Point3f deprojectPixelToPoint(float coord[], float z1);
cv::Point2i projectPointToPixel(cv::Point3f point3D);
void captureFramesAlign();
void startAlign();
cv::Mat getDepthFrameAlign()
{
return matDepth;
}
cv::Mat getRGBFrameAlign()
{
return matRGB.clone();
}
};
#endif
\ No newline at end of file
#include "./controller.h"
#include <numeric>
#include <fstream>
#include <string>
using namespace std;
using namespace cv;
Controller::Controller(){
bool configSanbox = true;
//cout << "All files there? " << configFilesFound() << endl;
if(configSanbox){
// Blue screen and edit colored frame routine
sanboxBorder();
// Matching cross under beamer routine
beamer.findBeamer(camera);
}else{
sandboxBorderLoad();
//beamer.findBeamerLoad(camera);
}
// Set full screen parameter for show function
createWindowsFullScreen(defaultWindowsName);
}
Mat Controller::getRGBFrame(){
camera.captureFramesAlign();
return camera.getRGBFrameAlign()(rectSandbox);
}
Mat Controller::getDepthFrame(){
camera.captureFramesAlign();
return camera.getDepthFrameAlign()(rectSandbox);
}
void Controller::showImage(cv::Mat* image){
showImage(image, defaultWindowsName);
}
void Controller::showImage(cv::Mat* image, char *windowName){
static Mat frameBeamer(Size(Beamer::width, Beamer::height), CV_8UC3);
camera.captureFramesAlign();
Mat depth = camera.getDepthFrameAlign()(rectSandbox);
resize(*image, *image, depth.size());
Mat imageCalibrate(depth.size(), CV_8UC3, Scalar(0, 0, 0));
calibrate.transformationFrame(depth, *image, imageCalibrate, camera, beamer.getPosition());
//flip to align frame with beamer
flip(imageCalibrate, imageCalibrate, 1);
flip(imageCalibrate, imageCalibrate, 0);
resize(imageCalibrate, frameBeamer, frameBeamer.size());
imshow(windowName, frameBeamer);
}
double Controller::toDegrees(double radians){
return radians * (180.0 / M_PI);
}
/*
bool Sandbox::readSandboxPosition(vector<Point> &rectPoints)
{
std::ifstream infile(SANDBOX_POSITION_FILE);
if (infile.good())
{
int x, y;
while (infile >> x >> y)
{
rectPoints.push_back(Point(x, y));
cout << x << " " << y << endl;
}
}
infile.close();
return rectPoints.size() == 4;
}
void Sandbox::writeSandboxPosition(vector<Point> rectPoints)
{
//export points to file
std::ofstream myfile;
myfile.open(SANDBOX_POSITION_FILE);
for (unsigned int i = 0; i < rectPoints.size(); i++)
myfile << rectPoints[i].x << " " << rectPoints[i].y << endl;
myfile.close();
}*/
/*
void Sandbox::sandboxBorderLoad(){
float distancePlan;
Mat matRotation;
// Distance Plan
std::ifstream infile(CALIBRATE_DISTANCE_FILE);
if (infile.good()){
while (infile >> distancePlan){
cout << distancePlan << endl;
}
}
infile.close();
// Matrix Rotation
char *filename = "";
strcpy(filename, CALIBRATE_MATRIX_FILE);
importFrame(filename, matRotation);
calibrate.setDistancePlan(distancePlan);
}
void Sandbox::exportToAsc(Mat src, char *fileName)
{
std::ofstream myfile;
myfile.open(fileName);
Mat frameSand = src(rectSandbox);
for (int y = 0; y < src.rows; y++)
{
for (int x = 0; x < src.cols; x++)
{
myfile << x << " " << y << " " << frameSand.at<uint16_t>(y, x) << "\n";
}
}
myfile.close();
}
void Sandbox::exportFrame(Mat src, char *fileName)
{
std::ofstream myfile;
myfile.open(fileName);
Mat frameSand = src(rectSandbox);
myfile << frameSand.cols << " " << frameSand.rows << endl;
for (int y = 0; y < frameSand.rows; y++)
{
for (int x = 0; x < frameSand.cols; x++)
{
myfile << frameSand.at<uint16_t>(y, x) << CHAR_DELIM;
}
myfile << endl;
}
myfile.close();
}
bool Sandbox::importFrame(char *fileName, Mat &output)
{
std::ifstream file(fileName);
if (file.is_open())
{
string elem;
int i = 0;
//read header
getline(file, elem, CHAR_DELIM);
int width = atoi(elem.c_str());
getline(file, elem, CHAR_DELIM);
int height = atoi(elem.c_str());
//create & read matrix
output = Mat(Size(width, height), CV_16UC1);
cout << width << " " << height << endl;
while (getline(file, elem, CHAR_DELIM))
{
output.at<uint16_t>(i / width, i % width) = (uint16_t)atoi(elem.c_str());
i++;
}
file.close();
return true;
}
return false;
}
bool Sandbox::configFilesFound(){
const char* files[] = { SANDBOX_POSITION_FILE,
CALIBRATE_DISTANCE_FILE,
CALIBRATE_MATRIX_FILE };
int size = (sizeof(files)/sizeof(char*));
for (int i = 0; i < size; i++)
{
char *filename = "";
strcpy(filename, files[i]);
std::ifstream infile(filename);
if(infile.fail()){
cout << "Configuration file missing...";
return false;
}
}
return true;
}*/
void Controller::sanboxBorder()
{
char windowName[] = "border";
createWindowsFullScreen(windowName);
Mat frameBeamer(Size(Beamer::width, Beamer::height), CV_8UC3, Scalar(255, 0, 0));
// Show blue frame
imshow(windowName, frameBeamer);
waitKey(100);
camera.startAlign(); // 1 seconde of warming up
camera.captureFramesAlign();
Mat frameData = camera.getDepthFrameAlign();
Mat coloredFrame = camera.getRGBFrameAlign();
Size s = frameData.size();
Point center(s.width / 2, s.height / 2);
// calibrate distance
//Point center(s.width / 2, s.height / 2);
//float distancePlan = static_cast<float>(mean(frameData(Rect(center, Size(10, 10))))[0]); // get sandbox distance from center of camera
//calibrate.setDistancePlan(distancePlan);
destroyAllWindows();
vector<Point> rectPoints;
float y = coloredFrame.size().height;
float x = coloredFrame.size().width;
rectPoints.push_back(Point(1.0/4*x, 1.0/4*y));
rectPoints.push_back(Point(1.0/4*x, 3.0/4*y));
rectPoints.push_back(Point(3.0/4*x, 3.0/4*y));
rectPoints.push_back(Point(3.0/4*x, 1.0/4*y));
cout << "Edit Rectangle" << endl;
BorderEdit::edit(coloredFrame, &rectPoints); // edit projected frame
// adjust model matrixe to forme a rectangle in sandbox
int widthTop = rectPoints[3].x - rectPoints[0].x;
double angle1 = atan((double)(rectPoints[3].y - rectPoints[0].y) / widthTop);
Mat matRotation = getRotationMatrix2D(center, toDegrees(angle1), 1);
calibrate.setMatrixRotation(matRotation);
cout << "Calibrate Rotation Matrixe" << endl;
Size rectSize = Size(widthTop, cvRound(widthTop / 1.33333) + 5);
Point p = calibrate.rotatePixel(rectPoints[0]);
// rectSandbox => CroppingMask
rectSandbox = Rect(p, rectSize);
//writeSandboxPosition(rectPoints); // update with adjusted sandbox's angles
destroyAllWindows();
/*
TODO : save matRotation in config_file
*/
/*cout << "Saving matrixe in file..." << endl;
myfile.open(CALIBRATE_MATRIX_FILE);
cout << "Data : " << matRotation.rows << " : " << matRotation.cols << endl;
myfile << matRotation.cols << " " << matRotation.rows << endl;
for (int y = 0; y < matRotation.rows; y++)
{
for (int x = 0; x < matRotation.cols; x++)
{
myfile << matRotation.at<uint16_t>(y, x) << CHAR_DELIM;
}
myfile << endl;
}
myfile.close();
cout << "Done..." << endl;*/
}
void Controller::createWindowsFullScreen(char *windowName)
{
namedWindow(windowName, CV_WINDOW_NORMAL);
setWindowProperty(windowName, CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
}
#ifndef CONTROLLER_H
#define CONTROLLER_H
#include "./components/camera.h"
#include "./components/calibrate.h"
#include "./components/beamer.h"
#include "./tools/borderedit.h"
#include "./tools/borderfinder.h"
class Controller{
public:
Controller();
cv::Mat getRGBFrame();
cv::Mat getDepthFrame();
void showImage(cv::Mat* image);
//void exportToAsc(cv::Mat frame, char *fileName);
//void exportFrame(cv::Mat src, char *fileName);
//bool importFrame(char *fileName, cv::Mat &output);
private:
const char *SANDBOX_POSITION_FILE = "./sandbox.dat";
const char *CALIBRATE_DISTANCE_FILE = "./distance.dat";
const char *CALIBRATE_MATRIX_FILE = "./matrixe.dat";
static const char CHAR_DELIM = ' ';
//const float SANDBOX_CONTOUR = 2.0f * (1020.0f + 770.0f);
//const float MARGE = 0.05 * SANDBOX_CONTOUR; // 5% of marge
static const int ESCAPE_CHAR = 27;
char *defaultWindowsName = (char*) "Image";
cv::Rect rectSandbox;
Calibrate calibrate;
Camera camera;
Beamer beamer;
void createWindowsFullScreen(char *windowName);
void showImage(cv::Mat* image, char *windowName);
bool readSandboxPosition(std::vector<cv::Point> &rectPoints);
void writeSandboxPosition(std::vector<cv::Point> rectPoints);
double toDegrees(double radians);
void sanboxBorder();
void sandboxBorderLoad();
bool configFilesFound();
};
#endif
\ No newline at end of file
include ../common.mk
SRCS_C=$(wildcard *.c)
OBJS_C=$(SRCS_C:%.c=%.o)
SRCS_CPP=$(wildcard *.cpp)
OBJS_CPP=$(SRCS_CPP:%.cpp=%.o)
build: $(OBJS_C) $(OBJS_CPP)
%.o: %.cpp
g++ $(CFLAGS) $(OPENCVFLAG) -c $< -o $@
%.o: %.c
gcc -Wall -Wextra -std=gnu11 -c $< -o $@
clean:
-rm -f *.o
#include <algorithm>
#include "borderedit.h"
using namespace std;
using namespace cv;
Mat BorderEdit::frameImage;
void BorderEdit::drawSquare(cv::Point *p, int n)
{
Mat imageCopy = frameImage.clone();
polylines(imageCopy, &p, &n, 1, true, Scalar(0, 255, 0), 1, LINE_AA);
imshow(wndname, imageCopy);
}
int BorderEdit::findPoints(int x, int y, vector<Point> &posSandbox)
{
for (int i = 0; i < (int)posSandbox.size(); i++)
{
//check if the point is clicked
if ((x >= (posSandbox[i].x - margeClick)) && (x <= (posSandbox[i].x + margeClick)) && (y >= (posSandbox[i].y - margeClick)) && (y <= (posSandbox[i].y + margeClick)))
return i;
}
return -1; //not found
}
void BorderEdit::mouseHandler(int event, int x, int y, int, void *param)
{
static int selectedPoint = -1;
vector<Point> &posSandbox = *((vector<Point> *)param);
switch (event)
{
case CV_EVENT_LBUTTONDOWN: //left button press
selectedPoint = BorderEdit::findPoints(x, y, posSandbox);
break;
case CV_EVENT_LBUTTONUP: //left mouse button release
if (selectedPoint != -1)
selectedPoint = -1;
break;
case CV_EVENT_MOUSEMOVE:
/* draw a rectangle*/
if (selectedPoint != -1)
{
posSandbox[selectedPoint].x = x;
posSandbox[selectedPoint].y = y;
BorderEdit::drawSquare(&posSandbox[0], (int)posSandbox.size());
Rect r = boundingRect(posSandbox);
double f = (double)r.width / (double)r.height;
cout << 4.0 / 3.0 << " == " << f << endl;
}
break;
}
}
void BorderEdit::edit(Mat frame, vector<Point> *posSandbox)
{
namedWindow(wndname, CV_WINDOW_AUTOSIZE);
frame.copyTo(frameImage);
setMouseCallback(wndname, BorderEdit::mouseHandler, (void *)posSandbox);
BorderEdit::drawSquare(&posSandbox->at(0), (int)posSandbox->size());
waitKey(0);
}
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment