void HeightmapSampling::initialize(const sensor_msgs::PointCloud& cluster,
const geometry_msgs::Pose& table)
{
table_pose_ = table;
boost::shared_ptr < PointCloud<PointXYZ> > tmp;
tmp.reset(new PointCloud<PointXYZ> ());
tmp->header.frame_id = cluster.header.frame_id;
tmp->points.clear();
for (vector<geometry_msgs::Point32>::const_iterator it = cluster.points.begin();
it != cluster.points.end(); ++it)
{
tmp->push_back(*(new PointXYZ(it->x, it->y, it->z)));
}
point_cloud_ = tmp;
img_ss_.initialize(cluster, viewp_trans_, viewp_rot_);
calculateConvexHull();
calculateNormalsFromHullSurface();
}
bool EyeCalibration::calibrate() {
char buf[512];
// Print starting message
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("Starting Calibration\n");
// Create buffer
if (this->eyeVectorArray == NULL)
this->eyeVectorArray = (float*) malloc(sizeof(float) * ClientHandler::getDikablisViewingSize());
sprintf_s(buf, "Creating buffer of size %d bytes\n", sizeof(float) * ClientHandler::getDikablisViewingSize());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
// Print the number of points used in the calibration
sprintf_s(buf, "A total of %d points was collected.\n", calibrationPoints.size());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
if (calibrationPoints.size() < 4) {
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("Not enough points to calculate calibration.\n");
return false;
}
// Min/Max coordinates of the polygon
CalibrationPoint
minX = calibrationPoints.at(0),
minY = calibrationPoints.at(0),
maxX = calibrationPoints.at(0),
maxY = calibrationPoints.at(0);
// Print each points information and get the min and max
for (unsigned int i = 0; i < calibrationPoints.size(); i++) {
CalibrationPoint point = calibrationPoints.at(i);
osg::Vec3 ray = point.ray();
// Print info
sprintf_s(buf, "Point %d (%d, %d) has a value of (%f, %f, %f)\n",
i + 1,
point.x(), point.y(),
ray.x(), ray.y(), ray.z());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
if (minX.x() > point.x())
minX = point;
if (minY.y() > point.y())
minY = point;
if (maxX.x() < point.x())
maxX = point;
if (maxY.y() < point.y())
maxY = point;
}
// Print Bounding Info
{
sprintf_s(buf, "Point with min X value (%d, %d)\n",
minX.x(), minX.y());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
sprintf_s(buf, "Point with min Y value (%d, %d)\n",
minY.x(), minY.y());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
sprintf_s(buf, "Point with max X value (%d, %d)\n",
maxX.x(), maxX.y());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
sprintf_s(buf, "Point with max Y value (%d, %d)\n",
maxY.x(), maxY.y());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
// Get center
this->center_x = (maxX.x() - minX.x())/2;
this->center_y = (maxY.y() - minY.y())/2;
sprintf_s(buf, "Center point is (%d, %d)\n",
center_x, center_y);
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
}
// Get Convex Hull
CalibrationPointVector boundingPoints;
SegmentVector convexHull = calculateConvexHull(calibrationPoints);
// Get the bounding points of the convex hull
boundingPointsOfHull(convexHull, boundingPoints);
// Print each segment information
for (unsigned int i = 0; i < convexHull.size(); i++) {
Segment segment = convexHull.at(i);
sprintf_s(buf, "Edge %d: From (%d, %d) to (%d, %d)\n",
i + 1,
segment.x1(), segment.y1(),
segment.x2(), segment.y2());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
}
// Resort the bounding points
sort(boundingPoints, center_x, center_y);
// Triangulate
CalibrationPointVector traingles;
triangulate(boundingPoints, traingles);
// Check if the triangulation is valid
if (traingles.size() % 3 != 0) {
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("Error: Triangulating polygon.\n");
return false;
}
// Get the remaining inner points of the convex hull
CalibrationPointVector innerPoints;
for (unsigned int i = 0; i < calibrationPoints.size(); i++) {
CalibrationPoint point = calibrationPoints[i];
bool found = false;
for (unsigned int k = 0; k < boundingPoints.size(); k++) {
if (point.equal(boundingPoints[k])) {
found = true;
break;
}
}
if (!found)
innerPoints.push_back(point);
}
unsigned int traingleCount = traingles.size() / 3;
for (unsigned int i = 0; i < innerPoints.size(); i++) {
CalibrationPoint P = innerPoints[i];
for (unsigned int k = 0; k < traingleCount; k++) {
CalibrationPoint A = traingles[3*k + 0];
CalibrationPoint B = traingles[3*k + 1];
CalibrationPoint C = traingles[3*k + 2];
if (insideTriangle(A,B,C,P)) {
traingles.erase(traingles.begin()+3*k, traingles.begin()+3*k+3);
CalibrationPoint center, P0;
// Triangle 1
CalibrationPointVector tri1;
tri1.push_back(A);
tri1.push_back(B);
tri1.push_back(P);
P0 = A + (B - A)/2.f;
center = P + (P0 - P)*(3.f/2.f);
sort(tri1, center.x(), center.y());
// Triangle 2
CalibrationPointVector tri2;
tri2.push_back(A);
tri2.push_back(C);
tri2.push_back(P);
P0 = A + (C - A)/2.f;
center = P + (P0 - P)*(3.f/2.f);
sort(tri2, center.x(), center.y());
// Triangle 3
CalibrationPointVector tri3;
tri3.push_back(B);
tri3.push_back(C);
tri3.push_back(P);
P0 = B + (C - B)/2.f;
center = P + (P0 - P)*(3.f/2.f);
sort(tri3, center.x(), center.y());
traingles.insert(traingles.end(), tri1.begin(), tri1.end());
traingles.insert(traingles.end(), tri2.begin(), tri2.end());
traingles.insert(traingles.end(), tri3.begin(), tri3.end());
break;
}
}
}
if (traingles.size() % 3 != 0) {
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("Error: Triangulating inner points in polygon.\n");
return false;
}
traingleCount = traingles.size() / 3;
// Flip edges of the triangles
for (int k = traingleCount - 1; k > 0 ; k--) {
CalibrationPointVector tri1;
tri1.push_back(traingles[3*k + 0]);
tri1.push_back(traingles[3*k + 1]);
tri1.push_back(traingles[3*k + 2]);
for (int i = k - 1; i >= 0; i--) {
CalibrationPointVector tri2;
tri2.push_back(traingles[3*i + 0]);
tri2.push_back(traingles[3*i + 1]);
tri2.push_back(traingles[3*i + 2]);
// Get shared edge if exists
int shared = 0;
CalibrationPointVector sharedPoints;
CalibrationPoint nonSharedPoint;
for (unsigned int idx1 = 0; idx1 < 3; idx1++) {
for (unsigned int idx2 = 0; idx2 < 3; idx2++) {
if (tri2[idx2].equal(tri1[idx1])) {
sharedPoints.push_back(tri1[idx1]);
shared++;
} else {
nonSharedPoint = tri2[idx2];
}
}
}
if (shared == 2) {
if (isInCircumCircle(nonSharedPoint, tri1)) {
sprintf_s(buf, "Point %d - (%d, %d): In circle of triangle (%d, %d), (%d, %d), (%d, %d)\n",
i + 1, nonSharedPoint.x(), nonSharedPoint.y(),
tri1[0].x(), tri1[0].y(),
tri1[1].x(), tri1[1].y(),
tri1[2].x(), tri1[2].y());
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog(buf);
// Sort tri1
for (unsigned int idx = 0; idx < 3; idx++) {
if (tri1[idx].equal(sharedPoints[0]) || tri1[idx].equal(sharedPoints[1])) {
CalibrationPoint shared = tri1[idx];
tri1.erase(tri1.begin()+idx);
tri1.insert(tri1.begin(), shared);
}
}
// Sort tri2
for (unsigned int idx = 0; idx < 3; idx++) {
if (tri2[idx].equal(sharedPoints[0]) || tri2[idx].equal(sharedPoints[1])) {
CalibrationPoint shared = tri2[idx];
tri2.erase(tri2.begin()+idx);
tri2.insert(tri2.begin(), shared);
}
}
// Clean up old triangles
traingles.erase(traingles.begin()+3*k, traingles.begin()+3*k+3);
traingles.erase(traingles.begin()+3*i, traingles.begin()+3*i+3);
CalibrationPoint center, P0;
// New Triangle 1
CalibrationPointVector new_tri1;
new_tri1.push_back(tri1[0]);
new_tri1.push_back(tri1[2]);
new_tri1.push_back(tri2[2]);
P0 = tri1[0] + (tri2[2] - tri1[0])/2.f;
center = tri1[2] + (P0 - tri1[2])*(3.f/2.f);
sort(new_tri1, center.x(), center.y());
// New Triangle 2
CalibrationPointVector new_tri2;
new_tri2.push_back(tri1[1]);
new_tri2.push_back(tri1[2]);
new_tri2.push_back(tri2[2]);
P0 = tri1[1] + (tri2[2] - tri1[1])/2.f;
center = tri1[2] + (P0 - tri1[2])*(3.f/2.f);
sort(new_tri2, center.x(), center.y());
traingles.insert(traingles.end(), new_tri1.begin(), new_tri1.end());
traingles.insert(traingles.end(), new_tri2.begin(), new_tri2.end());
break;
}
}
}
}
// Draw triangles
for (unsigned int j = 0; j < (viewingHeight+2*viewingMargin); j++) {
for (unsigned int i = 0; i < (viewingWidth+2*viewingMargin); i++) {
CalibrationPoint P(i - viewingMargin, j - viewingMargin, osg::Vec3(0.f, 0.f, 0.f));
for (unsigned int k = 0; k < traingleCount; k++) {
CalibrationPoint A = traingles[3*k + 0];
CalibrationPoint B = traingles[3*k + 1];
CalibrationPoint C = traingles[3*k + 2];
if (insideTriangle(A,B,C,P)) {
osg::Vec3 ray;
float det = A.x()*B.y()-B.x()*A.y()+B.x()*C.y()-C.x()*B.y()+C.x()*A.y()-A.x()*C.y();
osg::Vec3 a = (A.ray()*(B.y()-C.y())+B.ray()*(C.y()-A.y())+C.ray()*(A.y()-B.y())) / det;
osg::Vec3 b = (A.ray()*(C.x()-B.x())+B.ray()*(A.x()-C.x())+C.ray()*(B.x()-A.x())) / det;
osg::Vec3 c = (A.ray()*(B.x()*C.y()-C.x()*B.y())+B.ray()*(C.x()*A.y()-A.x()*C.y())+C.ray()*(A.x()*B.y()-B.x()*A.y())) / det;
ray = a*P.x()+b*P.y()+c;
unsigned long location = 3*(j*(viewingWidth+2*viewingMargin) + i);
this->eyeVectorArray[location + 0] = ray.x();
this->eyeVectorArray[location + 1] = ray.y();
this->eyeVectorArray[location + 2] = ray.z();
break;
}
}
}
}
// Draw Outside
for (unsigned int j = 0; j < (viewingHeight+2*viewingMargin); j++) {
for (unsigned int i = 0; i < (viewingWidth+2*viewingMargin); i++) {
CalibrationPoint P(i - viewingMargin, j - viewingMargin, osg::Vec3(0.f, 0.f, 0.f));
bool inside = false;
for (unsigned int k = 0; k < traingleCount; k++) {
CalibrationPoint A = traingles[3*k + 0];
CalibrationPoint B = traingles[3*k + 1];
CalibrationPoint C = traingles[3*k + 2];
if (insideTriangle(A,B,C,P)) {
inside = true;
break;
}
}
if (!inside) {
CalibrationPoint closest;
float min_distance = -1;
for (unsigned int k = 0; k < convexHull.size(); k++) {
CalibrationPoint A = convexHull[k].p1();
CalibrationPoint B = convexHull[k].p2();
CalibrationPoint P0 = getClosestPoint(A, B, P, true);
float diff_x = P0.x() - P.x();
float diff_y = P0.y() - P.y();
float distance = diff_x*diff_x + diff_y*diff_y;
if (min_distance < 0 || min_distance > distance) {
min_distance = distance;
closest = P0;
}
}
osg::Vec3 ray = closest.ray();
unsigned long location = 3*(j*(viewingWidth+2*viewingMargin) + i);
this->eyeVectorArray[location + 0] = ray.x();
this->eyeVectorArray[location + 1] = ray.y();
this->eyeVectorArray[location + 2] = ray.z();
}
}
}
// Testing
#if 0
// Draw points
for (unsigned int c = 0; c < calibrationPoints.size(); c++) {
CalibrationPoint point = calibrationPoints[c];
for (int j = -10; j < 10; j++) {
for (int i = -10; i < 10; i++) {
int x = point.x() + i + viewingMargin;
int y = point.y() + j + viewingMargin;
if (x >= 0 && x < (int)(viewingWidth+2*viewingMargin) &&
y >= 0 && y < (int)(viewingHeight+2*viewingMargin)) {
unsigned long location = 3*(y*(viewingWidth+2*viewingMargin) + x);
this->eyeVectorArray[location + 0] = 255;
this->eyeVectorArray[location + 1] = 0;
this->eyeVectorArray[location + 2] = 0;
}
}
}
}
// Draw bounding points
for (unsigned int c = 0; c < boundingPoints.size(); c++) {
CalibrationPoint point = boundingPoints[c];
for (int j = -10; j < 10; j++) {
for (int i = -10; i < 10; i++) {
int x = point.x() + i + viewingMargin;
int y = point.y() + j + viewingMargin;
if (x >= 0 && x < (int)(viewingWidth+2*viewingMargin) &&
y >= 0 && y < (int)(viewingHeight+2*viewingMargin)) {
unsigned long location = 3*(y*(viewingWidth+2*viewingMargin) + x);
this->eyeVectorArray[location + 0] = 0;
this->eyeVectorArray[location + 1] = 255;
this->eyeVectorArray[location + 2] = 0;
}
}
}
}
#endif
saveRayMap();
ClientHandler* client = AppData::getInstance()->getClient();
if (client) {
client->setRayCalibration(this->eyeVectorArray);
client->setHeadId(this->rbHeadId);
} else {
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("Failed: Unable to find client.\n");
return false;
}
EyeCalibrationWizardFormController::getInstance()->calibrationOutputLog("Done\n");
return true;
}
