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

adapt SandboxSetup to custom the output of the beamer location routine + update qt app

parent 15b15b80
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
src/components/beamer.cpp
+ 98
− 70
View file @ 9f6302fd
......@@ -13,84 +13,112 @@ Beamer::Beamer(){
*/
int Beamer::findBeamerFrom(Camera camera, ShowImageFn imshow)
{
cv::Mat depth;
cv::Mat rgb;
cv::Mat frameImage(resolution, CV_8UC3, cv::Scalar(0, 0, 0));
int Beamer::findBeamerFrom(Camera *camera){
unsigned int nbPoint = 3; //number of point to calculate 1 vector
char wname[] = "beamer location";
cv::namedWindow(wname, CV_WINDOW_NORMAL);
cv::setWindowProperty(wname, CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
// Setup cross for calibration
std::vector<cv::Point2i> crosses = getCrossList();
std::vector<cv::Point3d> directions; //vectors calculate for each point
std::vector<cv::Point3d> bases; //1 point for each vector (to calculate constante d)
double fact = -20.0;
// Setup cross for calibration
std::vector<cv::Point> points;
points.push_back( cv::Point( getWidth() * 5/14, getHeight() * 5/14));
points.push_back( cv::Point( getWidth() * 5/7 , getHeight() * 3/14));
points.push_back( cv::Point( getWidth() * 3/14, getHeight() * 4/7 ));
for (cv::Point2i cross : crosses){
for (int i = 0; i < (int)points.size(); i++)
{
std::vector<cv::Point3f> capturedPoints;
cv::Point projCross = points[i];
cv::Mat depth;
cv::Mat rgb;
while (capturedPoints.size() < nbPoint){
while( capturedPoints.size() < MAX_LINEAR_LINE_POINTS ){
// capture frame
camera.captureFrame();
depth = camera.getDepthFrame();
rgb = camera.getRGBFrame();
camera->captureFrame();
depth = camera->getDepthFrame();
rgb = camera->getRGBFrame();
// Look for the circle target
std::vector<int> crc = findCercleZ(rgb, profil.getContrast(), profil.getBrightness(), profil.getRadiusRatio(), profil.getUpperMinThreshold(), profil.getLowerMinThreshold());
cv::Scalar color = (!crc.empty()) ? cv::Scalar(0, 255, 0) : cv::Scalar(0, 0, 255);
std::vector<cv::Point3i> circle = findCercleZ(rgb, profil.getContrast(), profil.getBrightness(), profil.getRadiusRatio(), profil.getUpperMinThreshold(), profil.getLowerMinThreshold());
// Show black screen with cross
cv::line(frameImage, cv::Point(projCross.x, 0), cv::Point(projCross.x, frameImage.rows - 1), color, 4);
cv::line(frameImage, cv::Point(0, projCross.y), cv::Point(frameImage.cols - 1, projCross.y), color, 4);
cv::putText( frameImage,
std::to_string(capturedPoints.size() + 1) + "/" + std::to_string(nbPoint),
cv::Point( getWidth()/2, getHeight()/2 ),
cv::FONT_HERSHEY_SIMPLEX,
1,
cv::Scalar(255, 255, 255));
imshow(frameImage);
cv::Mat frameImage = getCrossFrame(cross, capturedPoints.size()+1, MAX_LINEAR_LINE_POINTS, !circle.empty());
cv::imshow(wname, frameImage);
// Wait for interaction
char keyCode = cv::waitKey(500);
int keypressed = cv::waitKey(500);
if(keypressed != -1){
char keyCode = (char)keypressed;
if (keyCode == ESCAPE_CHAR){
return 1;
}
else if (keyCode == ' ' && !crc.empty()){
// Point validate
float coord[2] = {(float)crc[0], (float)crc[1]};
float z = static_cast<float>(depth.at<uint16_t>(crc[1], crc[0]));
cv::Point3f p = camera.deprojectPixelToPoint(coord, z / 1000.0);
capturedPoints.push_back(p);
else if (keyCode == ' '){
if(!circle.empty()){
capturedPoints.push_back( deprojectPixel(circle.at(0), &depth, camera) );
}
}
}
}
findLinearLineFrom(&capturedPoints, &bases, &directions);
}
// clear frame
frameImage.setTo(cv::Scalar(0, 0, 0));
setPosition( (cv::Point3f)approximatePosition(&bases, &directions) );
cv::destroyAllWindows();
return 0;
}
cv::Point3f Beamer::deprojectPixel(cv::Point3i circle, cv::Mat *depth, Camera *camera){
float coord[2] = {(float)circle.x, (float)circle.y};
float z = static_cast<float>(depth->at<uint16_t>(circle.y, circle.x));
return camera->deprojectPixelToPoint(coord, z / 1000.0);
}
void Beamer::findLinearLineFrom(std::vector<cv::Point3f> *capturedPoints, std::vector<cv::Point3d> *bases, std::vector<cv::Point3d> *directions){
cv::Vec6f line;
cv::fitLine(capturedPoints, line, CV_DIST_L2, 0, 0.01, 0.01);
directions.push_back(cv::Point3d(line[3] * fact, line[4] * fact, line[5] * fact));
bases.push_back(cv::Point3d(line[0], line[1], line[2]));
cv::fitLine(*capturedPoints, line, CV_DIST_L2, 0, 0.01, 0.01);
cv::Point3d base = cv::Point3d(line[0], line[1], line[2]);
cv::Point3d direction = cv::Point3d( LINEAR_REGRESSION_FACT * line[3],
LINEAR_REGRESSION_FACT * line[4],
LINEAR_REGRESSION_FACT * line[5] );
bases->push_back(base);
directions->push_back(direction);
}
cv::Point3d beamerPoint = approximatePosition(directions, bases);
std::vector<cv::Point2i> Beamer::getCrossList(){
//set beamer position
beamerPosition.x = (float)beamerPoint.x;
beamerPosition.y = (float)beamerPoint.y;
beamerPosition.z = (float)beamerPoint.z;
std::vector<cv::Point2i> points;
return 0;
points.push_back( cv::Point( getWidth() * 5/14, getHeight() * 5/14));
points.push_back( cv::Point( getWidth() * 5/7 , getHeight() * 3/14));
points.push_back( cv::Point( getWidth() * 3/14, getHeight() * 4/7 ));
return points;
}
cv::Mat Beamer::getCrossFrame(cv::Point projectedCross, int step, int max, bool circlesFound){
cv::Mat frameImage(resolution, CV_8UC3, cv::Scalar(0, 0, 0));
cv::Scalar color = (circlesFound) ? cv::Scalar(0, 255, 0) : cv::Scalar(0, 0, 255);
cv::line(frameImage, cv::Point(projectedCross.x, 0), cv::Point(projectedCross.x, frameImage.rows - 1), color, 4);
cv::line(frameImage, cv::Point(0, projectedCross.y), cv::Point(frameImage.cols - 1, projectedCross.y), color, 4);
cv::putText( frameImage,
std::to_string(step) + "/" + std::to_string(max),
cv::Point( getWidth()/2, getHeight()/2 ),
cv::FONT_HERSHEY_SIMPLEX,
1,
cv::Scalar(255, 255, 255));
return frameImage;
}
void Beamer::printPosition(){
cv::Point3f pos = getPosition();
std::cout << "(" << pos.x << "," << pos.y << "," << pos.z << ")" << std::endl;
......@@ -181,7 +209,7 @@ cv::Mat Beamer::editContrast(cv::Mat image, double contrast, int brightness){
return new_image;
}
std::vector<int> Beamer::findCercleZ(cv::Mat &rgb, double contrast, int brightness, int ratioRadius, int upperMinThreshold, int lowerMinThreshold)
std::vector<cv::Point3i> Beamer::findCercleZ(cv::Mat &rgb, double contrast, int brightness, int ratioRadius, int upperMinThreshold, int lowerMinThreshold)
{
cv::Mat src_gray;
cv::cvtColor(rgb, src_gray, CV_BGR2GRAY);
......@@ -199,33 +227,33 @@ std::vector<int> Beamer::findCercleZ(cv::Mat &rgb, double contrast, int brightne
cv::HoughCircles(src_gray, circles, CV_HOUGH_GRADIENT, 1, minRadius, (double)upperMinThreshold, (double)lowerMinThreshold, 0, 0);
//doit tester si le cercle est bon (rayon);
std::vector<int> result;
std::vector<cv::Point3i> result;
if (!circles.empty())
{
for (int i = 0; i < 3; i++)
{
result.push_back(round(circles[0][i]));
}
cv::Vec3f first = circles[0];
result.push_back( cv::Point3i(round(first[0]), round(first[1]), round(first[2])) );
}
return result;
}
cv::Point3d Beamer::approximatePosition(std::vector<cv::Point3d> directions, std::vector<cv::Point3d> bases){
cv::Point3d Beamer::approximatePosition(std::vector<cv::Point3d> *bases, std::vector<cv::Point3d> *directions){
cv::Point3d pa, pb;
double mua;
double mub;
std::vector<cv::Point3d> beamerPoints;
LineLineIntersect(directions[0], bases[0], directions[1], bases[1], &pa, &pb, &mua, &mub);
beamerPoints.push_back(pa);
beamerPoints.push_back(pb);
LineLineIntersect(directions[0], bases[0], directions[2], bases[2], &pa, &pb, &mua, &mub);
beamerPoints.push_back(pa);
beamerPoints.push_back(pb);
LineLineIntersect(directions[1], bases[1], directions[2], bases[2], &pa, &pb, &mua, &mub);
int selected[3][2] = { {0,1}, {0,2}, {1,2} };
for(int i=0; i < 3; i++){
LineLineIntersect( directions->at(selected[i][0]),
bases->at(selected[i][0]),
directions->at(selected[i][1]),
bases->at(selected[i][1]),
&pa, &pb, &mua, &mub);
beamerPoints.push_back(pa);
beamerPoints.push_back(pb);
}
cv::Point3d beamerPoint(0.0, 0.0, 0.0);
for (unsigned int i = 0; i < beamerPoints.size(); i++)
......
src/lib/sandboxSetup.cpp
+ 32
− 22
View file @ 9f6302fd
......@@ -5,6 +5,10 @@ SandboxSetup::SandboxSetup(){
}
//
// PUBLIC
//
// return 1 when config can't be saved in file
int SandboxSetup::saveConfigFrom(char *path){
......@@ -36,12 +40,31 @@ int SandboxSetup::loadFrameProcessProfil(){
}
void SandboxSetup::setupAdjustMatrix(std::vector<cv::Point> rectPoints, cv::Point center){
// Set adjusting matrix for the projection
int widthTop = rectPoints[3].x - rectPoints[0].x;
double angle1 = atan((double)(rectPoints[3].y - rectPoints[0].y) / widthTop);
cv::Mat matRotation = cv::getRotationMatrix2D(center, toDegrees(angle1), 1); // adjustingMatrix
projection.setAdjustingMatrix(matRotation);
}
void SandboxSetup::setupCroppingMask(std::vector<cv::Point> rectPoints){
// Set cropping mask
int widthTop = rectPoints[3].x - rectPoints[0].x;
cv::Size rectSize = cv::Size(widthTop, cvRound(widthTop / 1.33333) + 5);
cv::Point p = projection.rotatePixel(rectPoints[0]);
camera.setCroppingMask(cv::Rect(p, rectSize)); // croppingMask
}
//
// Default setup routines
//
int SandboxSetup::setupBeamerResolution(){
int SandboxSetup::defaultSetupBeamerResolution(){
int width = 0;
int height = 0;
......@@ -64,16 +87,16 @@ int SandboxSetup::setupBeamerResolution(){
// return 1 when user exits process
int SandboxSetup::setupBeamerLocation(void (*showImage)(cv::Mat)){
int SandboxSetup::defaultSetupBeamerLocation(){
return beamer.findBeamerFrom(camera, showImage);
return beamer.findBeamerFrom(&camera);
}
// return 1 when user exits process
int SandboxSetup::setupProjection(){
int SandboxSetup::defaultSetupProjection(){
captureBlueScreen(300);
defaultCaptureBlueScreen(300);
cv::Mat frameData = camera.getDepthFrame();
cv::Mat coloredFrame = camera.getRGBFrame();
......@@ -99,7 +122,7 @@ int SandboxSetup::setupProjection(){
}
void SandboxSetup::captureBlueScreen(int delay){
void SandboxSetup::defaultCaptureBlueScreen(int delay){
// Blue screen
char windowName[] = "BlueScreen";
......@@ -115,23 +138,10 @@ void SandboxSetup::captureBlueScreen(int delay){
}
void SandboxSetup::setupAdjustMatrix(std::vector<cv::Point> rectPoints, cv::Point center){
// Set adjusting matrix for the projection
int widthTop = rectPoints[3].x - rectPoints[0].x;
double angle1 = atan((double)(rectPoints[3].y - rectPoints[0].y) / widthTop);
cv::Mat matRotation = cv::getRotationMatrix2D(center, toDegrees(angle1), 1); // adjustingMatrix
projection.setAdjustingMatrix(matRotation);
}
void SandboxSetup::setupCroppingMask(std::vector<cv::Point> rectPoints){
// Set cropping mask
int widthTop = rectPoints[3].x - rectPoints[0].x;
cv::Size rectSize = cv::Size(widthTop, cvRound(widthTop / 1.33333) + 5);
cv::Point p = projection.rotatePixel(rectPoints[0]);
camera.setCroppingMask(cv::Rect(p, rectSize)); // croppingMask
}
//
// PRIVATE
//
double SandboxSetup::toDegrees(double radians){
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment