void AMBeamConfiguration::alignPositionTwo()
{
QVector3D centerOne = findCenter(positionOne_);
QVector3D centerTwo = findCenter(positionTwo_);
QVector3D ray = centerTwo - centerOne;
QVector<QVector3D> newPosition;
for(int i = 0; i < positionOne_.count(); i++)
{
newPosition<<(positionOne_.at(i)+ray);
}
setPositionTwo(newPosition);
}
bool isIsomorphic(){
vector<int> r2 = findCenter(n);
vector<int> r1 = findCenter(0);
if(r1.size() != r2.size()){
return false;
}else{
if(r1.size() == 1){
return rootedTreeIsomorphic(r1[0], r2[0]);
}else {
return rootedTreeIsomorphic(r1[0], r2[0]) || rootedTreeIsomorphic(r1[0], r2[1]);
}
}
}
CHandPoint CTransformImage::findFinger()
{
findCenter();
if(!m_transImage)
return CHandPoint();
int width = m_transImage->width;
int height = 180;
int moveX = 0, moveY = height;
BOOL bClick = FALSE, bWheel = FALSE;
unsigned char ch;
for(int y = m_center.y; y < height; ++y)
{
for(int x = m_center.x-100; x < m_center.x+50; ++x)
{
if(x < 0 || x >= width || y < 0 || y >= height)
continue;
ch = m_transImage->imageData[y*width+x];
if(ch == 255)
{
moveX = x, moveY = y;
if(x < m_center.x-50)
bClick = TRUE;
break;
}
// CvBox2D box;
// box.center = cvPoint2D32f(x, y);
// box.size = cvSize2D32f(2, 2);
// box.angle = 90;
// cvEllipseBox(m_image, box, CV_RGB(0,255,255), 1);
}
if(moveY != y)
break;
}
// 좌표가 조금씩 흔들리는 것을 방지하기 위한 부분
if(abs(m_pastPt.x-moveX) < 2 || abs(m_pastPt.y-moveY) < 2)
moveX = m_pastPt.x, moveY = m_pastPt.y;
m_pastPt.x = moveX, m_pastPt.y = moveY;
CvBox2D box;
box.center = cvPoint2D32f(moveX, moveY);
box.size = cvSize2D32f(2, 2);
box.angle = 90;
cvEllipseBox(m_image, box, CV_RGB(0,255,0), 1);
return CHandPoint(moveX, height-moveY, bClick, bWheel);
}
// Go home and retrieve disk then move to center
void load_disk(void)
{
if (digitalRead(ACTUATOR_LIMIT) == HIGH) // Check if a disk is already on the actuator
{
delay(50); // debounce
if (digitalRead(ACTUATOR_LIMIT) == HIGH)
{
get_disk(); // No disk detected so we load one
}
}
findCenter();
delay(500);
place_disk();
}
void loop()
{
while (Serial.available()) // If serial data is available on the port
{
inByte = (char)Serial.read(); // Read a byte
if (echo) Serial.print(inByte); // If the scho is on then print it back to the host
if (inByte == '\r') // Once a /r is recvd check to see if it's a good command
{
if (echo) Serial.println(); // Dummy empty line
if (inData == "up") lift_arm();
else if (inData == "downs") down_seek(50, 0); // Lower arm until it finds bottom
else if (inData == "down") down(50, ARM_DN_POSITION); // Blindly lower arm
else if (inData == "pump on") pump(ON); // Start pump
else if (inData == "pump off") pump(OFF); // Stop pump
else if (inData == "vent") pump_release(); // Stop pump and vent the vacuum
else if (inData == "left") baseLeft(16); // Move base to the left
else if (inData == "center") findCenter(); // Move base to the right
else if (inData == "right") baseRight(BASE_CENTER_POSITION);// Move base to the right
else if (inData == "stop") baseStop(); // Stop base
else if (inData == "home") left_home(); // Mode base left until it finds home
else if (inData == "status") stats(); // Show current status
else if (inData == "stats") stats(); // Show current stats
else if (inData == "get disk") get_disk(); // Macro to get disk and lift to top
else if (inData == "load disk") load_disk(); // Macro to get disk and move to center
else if (inData == "unload disk") unload_disk(); // Macro to get disk and move to center
else if (inData == "place disk") place_disk(); // Macro to lower disk into tray
else if (inData == "debug on") debug_mode = true; // turn on debug mode, prints encoder values
else if (inData == "debug off") debug_mode = false; // Turn off debug mode
else if (inData == "echo on") echo = true; // Turn on echo mode
else if (inData == "echo off") echo = false; // Turn off echo mode
else // The string wasn't recognized so it was a bad command
{
Serial.print("Error, Unknown Command: "); // Show the error
Serial.println(inData); // Echo the command
}
inData = ""; // Clear the buffer (string)
if (echo) prompt(); // reprint the prompt
}
else inData += inByte; // If it's not a /r then keep adding to the string (buffer)
}
}
void sampleCavities()
{
int i=0;
double energy;
r_convergence_ratio = 1/c_convergence_ratio;
while (i<number_of_samples)
{
double dx, dy, dz;
double shift_x, shift_y, shift_z;
int valid = 0;
center_x = sample_x = box_x * rnd();
center_y = sample_y = box_y * rnd();
center_z = sample_z = box_z * rnd();
findCenter();
findDiameter();
valid = 1;
if (volume_sampling)
{
valid = 0;
for (shift_x = -box_x; shift_x < box_x; shift_x += box_x)
for (shift_y = -box_y; shift_y < box_y; shift_y += box_y)
for (shift_z = -box_z; shift_z < box_z; shift_z += box_z)
{
dx = shift_x + sample_x - center_x;
dy = shift_y + sample_y - center_y;
dz = shift_z + sample_z - center_z;
if ((dx*dx + dy*dy + dz*dz) < (.25*diameter*diameter)) valid = 1;
}
}
if (valid)
{
printf("%lf\t%lf\t%lf\t%lf\n", center_x, center_y, center_z, diameter);
i++;
}
}
}
void unload_disk()
{
findCenter();
int i;
lower_wrist(wrist_position + 40);
down(50, 60);
delay(1000);
for (i=0; i<10; i++)
{
shoulder.write(arm_position + i);
delay(50);
}
pump(1);
delay(500);
down_seek(50, 1);
delay(1000);
lift_wrist(wrist_position - 40);
delay(1000);
lift_arm();
}
ofxSVGParticle::ofxSVGParticle(ofxSVGObject* obj) {
this->obj = obj;
center = findCenter();
cout << "Center: " << center << endl;
position = ofVec3f(0,0,0);
positionTarget = ofVec3f(0,0,0);
positionVelocity = ofVec3f(0,0,0);
rotation = 0;
rotationTarget = 0;
rotationVelocity = 0;
scale = 1;
scaleTarget = 1;
noiseScale = 1;
springTension = 2;
blur = true;
blurSteps = 40;
mode = 0;
}
int main(void) {
floyd();
findCenter();
return 0;
}
CHandPoint CTransformImage::findFingerInfo()
{
if(!m_transImage)
return CHandPoint();
findCenter();
CHandPoint handPt;
std::vector<CvPoint> ptList;
double pi = 3.1415;
int width = m_transImage->width;
int fingerCnt = 0;
int x, y, radius = 80;
unsigned char ch, pastCh = 0;
for(double theta = 180; theta <= 360; ++theta)
{
x = (int)(m_center.x + radius*cos(theta*pi/180));
y = (int)(m_center.y - radius*sin(theta*pi/180));
ch = m_transImage->imageData[y*width+x];
if(ch == 255 && pastCh == 0) // Counting Finger
ptList.push_back(cvPoint(x,y)), ++fingerCnt;
pastCh = ch;
// Draw OutLine
CvBox2D box;
box.center = cvPoint2D32f(x, y);
box.size = cvSize2D32f(1, 1);
box.angle = 90;
cvEllipseBox(m_image, box, CV_RGB(255,242,0), 1);
}
// handPt Setting
float dist = 0, dist2 = 0;
switch(fingerCnt)
{
case 0: handPt.m_mode = CHandPoint::CLEAR;
break;
case 1: handPt.m_mode = CHandPoint::MOVE; findEndPoint(&handPt.m_nX, &handPt.m_nY);
break;
case 2:
{
CvPoint a = ptList[0], b = ptList[1];
float dist = sqrt((float)(abs(a.x-b.x)*abs(a.x-b.x) + abs(a.y-b.y)*abs(a.y-b.y)));
if(dist < 70) // DRAW mode
{ handPt.m_mode = CHandPoint::CIRCLE; handPt.m_nX = m_center.x, handPt.m_nY = m_center.y; }
else
{ handPt.m_mode = CHandPoint::DRAW; findEndPoint(&handPt.m_nX, &handPt.m_nY); }
}
break;
case 3:
{
CvPoint a = ptList[0], b = ptList[1], c = ptList[2];
dist = sqrt((float)(abs(a.x-b.x)*abs(a.x-b.x) + abs(a.y-b.y)*abs(a.y-b.y)));
dist2 = sqrt((float)(abs(c.x-b.x)*abs(c.x-b.x) + abs(c.y-b.y)*abs(c.y-b.y)));
if(abs(dist-dist2) < 10)
{ handPt.m_mode = CHandPoint::TRIANGE; handPt.m_nX = m_center.x, handPt.m_nY = m_center.y; }
else
{ handPt.m_mode = CHandPoint::SETTING; }
}
break;
case 4: handPt.m_mode = CHandPoint::RECT; handPt.m_nX = m_center.x, handPt.m_nY = m_center.y;
break;
case 5: handPt.m_mode = CHandPoint::STAR; handPt.m_nX = m_center.x, handPt.m_nY = m_center.y;
break;
default: handPt.m_mode = CHandPoint::NOTHING;
break;
}
TCHAR buf[256] = {0,};
swprintf(buf, sizeof(buf), _T("%d\t%f\n"), fingerCnt, dist);
::OutputDebugString(buf);
return handPt;
}
void icp(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target,
const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source,
float* h_R, float* h_t,
const registrationParameters ¶m)
{
int Xsize, Ysize;
boost::shared_array<float> h_X, h_Y;
cloud2data(cloud_target, h_X, Xsize);
cloud2data(cloud_source, h_Y, Ysize);
//
// initialize paramters
//
int maxIteration = param.maxIteration;
//
// memory allocation
//
float* h_Xx = h_X.get() + Xsize*0;
float* h_Xy = h_X.get() + Xsize*1;
float* h_Xz = h_X.get() + Xsize*2;
float* h_Yx = h_Y.get() + Ysize*0;
float* h_Yy = h_Y.get() + Ysize*1;
float* h_Yz = h_Y.get() + Ysize*2;
boost::shared_array<float> h_Xcorr ( new float[Ysize*3] ); // points in X corresponding to Y
float h_S[9];
float h_Xc[3];
float h_Yc[3];
findCenter(h_Y.get(), Ysize, h_Yc);
// building flann index
boost::shared_array<float> m_X ( new float [Xsize*3] );
for (int i = 0; i < Xsize; i++)
{
m_X[i*3 + 0] = h_Xx[i];
m_X[i*3 + 1] = h_Xy[i];
m_X[i*3 + 2] = h_Xz[i];
}
flann::Matrix<float> mat_X(m_X.get(), Xsize, 3); // Xsize rows and 3 columns
flann::Index< flann::L2<float> > index( mat_X, flann::KDTreeIndexParams() );
index.buildIndex();
boost::shared_array<float> m_Y ( new float [Ysize*3] );
float* h_Xcorrx = h_Xcorr.get() + Ysize*0;
float* h_Xcorry = h_Xcorr.get() + Ysize*1;
float* h_Xcorrz = h_Xcorr.get() + Ysize*2;
boost::shared_array<float> h_Ycentered ( new float [Ysize*3] );
{
float* h_Ycenteredx = h_Ycentered.get() + Ysize*0;
float* h_Ycenteredy = h_Ycentered.get() + Ysize*1;
float* h_Ycenteredz = h_Ycentered.get() + Ysize*2;
for(int i = 0; i < Ysize; i++){
h_Ycenteredx[i] = h_Yx[i] - h_Yc[0];
h_Ycenteredy[i] = h_Yy[i] - h_Yc[1];
h_Ycenteredz[i] = h_Yz[i] - h_Yc[2];
}
}
// ICP main loop
for(int iter=0; iter < maxIteration; iter++){
// find closest points
#pragma omp parallel for
for (int i = 0; i < Ysize; i++)
{
m_Y[i*3 + 0] = (h_R[0]*h_Yx[i] + h_R[1]*h_Yy[i] + h_R[2]*h_Yz[i]) + h_t[0];
m_Y[i*3 + 1] = (h_R[3]*h_Yx[i] + h_R[4]*h_Yy[i] + h_R[5]*h_Yz[i]) + h_t[1];
m_Y[i*3 + 2] = (h_R[6]*h_Yx[i] + h_R[7]*h_Yy[i] + h_R[8]*h_Yz[i]) + h_t[2];
}
flann::Matrix<float> mat_Y(m_Y.get(), Ysize, 3); // Ysize rows and 3 columns
std::vector< std::vector<size_t> > indices(Ysize);
std::vector< std::vector<float> > dists(Ysize);
index.knnSearch(mat_Y,
indices,
dists,
1, // k of knn
flann::SearchParams() );
#pragma omp parallel for
for(int i = 0; i < Ysize; i++){
// put the closest point to Ycorr
h_Xcorrx[i] = h_Xx[indices[i][0]];
h_Xcorry[i] = h_Xy[indices[i][0]];
h_Xcorrz[i] = h_Xz[indices[i][0]];
}
findCenter(h_Xcorr.get(), Ysize, h_Xc);
#pragma omp parallel for
for(int i = 0; i < Ysize; i++){
// put the closest point to Ycorr
h_Xcorrx[i] -= h_Xc[0];
h_Xcorry[i] -= h_Xc[1];
h_Xcorrz[i] -= h_Xc[2];
}
// compute S
{
// SGEMM(TRANSA,TRANSB,M,N,K,ALPHA,A,LDA,B,LDB,BETA,C,LDC)
// C := alpha*op( A )*op( B ) + beta*C
//
// h_X^T * h_Y => S
// m*k k*n m*n
int three = 3;
float one = 1.0f, zero = 0.0f;
sgemm_((char*)"t", (char*)"n",
&three, &three, &Ysize, // m,n,k
&one, h_Xcorr.get(), &Ysize, // alpha, op(A), lda
h_Ycentered.get(), &Ysize, // op(B), ldb
&zero, h_S, &three); // beta, C, ldc
}
// find RT from S
findRTfromS(h_Xc, h_Yc, h_S, h_R, h_t);
#ifndef NOVIEWER
if(!param.noviewer){
Eigen::Matrix4f transformation;
transformation <<
h_R[0], h_R[1], h_R[2], h_t[0],
h_R[3], h_R[4], h_R[5], h_t[1],
h_R[6], h_R[7], h_R[8], h_t[2],
0, 0, 0, 1;
pcl::transformPointCloud ( *param.cloud_source, *param.cloud_source_trans, transformation );
param.viewer->updatePointCloud ( param.cloud_source_trans, *param.source_trans_color, "source trans" );
param.viewer->spinOnce();
}
#endif
}
}
QList<Number> NelderMead::findMinimum(const QList<QList<Number> > &initialSimplex, Number reflectionCoefficient,
Number contractionCoefficient, Number expansionCoefficient, Number accuracy)
{
QList<QList<Number> > simplex = initialSimplex;
forever {
// Find best, worst and second worst values
QList<Number> functions;
foreach (const QList<Number> point, simplex) {
functions << function(point);
}
int best = indexOfMinimal(functions);
int worst = indexOfMaximal(functions);
int secondWorst = indexOfSecondMaximal(functions);
// Find center value
QList<Number> center = findCenter(simplex, worst);
// Check for exit
if (found(simplex, center, accuracy)) {
return simplex[best];
}
// Get reflected value
QList<Number> reflected = MathUtils::addVectorToVector(
center,
MathUtils::multiplyVectorByNumber(
MathUtils::subtractVectorFromVector(
center, simplex[worst]),
reflectionCoefficient));
// reflected <= best
if (function(reflected) <= function(simplex[best])) {
QList<Number> expanded = MathUtils::addVectorToVector(
center,
MathUtils::multiplyVectorByNumber(
MathUtils::subtractVectorFromVector(
reflected, center),
expansionCoefficient)
);
if (function(expanded) < function(simplex[best])) {
simplex[worst] = expanded;
}
else {
simplex[worst] = reflected;
}
}
// second-worst < reflected <= worst
else if (MathUtils::isBetween(function(reflected), function(simplex[secondWorst]), function(simplex[worst]), true)) {
QList<Number> contracted = MathUtils::addVectorToVector(
center,
MathUtils::multiplyVectorByNumber(
MathUtils::subtractVectorFromVector(
simplex[worst], center),
contractionCoefficient)
);
simplex[worst] = contracted;
}
// best < reflected <= worst
else if (MathUtils::isBetween(function(reflected), function(simplex[best]), function(simplex[worst]), true)) {
simplex[worst] = reflected;
}
// reflected > worst
else {
QList<Number> bestValue = simplex[best];
for (int i = 0; i < simplex.size(); ++i) {
simplex[i] = MathUtils::addVectorToVector(
bestValue,
MathUtils::multiplyVectorByNumber(
MathUtils::subtractVectorFromVector(
simplex[i], bestValue),
0.5)
);
}
}
}
int traceTrajectory() {
/***** DECLARATIONS *****/
FILE *currentFile = NULL;
char *video_path = (char*) malloc(sizeof (char) * MAX_PATH);
char *pattern_path = (char*) malloc(sizeof (char) * MAX_PATH);
char *center_path = (char*) malloc(sizeof (char) * MAX_PATH);
CvCapture* capture = 0;
bwimage_t *image_target, *image_pattern, *image_center;
Pixel peak;
error_e retval = EEA_OK;
/***** MAIN PROCEDURE *****/
do {
/*** Find the paths to the images that need to be processed ***/
currentFile = fopen("paths_center.txt", "r"); // file containing
// the paths to the images
if (currentFile != NULL) {
fgetstr(video_path, MAX_PATH, currentFile);
fgetstr(pattern_path, MAX_PATH, currentFile);
fgetstr(center_path, MAX_PATH, currentFile);
} else {
printf("paths_center.txt cannot be opened");
exit(EXIT_FAILURE);
}
fclose(currentFile);
/*** Load the input images ***/
image_target = EEACreateImage();
image_pattern = EEACreateImage();
retval = EEALoadImage(pattern_path, image_pattern);
if (retval != EEA_OK) break;
// free paths
free(video_path);
free(pattern_path);
/*** Initialize the video frame capture ***/
capture = cvCreateFileCapture("samples/parabolic_drop_256.avi");
if (!capture) {
return -1;
}
IplImage *frame = cvQueryFrame(capture); //Init the video reading
double fps = cvGetCaptureProperty(
capture,
CV_CAP_PROP_FPS
);
CvSize size = cvSize(
(int) cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH),
(int) cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT)
);
// Convert to grayscale frame
IplImage *frameGray = cvCreateImage(size, IPL_DEPTH_8U, 1);
/*** Process the images ***/
bwimage_t *image_temp = createBlackBwimage(image_pattern->height,
image_pattern->width);
Pixel center;
copyImage(image_temp, image_pattern);
trim(&image_temp, 0.08);
findCenter(¢er, image_temp);
EEAFreeImage(image_temp);
cvCvtColor(frame, frameGray, CV_RGB2GRAY);
iplImageToBwimage(&image_target, frameGray);
locateShape(image_target, image_pattern, 0.08, &peak);
createCenterImage(&image_center, image_target->height,
image_target->width, peak, center);
while (1) {
frame = cvQueryFrame(capture);
if (!frame) break;
// Convert to grayscale frame
cvCvtColor(frame, frameGray, CV_RGB2GRAY);
iplImageToBwimage(&image_target, frameGray);
locateShape(image_target, image_pattern, 0.08, &peak);
addPosition(image_center, peak, center);
}
cvReleaseImage(&frame);
cvReleaseImage(&frameGray);
cvReleaseCapture(&capture);
/*** Write the output images at the specified path ***/
if (EEA_OK != (retval = EEADumpImage(center_path, image_center))) break;
free(center_path);
} while (0);
/***** FREE HEAP MEMORY *****/
EEAFreeImage(image_target);
EEAFreeImage(image_pattern);
EEAFreeImage(image_center);
/***** ERROR HANDLING *****/
switch (retval) {
case EEA_OK:
break;
case EEA_ENOFILE:
fprintf(stderr, "Cannot open file\n");
break;
case EEA_EBADBPS:
fprintf(stderr, "Number of bits per sample must be equal to 8\n");
break;
case EEA_EBADPHOTOMETRIC:
fprintf(stderr, "Not a colormap image\n");
break;
case EEA_ENOCOLORMAP:
fprintf(stderr, "Image does not have a colormap\n");
break;
case EEA_ENOTGRAY:
fprintf(stderr, "At least one entry in the colormap is not gray\n");
case EEA_ENOMEM:
fprintf(stderr, "Cannot allocate memory\n");
break;
default:
; /* Can't happen */
}
return(EXIT_SUCCESS);
}
