bool isMoving(TRBuffer< std::map< std::string, humanMonitor::niut_JOINT_STR > > confBuffer,
std::string joint, long timelapse, double distanceThreshold, int dimentionality){
int index;
double dist = 0.0;
long actualTimelapse = 0;
long timeNew = confBuffer.getTimeFromIndex( confBuffer.size() -1 );
long timeOld = timeNew - timelapse;
humanMonitor::niut_JOINT_STR jointNew = confBuffer.getDataFromIndex( confBuffer.size() -1 )[joint];
index = confBuffer.getIndexAfter(timeOld);
actualTimelapse = timeNew - confBuffer.getTimeFromIndex(index); // Actual timelapse
humanMonitor::niut_JOINT_STR jointOld = confBuffer.getDataFromIndex( index )[joint];
if( dimentionality == 2)
dist = dist2D(jointOld, jointNew);
else
dist = dist3D(jointOld, jointNew);
if( dist < distanceThreshold * actualTimelapse / timelapse)
return false;
else
return true;
}
vec3f Plane::ClampPointInRect( const Rectangle &rect, const vec3f &point ) const
{
const vec3f dir = P - point;
vec2f dist2D( Dot( dir, rect.ex ), Dot( dir, rect.ey ) );
vec2f hSize( rect.hx, rect.hy );
dist2D.x = std::min( hSize.x, std::max( -hSize.x, dist2D.x ) );
dist2D.y = std::min( hSize.y, std::max( -hSize.y, dist2D.y ) );
return point + rect.ex * dist2D.x + rect.ey * dist2D.y;
}
int main(int argc, char** argv){
ros::init(argc, argv, "humanMonitor");
ros::NodeHandle node;
HumanReader humanRd(node);
double far = 2.5;
double close = 1.3;
double distBodies = 0.0;
double distLHandToGripper = 0.0;
double distRHandToGripper = 0.0;
tf::TransformListener listener;
int niut_TORSO = 3;
int niut_LEFT_HAND = 9;
int niut_RIGHT_HAND = 15;
std::string humanTorso = "torso";
std::string humanLHand = "l_hand";
std::string humanRHand = "r_hand";
std::string robotTorso = "torso_lift_link";
std::string robotLGripper = "l_gripper_l_finger_link";
std::string robotRGripper = "r_gripper_l_finger_link";
humanMonitor::niut_JOINT_STR rHandJoint;
humanMonitor::niut_JOINT_STR lHandJoint;
humanMonitor::niut_JOINT_STR torsoJoint;
humanMonitor::niut_JOINT_STR rHandJointW;
humanMonitor::niut_JOINT_STR lHandJointW;
humanMonitor::niut_JOINT_STR torsoJointW;
double kinectPos[6];
//TODO: Move this in a service
kinectPos[0] = 7.25;
kinectPos[1] = 6.55;
kinectPos[2] = 2.2;
kinectPos[3] = 0;
kinectPos[4] = 0.84;
kinectPos[5] = -1.57;
while( node.ok() ){
ros::spinOnce();
// For now we focus on human with trackedId = 0.
if(humanRd.isPresent()){
std::cout << "[Fact] Human is present!" << std::endl;
//Get Human joints in kinect frame
rHandJoint = humanRd.m_LastConfig[0].skeleton.joint[niut_RIGHT_HAND];
lHandJoint = humanRd.m_LastConfig[0].skeleton.joint[niut_LEFT_HAND];
torsoJoint = humanRd.m_LastConfig[0].skeleton.joint[niut_TORSO];
//Get joints in same frame
if(rHandJoint.position.x != 0.0){
projectJoint(rHandJoint, kinectPos, rHandJointW);
m_HumanLastConfig[humanRHand] = rHandJointW;
}
if(lHandJoint.position.x != 0.0){
projectJoint(lHandJoint, kinectPos, lHandJointW);
m_HumanLastConfig[humanLHand] = lHandJointW;
}
if(torsoJoint.position.x != 0.0){
projectJoint(torsoJoint, kinectPos, torsoJointW);
m_HumanLastConfig[humanTorso] = torsoJointW;
}
m_HumanRBuffer.push_back(humanRd.m_LastTime, m_HumanLastConfig);
//Compute human motion:
long timeThreshold = pow(10,9); // 1sec
double distanceThreshold = 0.2; // 10 cms
if( isMoving(m_HumanRBuffer, humanTorso, timeThreshold, distanceThreshold, 2) ){
std::cout << "[Fact] Human is moving!" << std::endl;
}else{
if( isMoving(m_HumanRBuffer, humanRHand, timeThreshold, distanceThreshold*4/3, 3) ){
std::cout << "[Fact] Human right hand is moving" << std::endl;
}
if( isMoving(m_HumanRBuffer, humanLHand, timeThreshold, distanceThreshold*4/3, 3) ){
std::cout << "[Fact] Human left hand is moving" << std::endl;
}
}
updateRobot(listener);
//Compute relative distances (human / robot):
std::cout << "Human Torso x: " << torsoJointW.position.x << "y: " << torsoJointW.position.y << std::endl;
std::cout << "Robot Torso x: " << m_RobotLastConfig[robotTorso].position.x << "y: " << m_RobotLastConfig[robotTorso].position.y << std::endl;
if(torsoJoint.position.x != 0.0){
distBodies = dist2D(torsoJointW, m_RobotLastConfig[robotTorso]);
std::cout << "Dist human robot: " << distBodies << std::endl;
if( distBodies > far ){
std::cout << "[Fact] Human is far" << std::endl;
}else if(distBodies > close){
std::cout << "[Fact] Human is near" << std::endl;
//We compute the hand to gripper distance in NEAR case.
//Left gripper:
distLHandToGripper = dist3D(lHandJointW, m_RobotLastConfig[robotLGripper]);
distRHandToGripper = dist3D(rHandJointW, m_RobotLastConfig[robotLGripper]);
if( (distLHandToGripper < 0.5) || (distRHandToGripper < 0.5)){
std::cout << "[Fact] Danger! Human hand is close to left gripper!" << std::endl;
}
//Right gripper
distLHandToGripper = dist3D(lHandJointW, m_RobotLastConfig[robotRGripper]);
distRHandToGripper = dist3D(rHandJointW, m_RobotLastConfig[robotRGripper]);
if( (distLHandToGripper < 0.5) || (distRHandToGripper < 0.5)){
std::cout << "[Fact] Danger! Human hand is close to right gripper!" << std::endl;
}
}else
std::cout << "[Fact] Human is too close!" << std::endl;
}else
std::cout << "Human Torso not available " << std::endl;
}else
std::cout << "[Fact] Human is not present" << std::endl;
}
}
int main(int argc,char** argv)
{
/* number of task, task rank */
int numtasks, rank, sendcnt, recvcnt;
/* point dimension, number of points*/
int dimension, lines;
/* file to be read */
FILE* fp;
char* filename;
/* content read from file */
char* dnaSource;
double* tdSource;
/* data points for every process */
char* dnaBuf;
double* tdBuf;
/* centroid */
char* dnaC;
double* tdC;
/* labels for each point */
int* labels;
/* whether this task is a dna clustering */
int dna;
/* how many clusters */
int cluster;
/* check the arguments */
if (argc < 5) {
printf("Usage: mpiKmeans <dna or 2d> <input file name> <lines> <clusters> <dimension>\n");
exit(1);
}
/* check the arguments */
if (!strcmp(argv[1],"dna") && argc < 6) {
printf("Usage: mpiKmeans <dna or 2d> <input file name> <lines> <clusters> <dimension>\n");
exit(1);
}
/* file name */
filename = argv[2];
/* how many points */
lines = atoi(argv[3]);
/* dna clustring or not */
dna = !strcmp(argv[1], "dna");
/* how many clusters */
cluster = atoi(argv[4]);
MPI_Init(&argc,&argv);
MPI_Comm_rank(MPI_COMM_WORLD, &rank);
MPI_Comm_size(MPI_COMM_WORLD, &numtasks);
if (dna) {
/* initialize dna clustring parameters */
dimension = atoi(argv[5]);
sendcnt = (lines + numtasks - 1)/numtasks * dimension;
recvcnt = sendcnt;
dnaSource = malloc(sizeof(char) * dimension * lines);
dnaBuf = malloc(sizeof(char) * recvcnt);
dnaC = malloc(sizeof(char) * cluster * dimension);
} else {
/* initialize 2d clustring parameters */
dimension = 2;
sendcnt = (lines + numtasks - 1)/numtasks * dimension;
recvcnt = sendcnt;
tdSource = malloc(sizeof(double) * dimension * lines);
tdBuf = malloc(sizeof(double) * recvcnt);
tdC = malloc(sizeof(double) * cluster * dimension);
}
/* if it's master, read the data source and genterate centroids */
if (rank == MASTER) {
fp = fopen(filename, "r");
if(fp == NULL) {
printf("Cannot open file %s\n", filename);
exit(1);
}
if (dna) {
readDNA(fp, dnaSource, dimension);
generateDNACenter(dnaC, dnaSource, lines, dimension, cluster);
} else {
read2D(fp, tdSource);
generate2DCenter(tdC, tdSource, lines, dimension, cluster);
}
fclose(fp);
}
/* compute the number and offset of data elements to send for each process */
int* sendcnts = malloc(sizeof(int)*numtasks);
int* strides = malloc(sizeof(int)*numtasks);
int i;
for(i = 0;i<numtasks-1;i++) {
sendcnts[i] = sendcnt;
}
sendcnts[numtasks-1] = lines * dimension - sendcnt * (numtasks - 1);
int base = 0;
for(i = 0;i<numtasks;i++) {
strides[i] = base;
base += sendcnts[i];
}
/* scatter the data and broadcast the center */
if(dna) {
MPI_Scatterv (dnaSource,sendcnts,strides,MPI_CHAR,dnaBuf,recvcnt,MPI_CHAR,MASTER,MPI_COMM_WORLD);
MPI_Bcast (dnaC,cluster * dimension,MPI_CHAR,MASTER,MPI_COMM_WORLD);
} else {
MPI_Scatterv (tdSource,sendcnts,strides,MPI_DOUBLE,tdBuf,recvcnt,MPI_DOUBLE,MASTER,MPI_COMM_WORLD);
MPI_Bcast (tdC,cluster * dimension,MPI_DOUBLE,MASTER,MPI_COMM_WORLD);
}
/* compute the number of points for each process */
int num = sendcnts[rank]/dimension;
/* allocate an array for labeling each point */
labels = malloc(sizeof(int) * num);
/* do k-means until converging */
do {
int j;
/* dna clustering */
if(dna) {
/* the count of each character in every position of each point in every process */
int* newcnts = malloc(sizeof(int) * cluster * dimension * 4);
/* the count of each character in every position of each point in sum */
int* recvcnts = malloc(sizeof(int) * cluster * dimension * 4);
/* initialize to zero */
memset(newcnts, 0, sizeof(int) * cluster * dimension * 4);
/* the new centroids for every cluster computed */
char* recvctrs = malloc(sizeof(char) * cluster * dimension);
/* for every point, compute its distance from each centroid, choose the minimum one and label it */
for(i = 0; i < num; i++) {
/* initialize label and distance */
int lb = -1;
int dDNA = MAX_DIST;
/* iterate each cluster centroid and update the label and distance */
for(j = 0; j < cluster;j++) {
int tmpDist = distDNA(dnaC+j*dimension, dnaBuf+i*dimension, dimension);
if (tmpDist < dDNA) {
dDNA = tmpDist;
lb = j;
}
}
/* iterate each position of the point and update the counts of each character */
for(j = 0; j < dimension; j++) {
switch(dnaBuf[i*dimension+j]) {
case 'A':
newcnts[4 * dimension * lb + 4 * j + 0]++;
break;
case 'C':
newcnts[4 * dimension * lb + 4 * j + 1]++;
break;
case 'G':
newcnts[4 * dimension * lb + 4 * j + 2]++;
break;
case 'T':
newcnts[4 * dimension * lb + 4 * j + 3]++;
break;
default:
break;
}
}
/* update the point's label */
labels[i] = lb;
}
/* reduce the character counts in every position of each point on the master node */
MPI_Reduce(newcnts, recvcnts, cluster * 4 * dimension, MPI_INT, MPI_SUM, MASTER, MPI_COMM_WORLD);
/* set the terminate flag */
int flag = 0;
/* on the master node, compute the new centroid for each cluster */
if(rank == MASTER) {
/* iterate each cluster */
for (i = 0; i < cluster; i++) {
/* iterate each position of a centroid */
for (j = 0; j < dimension; j++) {
/* choose the character with most counts */
int left =
recvcnts[i*dimension*4+4*j+0] > recvcnts[i*dimension*4+4*j+1]?0:1;
int right =
recvcnts[i*dimension*4+4*j+2] > recvcnts[i*dimension*4+4*j+3]?2:3;
int final = recvcnts[i*dimension*4+4*j+left] >
recvcnts[i*dimension*4+4*j+right]?left:right;
/* update the centroid */
recvctrs[i * dimension + j] = bases[final];
}
}
/* initialize the difference of centroids */
int dsum = 0;
/* iterate each cluster centroid and update the difference of centroids */
for(i =0;i<cluster;i++) {
dsum += distDNA(recvctrs+dimension*i,dnaC+dimension*i,dimension);
}
/* see if it is time to terminate */
if(dsum <= DNA_MIN)
flag = 1;
free(dnaC);
/* update the centroids */
dnaC = recvctrs;
}
/* broadcast the terminate flag */
MPI_Bcast (&flag,1,MPI_INT,MASTER,MPI_COMM_WORLD);
free(newcnts);
free(recvcnts);
if(flag)
break;
/* broadcast the new centroids */
MPI_Bcast (dnaC,cluster * dimension,MPI_CHAR,MASTER,MPI_COMM_WORLD);
} else {
/* the new centroids on each process */
double* newctrs = malloc(sizeof(double) * cluster * dimension);
/* the received centroids on the master */
double* recvctrs = malloc(sizeof(double) * cluster * dimension);
/* the number of points on each cluster */
int* newcnts = malloc(sizeof(int) * cluster);
/* the received number of points on each cluster */
int* recvcnts = malloc(sizeof(int) * cluster);
/* initialization */
memset(newctrs, 0, sizeof(double) * cluster * dimension);
memset(newcnts, 0, sizeof(int) * cluster);
/* iterate each point on one process */
for(i = 0; i < num; i++) {
/* initialize the label and distance */
int lb = -1;
double d2D = MAX_DIST;
/* iterate each centroid and update the label to the one with minimum distance */
for(j = 0; j < cluster;j++) {
double tmpDist =
dist2D(tdC+j*dimension, tdBuf+i*dimension);
if (tmpDist < d2D) {
d2D = tmpDist;
lb = j;
}
}
/* update the label */
labels[i] = lb;
/* update the count */
newcnts[lb]++;
/* sum each point */
for (j = 0; j < dimension; j++) {
newctrs[lb * dimension + j] += tdBuf[i * dimension + j];
}
}
/* reduce the new centroid sum to the master process */
MPI_Reduce(newctrs, recvctrs, cluster * dimension, MPI_DOUBLE, MPI_SUM, MASTER, MPI_COMM_WORLD);
/* reduce the cluster counts to the master process */
MPI_Reduce(newcnts, recvcnts, cluster, MPI_INT, MPI_SUM, MASTER, MPI_COMM_WORLD);
/* initilize the termination flag */
int flag = 0;
/* on the master process, compute the new centroids */
if (rank == MASTER){
/* iterate each cluster */
for (i = 0; i < cluster; i++) {
for (j = 0; j < dimension; j++) {
recvctrs[i * dimension + j] /= recvcnts[i];
}
}
/* intilize the difference sum */
double sum = 0;
/* iterate each cluster and update the sum */
for (i = 0; i < cluster; i++) {
sum += dist2D(recvctrs+i*dimension,tdC+i*dimension);
}
/* if the difference is less than a threshold, set the termination flag */
if (sum < TD_MIN)
flag = 1;
free(tdC);
tdC = recvctrs;
}
/* broadcast the temination flag */
MPI_Bcast (&flag,1,MPI_INT,MASTER,MPI_COMM_WORLD);
free(newcnts);
free(recvcnts);
free(newctrs);
if(flag)
break;
/* broadcast the new centroids */
MPI_Bcast (tdC,cluster * dimension,MPI_DOUBLE,MASTER,MPI_COMM_WORLD);
}
}while(1);
