void FSWorld::drawPoint()
{
list<FSObject*>::iterator p = objects.begin();
FSCamera* camera = (FSCamera*)(*p);
p++;
FSLaser* laser = (FSLaser*)(*p);
FSPoint camPos = camera->getPosition();
FSPoint laserPos = laser->getPosition();
FSPoint laserFrame = laser->getLaserPointPosition();
FSPoint camFrame = FSMakePoint(5,camPos.y,0);
glColor3f(1.0f, 0, 0);
drawFSPoint(camPos);
glColor3f(0, 1, 0);
drawFSPoint(laserPos);
glColor3f(1, 1, 0);
drawFSPoint(camFrame);
glColor3f(1, 1, 1);
drawFSPoint(laserFrame);
glBegin(GL_LINES);
glVertex3f(camPos.x, camPos.y, camPos.z);
glVertex3f(camFrame.x, camFrame.y, camFrame.z);
glVertex3f(laserFrame.x, laserFrame.y, laserFrame.z);
glVertex3f(laserPos.x, laserPos.y, laserPos.z);
glEnd();
//Line between camera and the projected laser point on the object the camera sees
FSLine2 l1 = computeLineFromPoints(camPos, camFrame);
//Line between the laser and the laser point if it would not have been projected on the object
FSLine2 l2 = computeLineFromPoints(laserPos, laserFrame);
FSPoint i = computeIntersectionOfLines(l1, l2);
i.y = camPos.y; //put it on the same plane as the other points
drawFSPoint(i);
}
void ScanProc::MapLaserPointToGlobalPoint(Mat &laserLine, Mat &laserOff)
{
configuration* config = Edit3DScanPlugin::getConfiguration();
Turntable* turntable = Edit3DScanPlugin::getTurntable();
//calculate position of laser on the back plane in cv frame
GlobalPoint gLaserLinePosition = config->getLaserPositionOnBackPlane();
CvPoint cvLaserLinePosition = convertGlobalPointToCvPoint(gLaserLinePosition);
int laserPos = cvLaserLinePosition.x; //const over all y
unsigned int cols = laserLine.cols;
unsigned int rows = laserLine.rows;
int dpiVertical = 1; // 1 for the best resolution
for(int y = config->UPPER_ANALYZING_FRAME_LIMIT;
y < laserLine.rows - (config->LOWER_ANALYZING_FRAME_LIMIT);
y += dpiVertical )
{
for(int x = laserLine.cols - 1;
x >= laserPos + config->ANALYZING_LASER_OFFSET;
x -= 1)
{
//qDebug() << "Pixel value: " << laserLine.at<uchar>(y,x);
if(laserLine.at<uchar>(y,x) == 255)
{
//qDebug() << "found point at x=" << x;
CvPoint cvNewPoint;
cvNewPoint.x = x;
cvNewPoint.y = y;
//convert to world coordinates withouth depth
GlobalPoint fsNewPoint = convertCvPointToGlobalPoint(cvNewPoint);
//cout << fsNewPoint.x << ":" << fsNewPoint.y << ":" << fsNewPoint.z << endl;
GlobalLine l1 = computeLineFromPoints(config->getCameraPosition(), fsNewPoint);
GlobalLine l2 = computeLineFromPoints(config->getLaserPosition(), config->getLaserPositionOnBackPlane());
GlobalPoint i = computeIntersectionOfLines(l1, l2);
fsNewPoint.x = i.x;
fsNewPoint.z = i.z;
//At this point we know the depth=z. Now we need to consider the scaling depending on the depth.
//First we move our point to a camera centered cartesion system.
fsNewPoint.y -= (config->getCameraPosition()).y;
fsNewPoint.y *= ((config->getCameraPosition()).z - fsNewPoint.z)/(config->getCameraPosition()).z;
//Redo the translation to the box centered cartesion system.
fsNewPoint.y += (config->getCameraPosition()).y;
//get color from picture without laser
unsigned char r = laserOff.at<cv::Vec3b>(y,x)[2];
unsigned char g = laserOff.at<cv::Vec3b>(y,x)[1];
unsigned char b = laserOff.at<cv::Vec3b>(y,x)[0];
fsNewPoint.color = MakeRGBColor(r, g, b);
//turning new point according to current angle of turntable
//translate coordinate system to the middle of the turntable
fsNewPoint.z -= config->TURNTABLE_POS_Z; //7cm radius of turntbale plus 5mm offset from back plane
GlobalPoint alphaDelta = turntable->getRotation();
double alphaOld = (float)atan(fsNewPoint.z / fsNewPoint.x);
double alphaNew = alphaOld + alphaDelta.y * (M_PI / 180.0f);
double hypotenuse = (float)sqrt(fsNewPoint.x * fsNewPoint.x + fsNewPoint.z * fsNewPoint.z);
if(fsNewPoint.z < 0 && fsNewPoint.x < 0)
{
alphaNew += M_PI;
}
else if(fsNewPoint.z > 0 && fsNewPoint.x < 0)
{
alphaNew -= M_PI;
}
fsNewPoint.z = (float)sin(alphaNew) * hypotenuse;
fsNewPoint.x = (float)cos(alphaNew) * hypotenuse;
if(fsNewPoint.y > 0.5 && hypotenuse < 7)
{
static int c = 0;
vcg::tri::Allocator<CMeshO>::AddVertices(mesh->cm, 1);
mesh->cm.vert[c].P() = vcg::Point3f(fsNewPoint.x, fsNewPoint.y, fsNewPoint.z);
mesh->cm.vert[c].C() = vcg::Color4b(255, 0, 0, 255);
c++;
mesh->meshModified() = true;
gla->update();
//eliminate points from the grounds, that are not part of the model
//qDebug("adding point");
//model->addPointToPointCloud(fsNewPoint);
}
break;
}
}
}
}
void FSVision::putPointsFromFrameToCloud(
cv::Mat &laserOff,
cv::Mat &laserOn,
int dpiVertical, //step between vertical points
double lowerLimit, int JD) //remove points below this limit
{
//qDebug() << "putPointsFromFrameToCloud";
//the following lines are just to make to code more readable
FSModel* model = FSController::getInstance()->model;
FSLaser* laser = FSController::getInstance()->laser;
FSTurntable* turntable = FSController::getInstance()->turntable;
FSWebCam* webcam = FSController::getInstance()->webcam;
//extract laser line from the two images
//cv::Mat laserLine = subLaser(laserOff,laserOn,threshold);
cv::Mat laserLine = subLaser2(laserOff,laserOn,JD);
// cv::imshow("extractedlaserLine2",laserLine);
// cv::waitKey(300) ;
//calculate position of laser in cv frame
//position of the laser line on the back plane in frame/image coordinates
FSPoint fsLaserLinePosition = laser->getLaserPointPosition();
//position of the laser line on the back plane in world coordinates
CvPoint cvLaserLinePosition = convertFSPointToCvPoint(fsLaserLinePosition);
double laserPos = cvLaserLinePosition.x; //const over all y
//laserLine is result of subLaser2, is in RGB
unsigned int cols = laserLine.cols;
unsigned int rows = laserLine.rows;
//create new image in black&white
cv::Mat bwImage( cols,rows,CV_8U,cv::Scalar(100) );
//qDebug("still works here");
//convert from rgb to b&w
cv::cvtColor(laserLine, bwImage, CV_RGB2GRAY); //convert to grayscale
cv::imshow("extractedlaserLine2",bwImage);
cv::waitKey(1) ;
//now iterating from top to bottom over bwLaserLine frame
//no bear outside of these limits :) cutting of top and bottom of frame
for(int y = UPPER_ANALYZING_FRAME_LIMIT;
y < bwImage.rows-(LOWER_ANALYZING_FRAME_LIMIT);
y+=dpiVertical )
{
//qDebug() << "checking point at line " << y << laserPos+ANALYZING_LASER_OFFSET;
//ANALYZING_LASER_OFFSET is the offset where we stop looking for a reflected laser, cos we might catch the non reflected
//now iteratinf from right to left over bwLaserLine frame
for(int x = bwImage.cols-1;
x >= laserPos+ANALYZING_LASER_OFFSET;
x -= 1) {
//qDebug() << "Pixel value: " << bwImage.at<uchar>(y,x);
if(bwImage.at<uchar>(y,x)==255) { //check if white=laser-reflection
//qDebug() << "found point at x=" << x;
//if (row[x] > 200){
//we have a white point in the grayscale image, so one edge laser line found
//no we should continue to look for the other edge and then take the middle of those two points
//to take the width of the laser line into account
//position of the reflected laser line on the image coord
CvPoint cvNewPoint;
cvNewPoint.x = x;
cvNewPoint.y = y;
//convert to world coordinates withouth depth
FSPoint fsNewPoint = FSVision::convertCvPointToFSPoint(cvNewPoint);
FSLine l1 = computeLineFromPoints(webcam->getPosition(), fsNewPoint);
FSLine l2 = computeLineFromPoints(laser->getPosition(), laser->getLaserPointPosition());
FSPoint i = computeIntersectionOfLines(l1, l2);
fsNewPoint.x = i.x;
fsNewPoint.z = i.z;
//At this point we know the depth=z. Now we need to consider the scaling depending on the depth.
//First we move our point to a camera centered cartesion system.
fsNewPoint.y -= (webcam->getPosition()).y;
fsNewPoint.y *= ((webcam->getPosition()).z - fsNewPoint.z)/(webcam->getPosition()).z;
//Redo the translation to the box centered cartesion system.
fsNewPoint.y += (webcam->getPosition()).y;
//get color from picture without laser
FSUChar r = laserOff.at<cv::Vec3b>(y,x)[2];
FSUChar g = laserOff.at<cv::Vec3b>(y,x)[1];
FSUChar b = laserOff.at<cv::Vec3b>(y,x)[0];
fsNewPoint.color = FSMakeColor(r, g, b);
//turning new point according to current angle of turntable
//translate coordinate system to the middle of the turntable
fsNewPoint.z -= TURNTABLE_POS_Z; //7cm radius of turntbale plus 5mm offset from back plane
FSPoint alphaDelta = turntable->getRotation();
double alphaOld = (float)atan(fsNewPoint.z/fsNewPoint.x);
double alphaNew = alphaOld+alphaDelta.y*(M_PI/180.0f);
double hypotenuse = (float)sqrt(fsNewPoint.x*fsNewPoint.x + fsNewPoint.z*fsNewPoint.z);
if(fsNewPoint.z < 0 && fsNewPoint.x < 0) {
alphaNew += M_PI;
} else if(fsNewPoint.z > 0 && fsNewPoint.x < 0) {
alphaNew -= M_PI;
}
fsNewPoint.z = (float)sin(alphaNew)*hypotenuse;
fsNewPoint.x = (float)cos(alphaNew)*hypotenuse;
if(fsNewPoint.y>lowerLimit+0.5 && hypotenuse < 7) { //eliminate points from the grounds, that are not part of the model
//qDebug("adding point");
model->addPointToPointCloud(fsNewPoint);
}
break;
}
}
}
}
